/**************************************************************************** * * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ #include "EKF2Selector.hpp" using namespace time_literals; using matrix::Quatf; using matrix::Vector2f; using math::constrain; using math::radians; EKF2Selector::EKF2Selector() : ModuleParams(nullptr), ScheduledWorkItem("ekf2_selector", px4::wq_configurations::nav_and_controllers) { } EKF2Selector::~EKF2Selector() { Stop(); } bool EKF2Selector::Start() { ScheduleNow(); return true; } void EKF2Selector::Stop() { for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { _instance[i].estimator_attitude_sub.unregisterCallback(); _instance[i].estimator_status_sub.unregisterCallback(); } ScheduleClear(); } void EKF2Selector::PrintInstanceChange(const uint8_t old_instance, uint8_t new_instance) { const char *old_reason = nullptr; if (_instance[old_instance].filter_fault) { old_reason = " (filter fault)"; } else if (_instance[old_instance].timeout) { old_reason = " (timeout)"; } else if (_gyro_fault_detected) { old_reason = " (gyro fault)"; } else if (_accel_fault_detected) { old_reason = " (accel fault)"; } else if (!_instance[_selected_instance].healthy.get_state() && (_instance[_selected_instance].healthy_count > 0)) { // skipped if previous instance was never healthy in the first place (eg initialization) old_reason = " (unhealthy)"; } const char *new_reason = nullptr; if (_request_instance.load() == new_instance) { new_reason = " (user selected)"; } if (old_reason || new_reason) { if (old_reason == nullptr) { old_reason = ""; } if (new_reason == nullptr) { new_reason = ""; } PX4_WARN("primary EKF changed %" PRIu8 "%s -> %" PRIu8 "%s", old_instance, old_reason, new_instance, new_reason); } } bool EKF2Selector::SelectInstance(uint8_t ekf_instance) { if ((ekf_instance != _selected_instance) && (ekf_instance < _available_instances)) { // update sensor_selection immediately sensor_selection_s sensor_selection{}; sensor_selection.accel_device_id = _instance[ekf_instance].accel_device_id; sensor_selection.gyro_device_id = _instance[ekf_instance].gyro_device_id; sensor_selection.timestamp = hrt_absolute_time(); _sensor_selection_pub.publish(sensor_selection); if (_selected_instance != INVALID_INSTANCE) { // switch callback registration _instance[_selected_instance].estimator_attitude_sub.unregisterCallback(); _instance[_selected_instance].estimator_status_sub.unregisterCallback(); PrintInstanceChange(_selected_instance, ekf_instance); } _instance[ekf_instance].estimator_attitude_sub.registerCallback(); _instance[ekf_instance].estimator_status_sub.registerCallback(); _selected_instance = ekf_instance; _instance_changed_count++; _last_instance_change = sensor_selection.timestamp; _instance[ekf_instance].time_last_selected = _last_instance_change; // reset all relative test ratios for (uint8_t i = 0; i < _available_instances; i++) { _instance[i].relative_test_ratio = 0; } return true; } return false; } bool EKF2Selector::UpdateErrorScores() { // first check imu inconsistencies _gyro_fault_detected = false; uint32_t faulty_gyro_id = 0; _accel_fault_detected = false; uint32_t faulty_accel_id = 0; if (_sensors_status_imu.updated()) { sensors_status_imu_s sensors_status_imu; if (_sensors_status_imu.copy(&sensors_status_imu)) { const float time_step_s = constrain((sensors_status_imu.timestamp - _last_update_us) * 1e-6f, 0.f, 0.02f); _last_update_us = sensors_status_imu.timestamp; { const float angle_rate_threshold = radians(_param_ekf2_sel_imu_angle_rate.get()); const float angle_threshold = radians(_param_ekf2_sel_imu_angle.get()); uint8_t n_gyros = 0; uint8_t n_gyro_exceedances = 0; float largest_accumulated_gyro_error = 0.0f; uint8_t largest_gyro_error_index = 0; for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) { // check for gyros with excessive difference to mean using accumulated error if (sensors_status_imu.gyro_device_ids[i] != 0) { n_gyros++; _accumulated_gyro_error[i] += (sensors_status_imu.gyro_inconsistency_rad_s[i] - angle_rate_threshold) * time_step_s; _accumulated_gyro_error[i] = fmaxf(_accumulated_gyro_error[i], 0.f); if (_accumulated_gyro_error[i] > angle_threshold) { n_gyro_exceedances++; } if (_accumulated_gyro_error[i] > largest_accumulated_gyro_error) { largest_accumulated_gyro_error = _accumulated_gyro_error[i]; largest_gyro_error_index = i; } } else { // no sensor _accumulated_gyro_error[i] = NAN; } } if (n_gyro_exceedances > 0) { if (n_gyros >= 3) { // If there are 3 or more sensors, the one with the largest accumulated error is faulty _gyro_fault_detected = true; faulty_gyro_id = sensors_status_imu.gyro_device_ids[largest_gyro_error_index]; } else if (n_gyros == 2) { // A fault is present, but the faulty sensor identity cannot be determined _gyro_fault_detected = true; } } } { const float accel_threshold = _param_ekf2_sel_imu_accel.get(); const float velocity_threshold = _param_ekf2_sel_imu_velocity.get(); uint8_t n_accels = 0; uint8_t n_accel_exceedances = 0; float largest_accumulated_accel_error = 0.0f; uint8_t largest_accel_error_index = 0; for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) { // check for accelerometers with excessive difference to mean using accumulated error if (sensors_status_imu.accel_device_ids[i] != 0) { n_accels++; _accumulated_accel_error[i] += (sensors_status_imu.accel_inconsistency_m_s_s[i] - accel_threshold) * time_step_s; _accumulated_accel_error[i] = fmaxf(_accumulated_accel_error[i], 0.f); if (_accumulated_accel_error[i] > velocity_threshold) { n_accel_exceedances++; } if (_accumulated_accel_error[i] > largest_accumulated_accel_error) { largest_accumulated_accel_error = _accumulated_accel_error[i]; largest_accel_error_index = i; } } else { // no sensor _accumulated_accel_error[i] = NAN; } } if (n_accel_exceedances > 0) { if (n_accels >= 3) { // If there are 3 or more sensors, the one with the largest accumulated error is faulty _accel_fault_detected = true; faulty_accel_id = sensors_status_imu.accel_device_ids[largest_accel_error_index]; } else if (n_accels == 2) { // A fault is present, but the faulty sensor identity cannot be determined _accel_fault_detected = true; } } } } } bool updated = false; bool primary_updated = false; // default estimator timeout const hrt_abstime status_timeout = 50_ms; // calculate individual error scores for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) { const bool prev_healthy = _instance[i].healthy.get_state(); estimator_status_s status; if (_instance[i].estimator_status_sub.update(&status)) { _instance[i].timestamp_sample_last = status.timestamp_sample; _instance[i].accel_device_id = status.accel_device_id; _instance[i].gyro_device_id = status.gyro_device_id; _instance[i].baro_device_id = status.baro_device_id; _instance[i].mag_device_id = status.mag_device_id; if ((i + 1) > _available_instances) { _available_instances = i + 1; updated = true; } if (i == _selected_instance) { primary_updated = true; } // test ratios are invalid when 0, >= 1 is a failure if (!PX4_ISFINITE(status.vel_test_ratio) || (status.vel_test_ratio <= 0.f)) { status.vel_test_ratio = 1.f; } if (!PX4_ISFINITE(status.pos_test_ratio) || (status.pos_test_ratio <= 0.f)) { status.pos_test_ratio = 1.f; } if (!PX4_ISFINITE(status.hgt_test_ratio) || (status.hgt_test_ratio <= 0.f)) { status.hgt_test_ratio = 1.f; } float combined_test_ratio = fmaxf(0.5f * (status.vel_test_ratio + status.pos_test_ratio), status.hgt_test_ratio); _instance[i].combined_test_ratio = combined_test_ratio; const bool healthy = (status.filter_fault_flags == 0) && (combined_test_ratio > 0.f); _instance[i].healthy.set_state_and_update(healthy, status.timestamp); _instance[i].warning = (combined_test_ratio >= 1.f); _instance[i].filter_fault = (status.filter_fault_flags != 0); _instance[i].timeout = false; if (!_instance[i].warning) { _instance[i].time_last_no_warning = status.timestamp_sample; } if (!PX4_ISFINITE(_instance[i].relative_test_ratio)) { _instance[i].relative_test_ratio = 0; } } else if (!_instance[i].timeout && (hrt_elapsed_time(&_instance[i].timestamp_sample_last) > status_timeout)) { _instance[i].healthy.set_state_and_update(false, hrt_absolute_time()); _instance[i].timeout = true; } // if the gyro used by the EKF is faulty, declare the EKF unhealthy without delay if (_gyro_fault_detected && (faulty_gyro_id != 0) && (_instance[i].gyro_device_id == faulty_gyro_id)) { _instance[i].healthy.set_state_and_update(false, hrt_absolute_time()); } // if the accelerometer used by the EKF is faulty, declare the EKF unhealthy without delay if (_accel_fault_detected && (faulty_accel_id != 0) && (_instance[i].accel_device_id == faulty_accel_id)) { _instance[i].healthy.set_state_and_update(false, hrt_absolute_time()); } if (prev_healthy != _instance[i].healthy.get_state()) { updated = true; _selector_status_publish = true; if (!prev_healthy) { _instance[i].healthy_count++; } } } // update relative test ratios if primary has updated if (primary_updated) { for (uint8_t i = 0; i < _available_instances; i++) { if (i != _selected_instance) { const float error_delta = _instance[i].combined_test_ratio - _instance[_selected_instance].combined_test_ratio; // reduce error only if its better than the primary instance by at least EKF2_SEL_ERR_RED to prevent unnecessary selection changes const float threshold = _gyro_fault_detected ? 0.0f : fmaxf(_param_ekf2_sel_err_red.get(), 0.05f); if (error_delta > 0 || error_delta < -threshold) { _instance[i].relative_test_ratio += error_delta; _instance[i].relative_test_ratio = constrain(_instance[i].relative_test_ratio, -_rel_err_score_lim, _rel_err_score_lim); if ((error_delta < -threshold) && (_instance[i].relative_test_ratio < 1.f)) { // increase status publication rate if there's movement towards a potential instance change _selector_status_publish = true; } } } } } return (primary_updated || updated); } void EKF2Selector::PublishVehicleAttitude() { // selected estimator_attitude -> vehicle_attitude vehicle_attitude_s attitude; if (_instance[_selected_instance].estimator_attitude_sub.update(&attitude)) { bool instance_change = false; if (_instance[_selected_instance].estimator_attitude_sub.get_instance() != _attitude_instance_prev) { _attitude_instance_prev = _instance[_selected_instance].estimator_attitude_sub.get_instance(); instance_change = true; } if (_attitude_last.timestamp != 0) { if (!instance_change && (attitude.quat_reset_counter == _attitude_last.quat_reset_counter + 1)) { // propogate deltas from estimator data while maintaining the overall reset counts ++_quat_reset_counter; _delta_q_reset = Quatf{attitude.delta_q_reset}; } else if (instance_change || (attitude.quat_reset_counter != _attitude_last.quat_reset_counter)) { // on reset compute deltas from last published data ++_quat_reset_counter; _delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized(); } } else { _quat_reset_counter = attitude.quat_reset_counter; _delta_q_reset = Quatf{attitude.delta_q_reset}; } bool publish = true; // ensure monotonically increasing timestamp_sample through reset, don't publish // estimator's attitude for system (vehicle_attitude) if it's stale if ((attitude.timestamp_sample <= _attitude_last.timestamp_sample) || (attitude.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { publish = false; } // save last primary estimator_attitude as published with original resets _attitude_last = attitude; if (publish) { // republish with total reset count and current timestamp attitude.quat_reset_counter = _quat_reset_counter; _delta_q_reset.copyTo(attitude.delta_q_reset); attitude.timestamp = hrt_absolute_time(); _vehicle_attitude_pub.publish(attitude); } } } void EKF2Selector::PublishVehicleLocalPosition() { // selected estimator_local_position -> vehicle_local_position vehicle_local_position_s local_position; if (_instance[_selected_instance].estimator_local_position_sub.update(&local_position)) { bool instance_change = false; if (_instance[_selected_instance].estimator_local_position_sub.get_instance() != _local_position_instance_prev) { _local_position_instance_prev = _instance[_selected_instance].estimator_local_position_sub.get_instance(); instance_change = true; } if (_local_position_last.timestamp != 0) { // XY reset if (!instance_change && (local_position.xy_reset_counter == _local_position_last.xy_reset_counter + 1)) { ++_xy_reset_counter; _delta_xy_reset = Vector2f{local_position.delta_xy}; } else if (instance_change || (local_position.xy_reset_counter != _local_position_last.xy_reset_counter)) { ++_xy_reset_counter; _delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y}; } // Z reset if (!instance_change && (local_position.z_reset_counter == _local_position_last.z_reset_counter + 1)) { ++_z_reset_counter; _delta_z_reset = local_position.delta_z; } else if (instance_change || (local_position.z_reset_counter != _local_position_last.z_reset_counter)) { ++_z_reset_counter; _delta_z_reset = local_position.z - _local_position_last.z; } // VXY reset if (!instance_change && (local_position.vxy_reset_counter == _local_position_last.vxy_reset_counter + 1)) { ++_vxy_reset_counter; _delta_vxy_reset = Vector2f{local_position.delta_vxy}; } else if (instance_change || (local_position.vxy_reset_counter != _local_position_last.vxy_reset_counter)) { ++_vxy_reset_counter; _delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy}; } // VZ reset if (!instance_change && (local_position.vz_reset_counter == _local_position_last.vz_reset_counter + 1)) { ++_vz_reset_counter; _delta_vz_reset = local_position.delta_vz; } else if (instance_change || (local_position.vz_reset_counter != _local_position_last.vz_reset_counter)) { ++_vz_reset_counter; _delta_vz_reset = local_position.vz - _local_position_last.vz; } // heading reset if (!instance_change && (local_position.heading_reset_counter == _local_position_last.heading_reset_counter + 1)) { ++_heading_reset_counter; _delta_heading_reset = local_position.delta_heading; } else if (instance_change || (local_position.heading_reset_counter != _local_position_last.heading_reset_counter)) { ++_heading_reset_counter; _delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading); } } else { _xy_reset_counter = local_position.xy_reset_counter; _z_reset_counter = local_position.z_reset_counter; _vxy_reset_counter = local_position.vxy_reset_counter; _vz_reset_counter = local_position.vz_reset_counter; _heading_reset_counter = local_position.heading_reset_counter; _delta_xy_reset = Vector2f{local_position.delta_xy}; _delta_z_reset = local_position.delta_z; _delta_vxy_reset = Vector2f{local_position.delta_vxy}; _delta_vz_reset = local_position.delta_vz; _delta_heading_reset = local_position.delta_heading; } bool publish = true; // ensure monotonically increasing timestamp_sample through reset, don't publish // estimator's local position for system (vehicle_local_position) if it's stale if ((local_position.timestamp_sample <= _local_position_last.timestamp_sample) || (local_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { publish = false; } // save last primary estimator_local_position as published with original resets _local_position_last = local_position; if (publish) { // republish with total reset count and current timestamp local_position.xy_reset_counter = _xy_reset_counter; local_position.z_reset_counter = _z_reset_counter; local_position.vxy_reset_counter = _vxy_reset_counter; local_position.vz_reset_counter = _vz_reset_counter; local_position.heading_reset_counter = _heading_reset_counter; _delta_xy_reset.copyTo(local_position.delta_xy); local_position.delta_z = _delta_z_reset; _delta_vxy_reset.copyTo(local_position.delta_vxy); local_position.delta_vz = _delta_vz_reset; local_position.delta_heading = _delta_heading_reset; local_position.timestamp = hrt_absolute_time(); _vehicle_local_position_pub.publish(local_position); } } } void EKF2Selector::PublishVehicleOdometry() { // selected estimator_odometry -> vehicle_odometry vehicle_odometry_s odometry; if (_instance[_selected_instance].estimator_odometry_sub.update(&odometry)) { bool publish = true; // ensure monotonically increasing timestamp_sample through reset, don't publish // estimator's odometry for system (vehicle_odometry) if it's stale if ((odometry.timestamp_sample <= _odometry_last.timestamp_sample) || (odometry.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { publish = false; } // save last primary estimator_odometry _odometry_last = odometry; if (publish) { odometry.timestamp = hrt_absolute_time(); _vehicle_odometry_pub.publish(odometry); } } } void EKF2Selector::PublishVehicleGlobalPosition() { // selected estimator_global_position -> vehicle_global_position vehicle_global_position_s global_position; if (_instance[_selected_instance].estimator_global_position_sub.update(&global_position)) { bool instance_change = false; if (_instance[_selected_instance].estimator_global_position_sub.get_instance() != _global_position_instance_prev) { _global_position_instance_prev = _instance[_selected_instance].estimator_global_position_sub.get_instance(); instance_change = true; } if (_global_position_last.timestamp != 0) { // lat/lon reset if (!instance_change && (global_position.lat_lon_reset_counter == _global_position_last.lat_lon_reset_counter + 1)) { ++_lat_lon_reset_counter; // TODO: delta latitude/longitude _delta_lat_reset = global_position.lat - _global_position_last.lat; _delta_lon_reset = global_position.lon - _global_position_last.lon; } else if (instance_change || (global_position.lat_lon_reset_counter != _global_position_last.lat_lon_reset_counter)) { ++_lat_lon_reset_counter; _delta_lat_reset = global_position.lat - _global_position_last.lat; _delta_lon_reset = global_position.lon - _global_position_last.lon; } // alt reset if (!instance_change && (global_position.alt_reset_counter == _global_position_last.alt_reset_counter + 1)) { ++_alt_reset_counter; _delta_alt_reset = global_position.delta_alt; } else if (instance_change || (global_position.alt_reset_counter != _global_position_last.alt_reset_counter)) { ++_alt_reset_counter; _delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt; } } else { _lat_lon_reset_counter = global_position.lat_lon_reset_counter; _alt_reset_counter = global_position.alt_reset_counter; _delta_alt_reset = global_position.delta_alt; } bool publish = true; // ensure monotonically increasing timestamp_sample through reset, don't publish // estimator's global position for system (vehicle_global_position) if it's stale if ((global_position.timestamp_sample <= _global_position_last.timestamp_sample) || (global_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { publish = false; } // save last primary estimator_global_position as published with original resets _global_position_last = global_position; if (publish) { // 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; global_position.delta_alt = _delta_alt_reset; global_position.timestamp = hrt_absolute_time(); _vehicle_global_position_pub.publish(global_position); } } } void EKF2Selector::PublishWindEstimate() { // selected estimator_wind -> wind wind_s wind; if (_instance[_selected_instance].estimator_wind_sub.update(&wind)) { bool publish = true; // ensure monotonically increasing timestamp_sample through reset, don't publish // estimator's wind for system (wind) if it's stale if ((wind.timestamp_sample <= _wind_last.timestamp_sample) || (wind.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { publish = false; } // save last primary wind _wind_last = wind; // publish estimator's wind for system unless it's stale if (publish) { // republish with current timestamp wind.timestamp = hrt_absolute_time(); _wind_pub.publish(wind); } } } void EKF2Selector::Run() { // re-schedule as backup timeout ScheduleDelayed(FILTER_UPDATE_PERIOD); // check for parameter updates if (_parameter_update_sub.updated()) { // clear update parameter_update_s pupdate; _parameter_update_sub.copy(&pupdate); // update parameters from storage updateParams(); } // update combined test ratio for all estimators const bool updated = UpdateErrorScores(); // 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].accel_device_id != 0) && (_instance[i].gyro_device_id != 0)) { if (SelectInstance(i)) { break; } } } // if still invalid return early and check again on next scheduled run if (_selected_instance == INVALID_INSTANCE) { return; } } 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 float best_test_ratio = FLT_MAX; uint8_t best_ekf = _selected_instance; uint8_t best_ekf_alternate = INVALID_INSTANCE; uint8_t best_ekf_different_imu = INVALID_INSTANCE; // loop through all available instances to find if an alternative is available for (int i = 0; i < _available_instances; i++) { // Use an alternative instance if - // (healthy and has updated recently) // AND // (has relative error less than selected instance and has not been the selected instance for at least 10 seconds // OR // selected instance has stopped updating if (_instance[i].healthy.get_state() && (i != _selected_instance)) { const float test_ratio = _instance[i].combined_test_ratio; const float relative_error = _instance[i].relative_test_ratio; if (relative_error < alternative_error) { best_ekf_alternate = i; alternative_error = relative_error; // relative error less than selected instance and has not been the selected instance for at least 10 seconds if ((relative_error <= -_rel_err_thresh) && hrt_elapsed_time(&_instance[i].time_last_selected) > 10_s) { lower_error_available = true; } } if ((test_ratio > 0) && (test_ratio < best_test_ratio)) { best_ekf = i; best_test_ratio = test_ratio; // also check next best available ekf using a different IMU if (_instance[i].accel_device_id != _instance[_selected_instance].accel_device_id) { best_ekf_different_imu = i; } } } } if (!_instance[_selected_instance].healthy.get_state()) { // prefer the best healthy instance using a different IMU if (!SelectInstance(best_ekf_different_imu)) { // otherwise switch to the healthy instance with best overall test ratio SelectInstance(best_ekf); } } else if (lower_error_available && ((hrt_elapsed_time(&_last_instance_change) > 10_s) || (_instance[_selected_instance].warning && (hrt_elapsed_time(&_instance[_selected_instance].time_last_no_warning) > 1_s)))) { // if this instance has a significantly lower relative error to the active primary, we consider it as a // better instance and would like to switch to it even if the current primary is healthy SelectInstance(best_ekf_alternate); } else if (_request_instance.load() != INVALID_INSTANCE) { const uint8_t new_instance = _request_instance.load(); // attempt to switch to user manually selected instance if (!SelectInstance(new_instance)) { PX4_ERR("unable to switch to user selected instance %d", new_instance); } // reset _request_instance.store(INVALID_INSTANCE); } // 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) { PublishEstimatorSelectorStatus(); _selector_status_publish = false; } } // republish selected estimator data for system PublishVehicleAttitude(); PublishVehicleLocalPosition(); PublishVehicleGlobalPosition(); PublishVehicleOdometry(); PublishWindEstimate(); } void EKF2Selector::PublishEstimatorSelectorStatus() { 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].accel_device_id; selector_status.baro_device_id = _instance[_selected_instance].baro_device_id; selector_status.gyro_device_id = _instance[_selected_instance].gyro_device_id; selector_status.mag_device_id = _instance[_selected_instance].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.get_state(); } 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); _last_status_publish = selector_status.timestamp; } void EKF2Selector::PrintStatus() { PX4_INFO("available instances: %" PRIu8, _available_instances); if (_selected_instance == INVALID_INSTANCE) { PX4_WARN("selected instance: None"); } for (int i = 0; i < _available_instances; i++) { const EstimatorInstance &inst = _instance[i]; PX4_INFO("%" PRIu8 ": ACC: %" PRIu32 ", GYRO: %" PRIu32 ", MAG: %" PRIu32 ", %s, test ratio: %.7f (%.5f) %s", inst.instance, inst.accel_device_id, inst.gyro_device_id, inst.mag_device_id, inst.healthy.get_state() ? "healthy" : "unhealthy", (double)inst.combined_test_ratio, (double)inst.relative_test_ratio, (_selected_instance == i) ? "*" : ""); } }