zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF get_ekf_soln_status() fix pred_pos_horiz_abs

master
Daniel Agar 7 years ago
parent
1bd1809d6e
commit
eec71d1a10
1 changed files with 3 additions and 2 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      EKF/ekf_helper.cpp

5
EKF/ekf_helper.cpp
Unescape View File

@ -1131,7 +1131,8 @@ void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, f @@ -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
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.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);
@ -1141,7 +1142,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) @@ -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.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_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_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);

Write Preview
Loading…
Cancel
Save