diff --git a/Tools/ecl_ekf/analysis/metrics.py b/Tools/ecl_ekf/analysis/metrics.py index d435f88af9..47a8bb412e 100644 --- a/Tools/ecl_ekf/analysis/metrics.py +++ b/Tools/ecl_ekf/analysis/metrics.py @@ -54,7 +54,7 @@ def calculate_sensor_metrics( # calculates peak, mean, percentage above 0.5 std, and percentage above std metrics for # estimator status variables for signal, result_id in [('hgt_test_ratio', 'hgt'), - ('mag_test_ratio', 'mag'), + ('hdg_test_ratio', 'mag'), ('vel_test_ratio', 'vel'), ('pos_test_ratio', 'pos'), ('tas_test_ratio', 'tas'), diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index 029589ba56..35821fbc46 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -160,7 +160,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str # plot normalised innovation test levels # define variables to plot - variables = [['mag_test_ratio'], ['vel_test_ratio', 'pos_test_ratio'], ['hgt_test_ratio']] + variables = [['hdg_test_ratio'], ['vel_test_ratio', 'pos_test_ratio'], ['hgt_test_ratio']] y_labels = ['mag', 'vel, pos', 'hgt'] legend = [['mag'], ['vel', 'pos'], ['hgt']] if np.amax(estimator_status['hagl_test_ratio']) > 0.0: # plot hagl test ratio, if applicable diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index 0b479b1545..dd62bc4aca 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -71,7 +71,7 @@ uint32 filter_fault_flags # Bitmask to indicate EKF internal faults float32 pos_horiz_accuracy # 1-Sigma estimated horizontal position accuracy relative to the estimators origin (m) float32 pos_vert_accuracy # 1-Sigma estimated vertical position accuracy relative to the estimators origin (m) -float32 mag_test_ratio # low-pass filtered ratio of the largest magnetometer innovation component to the innovation test limit +float32 hdg_test_ratio # low-pass filtered ratio of the largest heading innovation component to the innovation test limit float32 vel_test_ratio # low-pass filtered ratio of the largest velocity innovation component to the innovation test limit float32 pos_test_ratio # low-pass filtered ratio of the largest horizontal position innovation component to the innovation test limit float32 hgt_test_ratio # low-pass filtered ratio of the vertical position innovation to the innovation test limit diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index cbe7ab5352..85dd4c1846 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -368,7 +368,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet) test_ratio = 0.1f; } - estimator_status.mag_test_ratio = test_ratio; + estimator_status.hdg_test_ratio = test_ratio; estimator_status.vel_test_ratio = test_ratio; estimator_status.pos_test_ratio = test_ratio; estimator_status.hgt_test_ratio = test_ratio; diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 4ebacdbeb8..acdc88ef22 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -58,7 +58,7 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat update_in_fixed_wing_flight(input_data.in_fixed_wing_flight); check_airspeed_data_stuck(input_data.timestamp); check_load_factor(input_data.accel_z); - check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio, + check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.hdg_test_ratio, input_data.ground_velocity, input_data.gnss_valid); check_first_principle(input_data.timestamp, input_data.fixed_wing_tecs_throttle, input_data.fixed_wing_tecs_throttle_trim, input_data.tecs_timestamp, input_data.q_att); @@ -214,7 +214,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now) void AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio, - float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool gnss_valid) + float estimator_status_hdg_test_ratio, const matrix::Vector3f &vI, bool gnss_valid) { // Check normalised innovation levels with requirement for continuous data and use of hysteresis // to prevent false triggering. @@ -228,7 +228,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ _innovations_check_failed = false; _aspd_innov_integ_state = 0.f; - } else if (!gnss_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) { + } else if (!gnss_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_hdg_test_ratio > 1.f) { //nav velocity data is likely not good //don't run the test but don't reset the check if it had previously failed when nav velocity data was still likely good _aspd_innov_integ_state = 0.f; diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 19c3cc88d1..74f5747989 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -66,7 +66,7 @@ struct airspeed_validator_update_data { float air_temperature_celsius; float accel_z; float vel_test_ratio; - float mag_test_ratio; + float hdg_test_ratio; bool in_fixed_wing_flight; float fixed_wing_tecs_throttle; float fixed_wing_tecs_throttle_trim; @@ -213,7 +213,7 @@ private: void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius); void check_airspeed_data_stuck(uint64_t timestamp); void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio, - float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool gnss_valid); + float estimator_status_hdg_test_ratio, const matrix::Vector3f &vI, bool gnss_valid); void check_load_factor(float accel_z); void check_first_principle(const uint64_t timestamp, const float throttle, const float throttle_trim, const uint64_t tecs_timestamp, const Quatf &att_q); diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index a3f8bcbc23..1880d5ab3c 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -370,7 +370,7 @@ AirspeedModule::Run() input_data.air_pressure_pa = _vehicle_air_data.baro_pressure_pa; input_data.accel_z = _accel.xyz[2]; input_data.vel_test_ratio = _estimator_status.vel_test_ratio; - input_data.mag_test_ratio = _estimator_status.mag_test_ratio; + input_data.hdg_test_ratio = _estimator_status.hdg_test_ratio; input_data.tecs_timestamp = _tecs_status.timestamp; input_data.fixed_wing_tecs_throttle = _tecs_status.throttle_sp; input_data.fixed_wing_tecs_throttle_trim = _tecs_status.throttle_trim; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 929e3125fa..b5d5dda1ce 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1796,7 +1796,7 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.vel_test_ratio = NAN; } - status.mag_test_ratio = _ekf.getHeadingInnovationTestRatio(); + status.hdg_test_ratio = _ekf.getHeadingInnovationTestRatio(); status.pos_test_ratio = _ekf.getHorizontalPositionInnovationTestRatio(); status.hgt_test_ratio = _ekf.getVerticalPositionInnovationTestRatio(); status.tas_test_ratio = _ekf.getAirspeedInnovationTestRatio(); @@ -1816,7 +1816,7 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.time_slip = _last_time_slip_us * 1e-6f; static constexpr float kMinTestRatioPreflight = 0.5f; - status.pre_flt_fail_innov_heading = (kMinTestRatioPreflight < status.mag_test_ratio); + status.pre_flt_fail_innov_heading = (kMinTestRatioPreflight < status.hdg_test_ratio); status.pre_flt_fail_innov_height = (kMinTestRatioPreflight < status.hgt_test_ratio); status.pre_flt_fail_innov_pos_horiz = (kMinTestRatioPreflight < status.pos_test_ratio); status.pre_flt_fail_innov_vel_horiz = (kMinTestRatioPreflight < vel_xy_test_ratio); diff --git a/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp b/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp index 3764940c31..0a27a90eb0 100644 --- a/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp +++ b/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp @@ -80,7 +80,7 @@ private: est_msg.vel_ratio = est.vel_test_ratio; est_msg.pos_horiz_ratio = est.pos_test_ratio; est_msg.pos_vert_ratio = est.hgt_test_ratio; - est_msg.mag_ratio = est.mag_test_ratio; + est_msg.mag_ratio = est.hdg_test_ratio; est_msg.hagl_ratio = est.hagl_test_ratio; est_msg.tas_ratio = est.tas_test_ratio; est_msg.pos_horiz_accuracy = est.pos_horiz_accuracy;