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

AP_NavEKF2: Publish the position and velocity state reset deltas

Browse Source 
Used by the control loops to compensate for step changes in position and velocity when the EKF needs to do a reset of these states.
This commit is contained in:
Paul Riseborough 2015-10-29 15:03:51 +11:00 committed by Randy Mackay
parent 7c40448bab
commit 4fdec67546
2 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -128,6 +128,22 @@ uint32_t NavEKF2_core::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 NavEKF2_core::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 NavEKF2_core::getLastVelNorthEastReset(Vector2f &vel)
{
vel = velResetNE;
return lastVelReset_ms;
}
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void NavEKF2_core::getWind(Vector3f &wind) const void NavEKF2_core::getWind(Vector3f &wind) const
{ {

12
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -248,6 +248,14 @@ 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);
// 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);
private: private:
// Reference to the global EKF frontend for parameters // Reference to the global EKF frontend for parameters
NavEKF2 &frontend; NavEKF2 &frontend;
@ -765,6 +773,10 @@ private:
bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed
bool magFuseTiltInhibit; // true when the 3-axis magnetoemter fusion is prevented from changing tilt angle bool magFuseTiltInhibit; // true when the 3-axis magnetoemter fusion is prevented from changing tilt angle
uint32_t magFuseTiltInhibit_ms; // time in msec that the condition indicated by magFuseTiltInhibit was commenced uint32_t magFuseTiltInhibit_ms; // time in msec that the condition indicated by magFuseTiltInhibit was commenced
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
// variables used to calulate a vertical velocity that is kinematically consistent with the verical position // variables used to calulate a vertical velocity that is kinematically consistent with the verical position
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.