Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: publish position and velocity state reset data

Browse Source 
Used by the control loops to compensate for steps in velocity and/or position resulting from state resets.
This commit is contained in:
Paul Riseborough 2015-10-29 14:10:13 +11:00 committed by Randy Mackay
parent 0d36c4c07b
commit 5489ff9118
2 changed files with 30 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -5616,6 +5616,22 @@ uint32_t NavEKF::getLastYawResetAngle(float &yawAng)
return lastYawReset_ms; return lastYawReset_ms;
} }
// return the amount of NE position change due to the last position reset in metres
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF::getLastPosNorthEastReset(Vector2f &pos)
{
pos = posResetNE;
return lastPosReset_ms;
}
// return the amount of NE velocity change due to the last velocity reset in metres/sec
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF::getLastVelNorthEastReset(Vector2f &vel)
{
vel = velResetNE;
return lastVelReset_ms;
}
// Check for signs of bad gyro health before flight // Check for signs of bad gyro health before flight
bool NavEKF::checkGyroHealthPreFlight(void) const bool NavEKF::checkGyroHealthPreFlight(void) const
{ {

15
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -280,7 +280,15 @@ public:
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t getLastYawResetAngle(float &yawAng); uint32_t getLastYawResetAngle(float &yawAng);
// report any reason for why the backend is refusing to initialise // return the amount of NE position change due to the last position reset in metres
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosNorthEastReset(Vector2f &pos);
// return the amount of NE velocity change due to the last velocity reset in metres/sec
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastVelNorthEastReset(Vector2f &vel);
// report any reason for why the backend is refusing to initialise
const char *prearm_failure_reason(void) const; const char *prearm_failure_reason(void) const;
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
@ -721,6 +729,11 @@ private:
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD. float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
float posDown; // Down position state used in calculation of posDownRate float posDown; // Down position state used in calculation of posDownRate
Vector3f delAngBiasAtArming; // value of the gyro delta angle bias at arming Vector3f delAngBiasAtArming; // value of the gyro delta angle bias at arming
Vector2f posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
uint32_t lastPosReset_ms; // System time at which the last position reset occurred. Returned by getLastPosNorthEastReset
Vector2f velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
uint32_t lastVelReset_ms; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
// Used by smoothing of state corrections // Used by smoothing of state corrections
Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement