aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: fix getLastYawResetAngle to return yaw reset system time

Browse Source
This commit is contained in:
Jonathan Challinger 2015-09-17 15:00:06 -07:00 committed by Randy Mackay
parent 06c0ad987e
commit 51fb13a329
2 changed files with 13 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4516,10 +4516,11 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
// store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset
Vector3f tempEuler;
state.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
yawResetAngle = wrap_PI(yaw - tempEuler.z);
// set the flag to indicate that an in-flight yaw reset has been performed
// this will be cleared when the reset value is retrieved
yawResetAngleWaiting = true;
if (imuSampleTime_ms != lastYawReset_ms) {
yawResetAngle = 0;
}
yawResetAngle += wrap_PI(yaw - tempEuler.z);
lastYawReset_ms = imuSampleTime_ms;
// calculate an initial quaternion using the new yaw value
initQuat.from_euler(roll, pitch, yaw);
} else {
@ -4777,7 +4778,7 @@ void NavEKF::InitialiseVariables()
highYawRate = false;
yawRateFilt = 0.0f;
yawResetAngle = 0.0f;
yawResetAngleWaiting = false;
lastYawReset_ms = 0;
imuNoiseFiltState1 = 0.0f;
imuNoiseFiltState2 = 0.0f;
lastImuSwitchState = IMUSWITCH_MIXED;
@ -5399,18 +5400,11 @@ void NavEKF::alignMagStateDeclination()
}
// return the amount of yaw angle change due to the last yaw angle reset in radians
// returns true if a reset yaw angle has been updated and not queried
// this function should not have more than one client
bool NavEKF::getLastYawResetAngle(float &yawAng)
// returns the system time at which the yaw angle was reset
uint32_t NavEKF::getLastYawResetAngle(float &yawAng)
{
if (yawResetAngleWaiting) {
yawAng = yawResetAngle;
yawResetAngleWaiting = false;
return true;
} else {
yawAng = yawResetAngle;
return false;
}
yawAng = yawResetAngle;
return lastYawReset_ms;
}
// Check for signs of bad gyro health before flight

7
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -259,9 +259,8 @@ public:
bool getHeightControlLimit(float &height) const;
// return the amount of yaw angle change due to the last yaw angle reset in radians
// returns true if a reset yaw angle has been updated and not queried
// this function should not have more than one client
bool getLastYawResetAngle(float &yawAng);
// returns the system time at which the yaw angle was reset
uint32_t getLastYawResetAngle(float &yawAng);
// report any reason for why the backend is refusing to initialise
const char *prearm_failure_reason(void) const;
@ -683,7 +682,7 @@ private:
float yawRateFilt; // filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
bool useGpsVertVel; // true if GPS vertical velocity should be used
float yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
bool yawResetAngleWaiting; // true when the yaw reset angle has been updated and has not been retrieved via the getLastYawResetAngle() function
uint32_t lastYawReset_ms; // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle
uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
bool consistentMagData; // true when the magnetometers are passing consistency checks