Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Fix bug allowing terrain to be above vehicle position 

The terrain state and vehicle state need to be compared at the same time horizon.
This commit is contained in:
Paul Riseborough 2015-04-10 18:02:26 +10:00 committed by Randy Mackay
parent 6d58c63c4c
commit e79ccf1fcc
1 changed files with 2 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3938,6 +3938,8 @@ void NavEKF::ConstrainStates()
for (uint8_t i=16; i<=18; i++) states[i] = constrain_float(states[i],-1.0f,1.0f); for (uint8_t i=16; i<=18; i++) states[i] = constrain_float(states[i],-1.0f,1.0f);
// body magnetic field limit // body magnetic field limit
for (uint8_t i=19; i<=21; i++) states[i] = constrain_float(states[i],-0.5f,0.5f); for (uint8_t i=19; i<=21; i++) states[i] = constrain_float(states[i],-0.5f,0.5f);
// constrain the terrain offset state
terrainState = max(terrainState, state.position.z + RNG_MEAS_ON_GND);
} }
// update IMU delta angle and delta velocity measurements // update IMU delta angle and delta velocity measurements