fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/ecl_ekf/analysis/metrics.py
View File

@ -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'),

2
Tools/ecl_ekf/plotting/pdf_report.py
View File

@ -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

2
msg/EstimatorStatus.msg
View File

@ -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

2
src/drivers/ins/vectornav/VectorNav.cpp
View File

@ -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;

6
src/modules/airspeed_selector/AirspeedValidator.cpp
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;

4
src/modules/airspeed_selector/AirspeedValidator.hpp
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);

2
src/modules/airspeed_selector/airspeed_selector_main.cpp
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;

4
src/modules/ekf2/EKF2.cpp
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);

2
src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp
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;