Skip to content

Commit

Permalink
PX4Accelerometer and PX4Gyroscope add vibration metrics and always pu…
Browse files Browse the repository at this point in the history 
…blish status
  • Loading branch information
@dagar
dagar committed Dec 28, 2019
1 parent e189733 commit 89e1f47
Show file tree
Hide file tree
Showing 7 changed files with 122 additions and 4 deletions.
4 changes: 4 additions & 0 deletions .ci/Jenkinsfile-hardware
View file
Original file line number Diff line number Diff line change
Expand Up @@ -686,8 +686,10 @@ void statusFTDI() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_baro"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_mag"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
Expand Down Expand Up @@ -725,8 +727,10 @@ void statusSEGGER() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "free"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_baro"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_mag"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /dev"'
Expand Down
2 changes: 2 additions & 0 deletions msg/sensor_accel_status.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,3 +13,5 @@ uint16 measure_rate
uint16 sample_rate

float32 full_scale_range

float32 vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s)
4 changes: 4 additions & 0 deletions msg/sensor_gyro_status.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,3 +13,7 @@ uint16 measure_rate
uint16 sample_rate

float32 full_scale_range

float32 vibration_metric # high frequency vibration level in the IMU delta angle data (rad)

float32 coning_vibration # Level of coning vibration in the IMU delta angles (rad^2)
47 changes: 47 additions & 0 deletions src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,17 @@ PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)

const Vector3f raw{x, y, z};

// Clipping
sensor_accel_status_s &status = _sensor_status_pub.get();
const float clip_limit = (_range / _scale) * 0.95f;

for (int i = 0; i < 3; i++) {
if (fabsf(raw(i)) > clip_limit) {
status.clipping[i]++;
_integrator_clipping++;
}
}

// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};

Expand All @@ -133,6 +144,8 @@ PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
Vector3f integrated_value;
uint32_t integral_dt = 0;

_integrator_samples++;

if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {

sensor_accel_s report{};
Expand All @@ -152,12 +165,33 @@ PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
report.z = val_filtered(2);

report.integral_dt = integral_dt;
report.integral_samples = _integrator_samples;
report.x_integral = integrated_value(0);
report.y_integral = integrated_value(1);
report.z_integral = integrated_value(2);
report.integral_clip_count = _integrator_clipping;

_sensor_pub.publish(report);

// reset integrator
ResetIntegrator();

// update vibration metrics
const Vector3f delta_velocity = integrated_value * (integral_dt * 1.e-6f);
UpdateVibrationMetrics(delta_velocity);
}

// publish status
status.device_id = _device_id;
status.error_count = _error_count;
status.full_scale_range = _range;
status.rotation = _rotation;
status.measure_rate = _update_rate;
status.sample_rate = _sample_rate;
status.temperature = _temperature;
status.vibration_metric = _vibration_metric;
status.timestamp = hrt_absolute_time();
_sensor_status_pub.publish(status);
}

void
Expand Down Expand Up @@ -292,6 +326,9 @@ PX4Accelerometer::updateFIFO(const FIFOSample &sample)
report.timestamp = _integrator_timestamp_sample;
_sensor_pub.publish(report);

// update vibration metrics
const Vector3f delta_velocity = val_int_calibrated * (integrator_dt_us * 1.e-6f);
UpdateVibrationMetrics(delta_velocity);

// reset integrator
ResetIntegrator();
Expand Down Expand Up @@ -340,6 +377,16 @@ PX4Accelerometer::ConfigureFilter(float cutoff_freq)
_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
}

void
PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
{
// Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
const Vector3f delta_velocity_diff = delta_velocity - _delta_velocity_prev;
_vibration_metric = 0.99f * _vibration_metric + 0.01f * delta_velocity_diff.norm();

_delta_velocity_prev = delta_velocity;
}

void
PX4Accelerometer::print_status()
{
Expand Down
8 changes: 6 additions & 2 deletions src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,8 +88,9 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams

private:

void ConfigureFilter(float cutoff_freq);
void ResetIntegrator();
void ConfigureFilter(float cutoff_freq);
void ResetIntegrator();
void UpdateVibrationMetrics(const matrix::Vector3f &delta_velocity);

uORB::PublicationMulti<sensor_accel_s> _sensor_pub; // legacy message
uORB::PublicationMulti<sensor_accel_fifo_s> _sensor_fifo_pub;
Expand All @@ -106,6 +107,9 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams
matrix::Vector3f _calibration_scale{1.0f, 1.0f, 1.0f};
matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};

