diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 99fb863eed..15306ba35f 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -245,6 +245,33 @@ bool EKF2Selector::UpdateErrorScores() } } } + + // find highest priority IMU and record which estimator instances are using it + uint8_t highest_priority = 0; + + for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) { + if ((sensors_status_imu.accel_device_ids[i] != 0) + && (sensors_status_imu.accel_priority[i] > highest_priority) + ) { + highest_priority = sensors_status_imu.accel_priority[i]; + } + } + + for (auto &inst : _instance) { + inst.imu_highest_priority = false; // clear + + if (inst.accel_device_id != 0) { + for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) { + if ((inst.accel_device_id == sensors_status_imu.accel_device_ids[i]) + && (sensors_status_imu.accel_priority[i] > 0) + && (sensors_status_imu.accel_priority[i] == highest_priority) + ) { + inst.imu_highest_priority = true; + } + } + } + } + } } @@ -254,7 +281,7 @@ bool EKF2Selector::UpdateErrorScores() // default estimator timeout const hrt_abstime status_timeout = 50_ms; - // calculate individual error scores +// calculate individual error scores for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) { const bool prev_healthy = _instance[i].healthy.get_state(); @@ -742,6 +769,16 @@ void EKF2Selector::Run() } } + // prefer higher priority IMU if relative error is the same or better + if (_instance[i].imu_highest_priority && !_instance[_selected_instance].imu_highest_priority) { + + if (!lower_error_available && (relative_error <= 0.f) + && (hrt_elapsed_time(&_instance[i].time_last_selected) > 10_s)) { + best_ekf_alternate = i; + lower_error_available = true; + } + } + if ((test_ratio > 0) && (test_ratio < best_test_ratio)) { best_ekf = i; best_test_ratio = test_ratio; diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 6507233bd3..50c71767e4 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -134,6 +134,9 @@ private: systemlib::Hysteresis healthy{false}; + bool imu_highest_priority{false}; + bool mag_highest_priority{false}; + bool warning{false}; bool filter_fault{false}; bool timeout{false};