fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 04:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

msgs/EstimatorStatus.msg rename mag_test_ratio -> hdg_test_ratio

Browse Source 
- this is used for more than just mag
This commit is contained in:
Daniel Agar
2024-07-17 10:26:39 -04:00
committed by Mathieu Bresciani
parent eac14b7db2
commit ca9948a84d
9 changed files with 13 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airspeed_selector/AirspeedValidator.cpp
+3 -3
View File
@@ -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;
src/modules/airspeed_selector/AirspeedValidator.hpp
+2 -2
View File
@@ -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);
src/modules/airspeed_selector/airspeed_selector_main.cpp
+1 -1
View File
@@ -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;
src/modules/ekf2/EKF2.cpp
+2 -2
View File
@@ -1796,7 +1796,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
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 &timestamp)
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);
src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp
+1 -1
View File
@@ -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;