mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-02 05:58:30 -04:00
AP_NavEKF : Fixes bug that causes accel bias to diverge in static mode, preventing copter arming
This commit is contained in:
parent
71f61c9bef
commit
b2c0979947
@ -1544,7 +1544,7 @@ void NavEKF::FuseVelPosNED()
|
||||
// because there may be no stored states due to lack of real measurements.
|
||||
// in static mode, only position and height fusion is used
|
||||
if (staticMode) {
|
||||
for (uint8_t i=7; i<=9; i++) {
|
||||
for (uint8_t i=0; i<=30; i++) {
|
||||
statesAtPosTime[i] = states[i];
|
||||
statesAtHgtTime[i] = states[i];
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user