sensors/vehicle_imu: replace coning metric with actual integrator coning correction (averaged)

- this saves a relatively expensive higih rate cross product and gives
better visibility into what's actually happening internally

msg/vehicle_imu_status.msg

@@ -14,9 +14,9 @@ float32 gyro_rate_hz
 float32 accel_raw_rate_hz       # full raw sensor sample rate (Hz)
 float32 gyro_raw_rate_hz        # full raw sensor sample rate (Hz)
 
-float32 accel_vibration_metric  # high frequency vibration level in the IMU delta velocity data (m/s)
+float32 accel_vibration_metric  # high frequency vibration level in the accelerometer data (m/s/s)
-float32 gyro_vibration_metric   # high frequency vibration level in the IMU delta velocity data (m/s)
+float32 gyro_vibration_metric   # high frequency vibration level in the gyro data (rad/s)
-float32 gyro_coning_vibration   # Level of coning vibration in the IMU delta angles (rad^2)
+float32 delta_angle_coning_metric # average IMU delta angle coning correction (rad^2)
 
 float32[3] mean_accel           # average accelerometer readings since last publication
 float32[3] mean_gyro            # average gyroscope readings since last publication

src/modules/mavlink/streams/VIBRATION.hpp

@@ -84,7 +84,7 @@ private:
 
 					if (x.copy(&status)) {
 						if (status.accel_device_id == sensor_selection.accel_device_id) {
-							msg.vibration_x = status.gyro_coning_vibration;
+							msg.vibration_x = status.delta_angle_coning_metric;
 							msg.vibration_y = status.gyro_vibration_metric;
 							msg.vibration_z = status.accel_vibration_metric;
 							break;

src/modules/sensors/vehicle_imu/Integrator.hpp

@@ -188,6 +188,8 @@ public:
 		_last_alpha.zero();
 	}
 
+	const matrix::Vector3f &accumulated_coning_corrections() const { return _beta; }
+
 	/* Reset integrator and return current integral & integration time
 	 *
 	 * @param integral_dt	Get the dt in us of the current integration.

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -547,6 +547,8 @@ bool VehicleIMU::Publish()
 	Vector3f delta_angle;
 	Vector3f delta_velocity;
 
+	const Vector3f accumulated_coning_corrections = _gyro_integrator.accumulated_coning_corrections();
+
 	if (_accel_integrator.reset(delta_velocity, imu.delta_velocity_dt)
 	    && _gyro_integrator.reset(delta_angle, imu.delta_angle_dt)) {
 
@@ -554,17 +556,22 @@ bool VehicleIMU::Publish()
 
 			// delta angle: apply offsets, scale, and board rotation
 			_gyro_calibration.SensorCorrectionsUpdate();
-			const float gyro_dt_inv = 1.e6f / imu.delta_angle_dt;
+			const float gyro_dt_s = 1.e-6f * imu.delta_angle_dt;
-			const Vector3f angular_velocity{_gyro_calibration.Correct(delta_angle * gyro_dt_inv)};
+			const Vector3f angular_velocity{_gyro_calibration.Correct(delta_angle / gyro_dt_s)};
 			UpdateGyroVibrationMetrics(angular_velocity);
-			const Vector3f delta_angle_corrected{angular_velocity / gyro_dt_inv};
+			const Vector3f delta_angle_corrected{angular_velocity * gyro_dt_s};
+
+			// accumulate delta angle coning corrections
+			_coning_norm_accum += accumulated_coning_corrections.norm() * gyro_dt_s;
+			_coning_norm_accum_total_time_s += gyro_dt_s;
+
 
 			// delta velocity: apply offsets, scale, and board rotation
 			_accel_calibration.SensorCorrectionsUpdate();
-			const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt;
+			const float accel_dt_s = 1.e-6f * imu.delta_velocity_dt;
-			const Vector3f acceleration{_accel_calibration.Correct(delta_velocity * accel_dt_inv)};
+			const Vector3f acceleration{_accel_calibration.Correct(delta_velocity / accel_dt_s)};
 			UpdateAccelVibrationMetrics(acceleration);
-			const Vector3f delta_velocity_corrected{acceleration / accel_dt_inv};
+			const Vector3f delta_velocity_corrected{acceleration * accel_dt_s};
 
 			// vehicle_imu_status
 			//  publish before vehicle_imu so that error counts are available synchronously if needed
@@ -591,6 +598,11 @@ bool VehicleIMU::Publish()
 				Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.variance()).copyTo(_status.var_gyro);
 				_raw_gyro_mean.reset();
 
+				// Gyro delta angle coning metric = length of coning corrections averaged since last status publication
+				_status.delta_angle_coning_metric = _coning_norm_accum / _coning_norm_accum_total_time_s;
+				_coning_norm_accum = 0;
+				_coning_norm_accum_total_time_s = 0;
+
 				// gyro temperature
 				_status.temperature_gyro = _gyro_temperature_sum / _gyro_temperature_sum_count;
 				_gyro_temperature_sum = NAN;
@@ -692,10 +704,6 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &angular_velocity)
 	_status.gyro_vibration_metric = 0.99f * _status.gyro_vibration_metric
 					+ 0.01f * Vector3f(angular_velocity - _angular_velocity_prev).norm();
 
-	// Gyro delta angle coning metric = filtered length of (angular_velocity x angular_velocity_prev)
-	const Vector3f coning_metric{angular_velocity % _angular_velocity_prev};
-	_status.gyro_coning_vibration = 0.99f * _status.gyro_coning_vibration + 0.01f * coning_metric.norm();
-
 	_angular_velocity_prev = angular_velocity;
 }
 

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -84,8 +84,9 @@ private:
 	bool UpdateGyro();
 
 	void UpdateIntegratorConfiguration();
-	void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration);
+
-	void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
+	inline void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration);
+	inline void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
 
 	void SensorCalibrationUpdate();
 	void SensorCalibrationSaveAccel();
@@ -144,6 +145,9 @@ private:
 
 	vehicle_imu_status_s _status{};
 
+	float _coning_norm_accum{0};
+	float _coning_norm_accum_total_time_s{0};
+
 	uint8_t _delta_velocity_clipping{0};
 
 	hrt_abstime _last_clipping_notify_time{0};