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

AP_NavEKF: Publish the INS delta quaternion

Browse Source
This commit is contained in:
Paul Riseborough 2015-04-27 21:42:28 +10:00 committed by Randy Mackay
parent d273302ce7
commit 5dc29699ab
2 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -677,6 +677,11 @@ bool NavEKF::InitialiseFilterBootstrap(void)
// Update Filter States - this should be called whenever new IMU data is available
void NavEKF::UpdateFilter()
{
// zero the delta quaternion used by the strapdown navigation because it is published
// and we need to return a zero rotation of the INS fails to update it
memset(&correctedDelAngQuat[0], 0, sizeof(correctedDelAngQuat));
correctedDelAngQuat[0] = 1.0f;
// don't run filter updates if states have not been initialised
if (!statesInitialised) {
return;
@ -5101,4 +5106,13 @@ bool NavEKF::getHeightControlLimit(float &height) const
}
}
// provides the delta quaternion that was used by the INS calculation to rotate from the previous orientation to the orientation at the current time
// the delta quaternion returned will be a zero rotation if the INS calculation was not performed on that time step
Quaternion NavEKF::getDeltaQuaternion(void) const
{
// Note: correctedDelAngQuat is reset to a zero rotation at the start of every update cycle in UpdateFilter()
return correctedDelAngQuat;
}
#endif // HAL_CPU_CLASS

4
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -247,6 +247,10 @@ public:
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation
bool getHeightControlLimit(float &height) const;
// provides the quaternion that was used by the INS calculation to rotate from the previous orientation to the orientaion at the current time step
// returns a zero rotation quaternion if the INS calculation was not performed on that time step.
Quaternion getDeltaQuaternion(void) const;
static const struct AP_Param::GroupInfo var_info[];
private: