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

AP_NavEKF2: Stop bad INS velocity spoiling height reset 

We should reset the velocity as well as the height if a reasonable reset value is available.
This commit is contained in:
Paul Riseborough 2015-11-13 14:54:02 +11:00 committed by Andrew Tridgell
parent 97799ef0b3
commit af3507ef3c
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -93,6 +93,19 @@ void NavEKF2_core::ResetHeight(void)
}
outputDataNew.position.z = stateStruct.position.z;
outputDataDelayed.position.z = stateStruct.position.z;
// Reset the vertical velocity state using GPS vertical velocity if we are airborne
// Check that GPS vertical velocity data is available and can be used
if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0) {
stateStruct.velocity.z = gpsDataNew.vel.z;
} else if (onGround) {
stateStruct.velocity.z = 0.0f;
}
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
storedOutput[i].velocity.z = stateStruct.velocity.z;
}
outputDataNew.velocity.z = stateStruct.velocity.z;
outputDataDelayed.velocity.z = stateStruct.velocity.z;
}
// Reset the baro so that it reads zero at the current height