|
|
|
@ -1191,6 +1191,24 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
@@ -1191,6 +1191,24 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
|
|
|
|
|
return 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// return the amount of vertical position change due to the last reset in meters
|
|
|
|
|
// returns the time of the last reset or 0 if no reset has ever occurred
|
|
|
|
|
uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta) const |
|
|
|
|
{ |
|
|
|
|
switch (ekf_type()) { |
|
|
|
|
case 1: |
|
|
|
|
return 0; |
|
|
|
|
case 2: |
|
|
|
|
return EKF2.getLastPosDownReset(posDelta); |
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
|
|
|
case EKF_TYPE_SITL: |
|
|
|
|
return 0; |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
return 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Resets the baro so that it reads zero at the current height
|
|
|
|
|
// Resets the EKF height to zero
|
|
|
|
|
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
|
|
|
|
|