fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 16:17:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF get_ekf_soln_status() fix pred_pos_horiz_abs

Browse Source
This commit is contained in:
Daniel Agar
2018-04-23 21:13:52 -04:00
parent 1bd1809d6e
commit eec71d1a10
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+3 -2
View File
@@ -1131,7 +1131,8 @@ void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, f
// return a bitmask integer that describes which state estimates are valid // return a bitmask integer that describes which state estimates are valid
void Ekf::get_ekf_soln_status(uint16_t *status) void Ekf::get_ekf_soln_status(uint16_t *status)
{ {
ekf_solution_status soln_status{}; ekf_solution_status soln_status;
soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0); soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
@@ -1141,7 +1142,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
soln_status.flags.pos_vert_agl = get_terrain_valid(); soln_status.flags.pos_vert_agl = get_terrain_valid();
soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_vert_abs; soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
bool gps_vel_innov_bad = (_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f); bool gps_vel_innov_bad = (_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f);
bool gps_pos_innov_bad = (_vel_pos_test_ratio[3] > 1.0f) || (_vel_pos_test_ratio[4] > 1.0f); bool gps_pos_innov_bad = (_vel_pos_test_ratio[3] > 1.0f) || (_vel_pos_test_ratio[4] > 1.0f);
bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f); bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f);