From f73d93ef6c30e0efd13ff90f694d7df27ec4d520 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 8 Jan 2021 11:27:12 -0500 Subject: [PATCH] ekf2: selector improve status reporting - publish estimator_selector_status at minimal rate or immediately on change - log all estimator_selector_status updates --- src/modules/ekf2/EKF2Selector.cpp | 138 +++++++++++++++------------ src/modules/ekf2/EKF2Selector.hpp | 12 ++- src/modules/logger/logged_topics.cpp | 2 +- 3 files changed, 89 insertions(+), 63 deletions(-) diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 5274dc05ff..b2b1668dc5 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -69,12 +69,12 @@ void EKF2Selector::Stop() bool EKF2Selector::SelectInstance(uint8_t ekf_instance) { - if (ekf_instance != _selected_instance) { + if ((ekf_instance != INVALID_INSTANCE) && (ekf_instance != _selected_instance)) { // update sensor_selection immediately sensor_selection_s sensor_selection{}; - sensor_selection.accel_device_id = _instance[ekf_instance].estimator_status.accel_device_id; - sensor_selection.gyro_device_id = _instance[ekf_instance].estimator_status.gyro_device_id; + sensor_selection.accel_device_id = _instance[ekf_instance].status.accel_device_id; + sensor_selection.gyro_device_id = _instance[ekf_instance].status.gyro_device_id; sensor_selection.timestamp = hrt_absolute_time(); _sensor_selection_pub.publish(sensor_selection); @@ -84,7 +84,8 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance) _instance[_selected_instance].estimator_status_sub.unregisterCallback(); if (!_instance[_selected_instance].healthy) { - PX4_WARN("primary EKF changed %d (unhealthy) -> %d", _selected_instance, ekf_instance); + PX4_WARN("primary EKF changed %d (%s) -> %d", _selected_instance, + _instance[_selected_instance].filter_fault ? "filter fault" : "unhealthy", ekf_instance); } } @@ -223,9 +224,9 @@ bool EKF2Selector::UpdateErrorScores() for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) { const bool prev_healthy = _instance[i].healthy; - const estimator_status_s &status = _instance[i].estimator_status; + const estimator_status_s &status = _instance[i].status; - if (_instance[i].estimator_status_sub.update(&_instance[i].estimator_status)) { + if (_instance[i].estimator_status_sub.update(&_instance[i].status)) { if ((i + 1) > _available_instances) { _available_instances = i + 1; @@ -248,12 +249,13 @@ bool EKF2Selector::UpdateErrorScores() _instance[i].combined_test_ratio = combined_test_ratio; _instance[i].healthy = tilt_align && yaw_align && (status.filter_fault_flags == 0); + _instance[i].filter_fault = (status.filter_fault_flags != 0); if (!PX4_ISFINITE(_instance[i].relative_test_ratio)) { _instance[i].relative_test_ratio = 0; } - } else if (hrt_elapsed_time(&status.timestamp) > 20_ms) { + } else if (hrt_elapsed_time(&status.timestamp) > (FILTER_UPDATE_PERIOD * 2)) { _instance[i].healthy = false; } @@ -269,6 +271,7 @@ bool EKF2Selector::UpdateErrorScores() if (prev_healthy != _instance[i].healthy) { updated = true; + _selector_status_publish = true; } } @@ -388,7 +391,7 @@ void EKF2Selector::PublishVehicleLocalPosition(bool reset) _local_position_last = local_position; // publish estimator's local position for system (vehicle_local_position) unless it's stale - if (local_position.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { + if (local_position.timestamp >= _instance[_selected_instance].status.timestamp_sample) { // republish with total reset count and current timestamp local_position.xy_reset_counter = _xy_reset_counter; local_position.z_reset_counter = _z_reset_counter; @@ -448,7 +451,7 @@ void EKF2Selector::PublishVehicleGlobalPosition(bool reset) _global_position_last = global_position; // publish estimator's global position for system (vehicle_global_position) unless it's stale - if (global_position.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { + if (global_position.timestamp >= _instance[_selected_instance].status.timestamp_sample) { // republish with total reset count and current timestamp global_position.lat_lon_reset_counter = _lat_lon_reset_counter; global_position.alt_reset_counter = _alt_reset_counter; @@ -463,7 +466,7 @@ void EKF2Selector::PublishVehicleGlobalPosition(bool reset) void EKF2Selector::Run() { // re-schedule as backup timeout - ScheduleDelayed(10_ms); + ScheduleDelayed(FILTER_UPDATE_PERIOD); // check for parameter updates if (_parameter_update_sub.updated()) { @@ -478,11 +481,11 @@ void EKF2Selector::Run() // update combined test ratio for all estimators const bool updated = UpdateErrorScores(); - // if no valid instance selected then select instance with valid IMU + // if no valid instance then force select first instance with valid IMU if (_selected_instance == INVALID_INSTANCE) { for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) { - if ((_instance[i].estimator_status.accel_device_id != 0) - && (_instance[i].estimator_status.gyro_device_id != 0)) { + if ((_instance[i].status.accel_device_id != 0) + && (_instance[i].status.gyro_device_id != 0)) { if (SelectInstance(i)) { break; @@ -497,6 +500,12 @@ void EKF2Selector::Run() } if (updated) { + + const uint8_t available_instances_prev = _available_instances; + const uint8_t selected_instance_prev = _selected_instance; + const uint32_t instance_changed_count_prev = _instance_changed_count; + const hrt_abstime last_instance_change_prev = _last_instance_change; + bool lower_error_available = false; float alternative_error = 0.f; // looking for instances that have error lower than the current primary uint8_t best_ekf_instance = _selected_instance; @@ -554,60 +563,69 @@ void EKF2Selector::Run() } } - // publish selector status - estimator_selector_status_s selector_status{}; - selector_status.primary_instance = _selected_instance; - selector_status.instances_available = _available_instances; - selector_status.instance_changed_count = _instance_changed_count; - selector_status.last_instance_change = _last_instance_change; - selector_status.accel_device_id = _instance[_selected_instance].estimator_status.accel_device_id; - selector_status.baro_device_id = _instance[_selected_instance].estimator_status.baro_device_id; - selector_status.gyro_device_id = _instance[_selected_instance].estimator_status.gyro_device_id; - selector_status.mag_device_id = _instance[_selected_instance].estimator_status.mag_device_id; - selector_status.gyro_fault_detected = _gyro_fault_detected; - selector_status.accel_fault_detected = _accel_fault_detected; - - for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { - selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio; - selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio; - selector_status.healthy[i] = _instance[i].healthy; - } + // publish selector status at ~1 Hz or immediately on any change + if (_selector_status_publish || (hrt_elapsed_time(&_last_status_publish) > 1_s) + || (available_instances_prev != _available_instances) + || (selected_instance_prev != _selected_instance) + || (instance_changed_count_prev != _instance_changed_count) + || (last_instance_change_prev != _last_instance_change) + || _accel_fault_detected || _gyro_fault_detected) { + + estimator_selector_status_s selector_status{}; + selector_status.primary_instance = _selected_instance; + selector_status.instances_available = _available_instances; + selector_status.instance_changed_count = _instance_changed_count; + selector_status.last_instance_change = _last_instance_change; + selector_status.accel_device_id = _instance[_selected_instance].status.accel_device_id; + selector_status.baro_device_id = _instance[_selected_instance].status.baro_device_id; + selector_status.gyro_device_id = _instance[_selected_instance].status.gyro_device_id; + selector_status.mag_device_id = _instance[_selected_instance].status.mag_device_id; + selector_status.gyro_fault_detected = _gyro_fault_detected; + selector_status.accel_fault_detected = _accel_fault_detected; + + for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { + selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio; + selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio; + selector_status.healthy[i] = _instance[i].healthy; + } - for (int i = 0; i < IMU_STATUS_SIZE; i++) { - selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i]; - selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i]; - } + for (int i = 0; i < IMU_STATUS_SIZE; i++) { + selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i]; + selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i]; + } - selector_status.timestamp = hrt_absolute_time(); - _estimator_selector_status_pub.publish(selector_status); + selector_status.timestamp = hrt_absolute_time(); + _estimator_selector_status_pub.publish(selector_status); + _last_status_publish = selector_status.timestamp; + _selector_status_publish = false; + } } // republish selected estimator data for system - if (_selected_instance != INVALID_INSTANCE) { - // selected estimator_attitude -> vehicle_attitude - if (_instance[_selected_instance].estimator_attitude_sub.updated()) { - PublishVehicleAttitude(); - } - // selected estimator_local_position -> vehicle_local_position - if (_instance[_selected_instance].estimator_local_position_sub.updated()) { - PublishVehicleLocalPosition(); - } + // selected estimator_attitude -> vehicle_attitude + if (_instance[_selected_instance].estimator_attitude_sub.updated()) { + PublishVehicleAttitude(); + } - // selected estimator_global_position -> vehicle_global_position - if (_instance[_selected_instance].estimator_global_position_sub.updated()) { - PublishVehicleGlobalPosition(); - } + // selected estimator_local_position -> vehicle_local_position + if (_instance[_selected_instance].estimator_local_position_sub.updated()) { + PublishVehicleLocalPosition(); + } - // selected estimator_odometry -> vehicle_odometry - if (_instance[_selected_instance].estimator_odometry_sub.updated()) { - vehicle_odometry_s vehicle_odometry; + // selected estimator_global_position -> vehicle_global_position + if (_instance[_selected_instance].estimator_global_position_sub.updated()) { + PublishVehicleGlobalPosition(); + } - if (_instance[_selected_instance].estimator_odometry_sub.update(&vehicle_odometry)) { - if (vehicle_odometry.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { - vehicle_odometry.timestamp = hrt_absolute_time(); - _vehicle_odometry_pub.publish(vehicle_odometry); - } + // selected estimator_odometry -> vehicle_odometry + if (_instance[_selected_instance].estimator_odometry_sub.updated()) { + vehicle_odometry_s vehicle_odometry; + + if (_instance[_selected_instance].estimator_odometry_sub.update(&vehicle_odometry)) { + if (vehicle_odometry.timestamp >= _instance[_selected_instance].status.timestamp_sample) { + vehicle_odometry.timestamp = hrt_absolute_time(); + _vehicle_odometry_pub.publish(vehicle_odometry); } } } @@ -625,8 +643,8 @@ void EKF2Selector::PrintStatus() const EstimatorInstance &inst = _instance[i]; PX4_INFO("%d: ACC: %d, GYRO: %d, MAG: %d, %s, test ratio: %.7f (%.5f) %s", - inst.instance, inst.estimator_status.accel_device_id, inst.estimator_status.gyro_device_id, - inst.estimator_status.mag_device_id, + inst.instance, inst.status.accel_device_id, inst.status.gyro_device_id, + inst.status.mag_device_id, inst.healthy ? "healthy" : "unhealthy", (double)inst.combined_test_ratio, (double)inst.relative_test_ratio, (_selected_instance == i) ? "*" : ""); diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 44edfa9f54..6663b273c1 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -56,6 +56,8 @@ static constexpr uint8_t EKF2_MAX_INSTANCES{9}; static_assert(EKF2_MAX_INSTANCES <= ORB_MULTI_MAX_INSTANCES, "EKF2_MAX_INSTANCES must be <= ORB_MULTI_MAX_INSTANCES"); +using namespace time_literals; + class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem { public: @@ -68,7 +70,9 @@ public: void PrintStatus(); private: - static constexpr uint8_t INVALID_INSTANCE = UINT8_MAX; + static constexpr uint8_t INVALID_INSTANCE{UINT8_MAX}; + static constexpr uint64_t FILTER_UPDATE_PERIOD{10_ms}; + void Run() override; void PublishVehicleAttitude(bool reset = false); void PublishVehicleLocalPosition(bool reset = false); @@ -96,7 +100,7 @@ private: uORB::Subscription estimator_global_position_sub; uORB::Subscription estimator_odometry_sub; - estimator_status_s estimator_status{}; + estimator_status_s status{}; hrt_abstime time_last_selected{0}; @@ -104,6 +108,7 @@ private: float relative_test_ratio{NAN}; bool healthy{false}; + bool filter_fault{false}; const uint8_t instance; }; @@ -147,6 +152,9 @@ private: uint32_t _instance_changed_count{0}; hrt_abstime _last_instance_change{0}; + hrt_abstime _last_status_publish{0}; + bool _selector_status_publish{false}; + // vehicle_attitude: reset counters vehicle_attitude_s _attitude_last{}; matrix::Quatf _delta_q_reset{}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 46d561ad3b..332e454749 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -114,7 +114,7 @@ void LoggedTopics::add_default_topics() // EKF multi topics (currently max 9 estimators) static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 4; - add_topic("estimator_selector_status", 200); + add_topic("estimator_selector_status"); add_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES);