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

EKF: Ensure use of EV aiding inhibits use of other height sources

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-14 21:17:29 +10:00
parent cf1df5861a
commit ac9b7a3df6
2 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+10 -2
View File
@@ -60,8 +60,12 @@ void Ekf::controlFusionModes()
if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
// check for a exernal vision measurement that has fallen behind the fusion time horizon // check for a exernal vision measurement that has fallen behind the fusion time horizon
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
// turn on use of external vision measurements for position // turn on use of external vision measurements for position and height
_control_status.flags.ev_pos = true; _control_status.flags.ev_pos = true;
// turn off other forms of height aiding
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
// reset the position, height and velocity // reset the position, height and velocity
resetPosition(); resetPosition();
resetVelocity(); resetVelocity();
@@ -456,7 +460,11 @@ void Ekf::controlFusionModes()
} }
// Control the soure of height measurements for the main filter // Control the soure of height measurements for the main filter
if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) { if (_control_status.flags.ev_pos) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) {
_control_status.flags.baro_hgt = true; _control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false; _control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false; _control_status.flags.rng_hgt = false;
EKF/vel_pos_fusion.cpp
+1 -1
View File
@@ -120,7 +120,7 @@ void Ekf::fuseVelPosHeight()
} }
if (_fuse_height) { if (_fuse_height) {
if (_control_status.flags.baro_hgt && !_control_status.flags.gps && !_control_status.flags.ev_pos && !(_control_status.flags.rng_hgt && (_R_to_earth(2, 2) > 0.7071f))) { if (_control_status.flags.baro_hgt) {
fuse_map[5] = true; fuse_map[5] = true;
// vertical position innovation - baro measurement has opposite sign to earth z axis // vertical position innovation - baro measurement has opposite sign to earth z axis
_vel_pos_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset; _vel_pos_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;