fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 20:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ekf: use helper functions to centralize the checks of horizontal aiding (gps, flow, ev_pos, ev_vel)

Browse Source
This commit is contained in:
bresch
2020-01-24 17:22:50 +04:00
committed by Kabir Mohammed
parent a3e1eb9d50
commit e3af91c9ce
5 changed files with 59 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+2 -4
View File
@@ -1231,7 +1231,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid;
const bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel);
const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
// Do not require limiting by default
*vxy_max = NAN;
@@ -1313,7 +1313,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
ekf_solution_status soln_status;
// TODO: Is this accurate enough?
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.ev_vel || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_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.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
@@ -1375,8 +1375,6 @@ void Ekf::uncorrelateQuatStates()
P.uncorrelateCovariance<4>(0);
}
bool Ekf::global_position_is_valid()
{
// return true if the origin is set we are not doing unconstrained free inertial navigation