diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index ca05f8a401..c171d4d562 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index cc123356ec..2589cbdfbf 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/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) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 2ebb1cfd6d..50b57f153a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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_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) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 1901a39f75..57c339d95e 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -86,6 +86,7 @@ #include #include #include +#include #include #include #include @@ -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 ×tamp); @@ -149,6 +154,8 @@ private: void PublishWindEstimate(const hrt_abstime ×tamp); void PublishYawEstimatorStatus(const hrt_abstime ×tamp); + 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 ×tamp); @@ -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)};