PX4Accelerometer and PX4Gyroscope add vibration metrics and always publish status

.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"'

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)

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)

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()
 {

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_accel_s>			_sensor_pub;		// legacy message
 	uORB::PublicationMulti<sensor_accel_fifo_s>		_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};
 
 

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()
 {

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_gyro_s>			_sensor_pub;		// legacy message
 	uORB::PublicationMulti<sensor_gyro_control_s>		_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};