From 89e1f478ac9f3863df4f5613cf111222ae9c7749 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 27 Dec 2019 15:14:38 -0500 Subject: [PATCH] PX4Accelerometer and PX4Gyroscope add vibration metrics and always publish status --- .ci/Jenkinsfile-hardware | 4 ++ msg/sensor_accel_status.msg | 2 + msg/sensor_gyro_status.msg | 4 ++ .../accelerometer/PX4Accelerometer.cpp | 47 +++++++++++++++++ .../accelerometer/PX4Accelerometer.hpp | 8 ++- src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 52 +++++++++++++++++++ src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 9 +++- 7 files changed, 122 insertions(+), 4 deletions(-) diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index b60678c1aa..190a53c3f4 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -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"' @@ -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"' diff --git a/msg/sensor_accel_status.msg b/msg/sensor_accel_status.msg index eb3a677a42..21ab217b95 100644 --- a/msg/sensor_accel_status.msg +++ b/msg/sensor_accel_status.msg @@ -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) diff --git a/msg/sensor_gyro_status.msg b/msg/sensor_gyro_status.msg index eb3a677a42..c8cead091f 100644 --- a/msg/sensor_gyro_status.msg +++ b/msg/sensor_gyro_status.msg @@ -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) diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index cb7301fd64..53ba25c5cd 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -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))}; @@ -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{}; @@ -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 @@ -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(); @@ -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() { diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 9430175276..8ea09475a0 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -88,8 +88,9 @@ public: 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_pub; // legacy message uORB::PublicationMulti _sensor_fifo_pub; @@ -106,6 +107,9 @@ private: 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}; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 778614e549..59ab98178e 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -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)}; @@ -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{}; @@ -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 @@ -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(); @@ -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() { diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 171b0f660b..799216984c 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -88,8 +88,9 @@ public: 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_pub; // legacy message uORB::PublicationMulti _sensor_control_pub; @@ -108,6 +109,10 @@ private: 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};