ekf2: remove redundant IMU vibration metrics

- consume IMU vibration metrics from vehicle_imu_status

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -63,7 +63,6 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 	_newest_high_rate_imu_sample = imu_sample;
 
 	// Do not change order of computeVibrationMetric and checkIfVehicleAtRest
-	computeVibrationMetric(imu_sample);
 	_control_status.flags.vehicle_at_rest = checkIfVehicleAtRest(dt, imu_sample);
 
 	_imu_updated = _imu_down_sampler.update(imu_sample);
@@ -84,23 +83,6 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 	}
 }
 
-void EstimatorInterface::computeVibrationMetric(const imuSample &imu)
-{
-	// calculate a metric which indicates the amount of coning vibration
-	Vector3f temp = imu.delta_ang % _delta_ang_prev;
-	_vibe_metrics(0) = 0.99f * _vibe_metrics(0) + 0.01f * temp.norm();
-
-	// calculate a metric which indicates the amount of high frequency gyro vibration
-	temp = imu.delta_ang - _delta_ang_prev;
-	_delta_ang_prev = imu.delta_ang;
-	_vibe_metrics(1) = 0.99f * _vibe_metrics(1) + 0.01f * temp.norm();
-
-	// calculate a metric which indicates the amount of high frequency accelerometer vibration
-	temp = imu.delta_vel - _delta_vel_prev;
-	_delta_vel_prev = imu.delta_vel;
-	_vibe_metrics(2) = 0.99f * _vibe_metrics(2) + 0.01f * temp.norm();
-}
-
 bool EstimatorInterface::checkIfVehicleAtRest(float dt, const imuSample &imu)
 {
 	// detect if the vehicle is not moving when on ground

src/modules/ekf2/EKF/estimator_interface.h

@@ -81,6 +81,9 @@ public:
 	2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
 	*/
 	const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; }
+	void setDeltaAngleConingMetric(float delta_angle_coning_metric) { _vibe_metrics(0) = delta_angle_coning_metric; }
+	void setDeltaAngleHighFrequencyVibrationMetric(float delta_angle_vibration_metric) { _vibe_metrics(1) = delta_angle_vibration_metric; }
+	void setDeltaVelocityHighFrequencyVibrationMetric(float delta_velocity_vibration_metric) { _vibe_metrics(2) = delta_velocity_vibration_metric; }
 
 	void setMagData(const magSample &mag_sample);
 
@@ -421,8 +424,6 @@ private:
 	unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
 
 	// IMU vibration and movement monitoring
-	Vector3f _delta_ang_prev{};	// delta angle from the previous IMU measurement
-	Vector3f _delta_vel_prev{};	// delta velocity from the previous IMU measurement
 	Vector3f _vibe_metrics{};	// IMU vibration metrics
 					// [0] Level of coning vibration in the IMU delta angles (rad^2)
 					// [1] high frequency vibration level in the IMU delta angle data (rad)

src/modules/ekf2/EKF2.cpp

@@ -352,6 +352,8 @@ void EKF2::Run()
 
 			_ekf.resetImuBias();
 			_imu_calibration_count = imu.calibration_count;
+
+			SelectImuStatus();
 		}
 
 	} else {
@@ -384,11 +386,13 @@ void EKF2::Run()
 				if (_device_id_accel != sensor_selection.accel_device_id) {
 					_ekf.resetAccelBias();
 					_device_id_accel = sensor_selection.accel_device_id;
+					SelectImuStatus();
 				}
 
 				if (_device_id_gyro != sensor_selection.gyro_device_id) {
 					_ekf.resetGyroBias();
 					_device_id_gyro = sensor_selection.gyro_device_id;
+					SelectImuStatus();
 				}
 			}
 		}
@@ -397,6 +401,8 @@ void EKF2::Run()
 	if (imu_updated) {
 		const hrt_abstime now = imu_sample_new.time_us;
 
+		UpdateImuStatus();
+
 		// push imu data into estimator
 		_ekf.setIMUData(imu_sample_new);
 		PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)
@@ -1296,6 +1302,42 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 	return amsl_hgt + _wgs84_hgt_offset;
 }
 
+void EKF2::SelectImuStatus()
+{
+	for (uint8_t imu_instance = 0; imu_instance < MAX_NUM_IMUS; imu_instance++) {
+		uORB::Subscription imu_status_sub{ORB_ID(vehicle_imu_status), imu_instance};
+
+		vehicle_imu_status_s imu_status{};
+		imu_status_sub.copy(&imu_status);
+
+		if (imu_status.accel_device_id == _device_id_accel) {
+			_vehicle_imu_status_sub.ChangeInstance(imu_instance);
+			return;
+		}
+	}
+
+	PX4_WARN("%d - IMU status not found for accel %d, gyro %d", _instance, _device_id_accel, _device_id_gyro);
+}
+
+void EKF2::UpdateImuStatus()
+{
+	vehicle_imu_status_s imu_status;
+
+	if (_vehicle_imu_status_sub.update(&imu_status)) {
+		if (imu_status.accel_device_id != _device_id_accel) {
+			SelectImuStatus();
+			return;
+		}
+
+		// accel -> delta velocity
+		_ekf.setDeltaVelocityHighFrequencyVibrationMetric(imu_status.accel_vibration_metric * _ekf.get_dt_imu_avg());
+
+		// gyro -> delta angle
+		_ekf.setDeltaAngleHighFrequencyVibrationMetric(imu_status.gyro_vibration_metric * _ekf.get_dt_imu_avg());
+		_ekf.setDeltaAngleConingMetric(imu_status.gyro_coning_vibration * _ekf.get_dt_imu_avg());
+	}
+}
+
 void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF airspeed sample
@@ -1810,7 +1852,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 		// allocate EKF2 instances until all found or arming
 		uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
 
-		bool ekf2_instance_created[4][4] {}; // IMUs * mags
+		bool ekf2_instance_created[MAX_NUM_IMUS][MAX_NUM_MAGS] {}; // IMUs * mags
 
 		while ((multi_instances_allocated < multi_instances)
 		       && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)

src/modules/ekf2/EKF2.hpp

@@ -86,6 +86,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_imu.h>
+#include <uORB/topics/vehicle_imu_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_magnetometer.h>
@@ -128,6 +129,10 @@ public:
 	int instance() const { return _instance; }
 
 private:
+
+	static constexpr uint8_t MAX_NUM_IMUS = 4;
+	static constexpr uint8_t MAX_NUM_MAGS = 4;
+
 	void Run() override;
 
 	void PublishAttitude(const hrt_abstime &timestamp);
@@ -149,6 +154,8 @@ private:
 	void PublishWindEstimate(const hrt_abstime &timestamp);
 	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
 
+	void SelectImuStatus();
+
 	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
@@ -157,6 +164,7 @@ private:
 	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
+	void UpdateImuStatus();
 
 	void UpdateMagCalibration(const hrt_abstime &timestamp);
 
@@ -236,6 +244,7 @@ private:
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};