matrix::Vector3f _delta_velocity_prev{0.0f, 0.0f, 0.0f}; // delta velocity from the previous IMU measurement
float _vibration_metric{0.0f}; // high frequency vibration level in the IMU delta velocity data (m/s)

int _class_device_instance{-1};


Expand Down
52 changes: 52 additions & 0 deletions src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,17 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)

const Vector3f raw{x, y, z};

// Clipping
sensor_gyro_status_s &status = _sensor_status_pub.get();
const float clip_limit = (_range / _scale) * 0.95f;

for (int i = 0; i < 3; i++) {
if (fabsf(raw(i)) > clip_limit) {
status.clipping[i]++;
_integrator_clipping++;
}
}

// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)};

Expand Down Expand Up @@ -157,6 +168,8 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
Vector3f integrated_value;
uint32_t integral_dt = 0;

_integrator_samples++;

if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {

sensor_gyro_s report{};
Expand All @@ -176,12 +189,34 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
report.z = val_filtered(2);

report.integral_dt = integral_dt;
report.integral_samples = _integrator_samples;
report.x_integral = integrated_value(0);
report.y_integral = integrated_value(1);
report.z_integral = integrated_value(2);
report.integral_clip_count = _integrator_clipping;

_sensor_pub.publish(report);

// reset integrator
ResetIntegrator();

// update vibration metrics
const Vector3f delta_angle = integrated_value * (integral_dt * 1.e-6f);
UpdateVibrationMetrics(delta_angle);
}

// publish status
status.device_id = _device_id;
status.error_count = _error_count;
status.full_scale_range = _range;
status.rotation = _rotation;
status.measure_rate = _update_rate;
status.sample_rate = _sample_rate;
status.temperature = _temperature;
status.vibration_metric = _vibration_metric;
status.coning_vibration = _coning_vibration;
status.timestamp = hrt_absolute_time();
_sensor_status_pub.publish(status);
}

void
Expand Down Expand Up @@ -342,6 +377,9 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
report.timestamp = _integrator_timestamp_sample;
_sensor_pub.publish(report);

// update vibration metrics
const Vector3f delta_angle = val_int_calibrated * (integrator_dt_us * 1.e-6f);
UpdateVibrationMetrics(delta_angle);

// reset integrator
ResetIntegrator();
Expand Down Expand Up @@ -390,6 +428,20 @@ PX4Gyroscope::ConfigureFilter(float cutoff_freq)
_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
}

void
PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
{
// Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
const Vector3f delta_angle_diff = delta_angle - _delta_angle_prev;
_vibration_metric = 0.99f * _vibration_metric + 0.01f * delta_angle_diff.norm();

// Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
const Vector3f coning_metric = delta_angle % _delta_angle_prev;
_coning_vibration = 0.99f * _coning_vibration + 0.01f * coning_metric.norm();

_delta_angle_prev = delta_angle;
}

void
PX4Gyroscope::print_status()
{
Expand Down
9 changes: 7 additions & 2 deletions src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,8 +88,9 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams

private:

void ConfigureFilter(float cutoff_freq);
void ResetIntegrator();
void ConfigureFilter(float cutoff_freq);
void ResetIntegrator();
void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle);

uORB::PublicationMulti<sensor_gyro_s> _sensor_pub; // legacy message
uORB::PublicationMulti<sensor_gyro_control_s> _sensor_control_pub;
Expand All @@ -108,6 +109,10 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams

matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};

matrix::Vector3f _delta_angle_prev{0.0f, 0.0f, 0.0f}; // delta angle from the previous IMU measurement
float _vibration_metric{0.0f}; // high frequency vibration level in the IMU delta angle data (rad)
float _coning_vibration{0.0f}; // Level of coning vibration in the IMU delta angles (rad^2)

int _class_device_instance{-1};


Expand Down

0 comments on commit 89e1f47

Please sign in to comment.