Browse Source

AP_NavEKF3: set terrainState to zero on height datum reset

mission-4.1.18
Andrew Tridgell 6 years ago
parent
commit
3f9e48951b
  1. 6
      libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

6
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@ -227,8 +227,10 @@ bool NavEKF3_core::resetHeightDatum(void) @@ -227,8 +227,10 @@ bool NavEKF3_core::resetHeightDatum(void)
}
}
// adjust the terrain state
terrainState += oldHgt;
// set the terrain state to zero (on ground). The adjustment for
// frame height will get added in the later constraints
terrainState = 0;
return true;
}

Loading…
Cancel
Save