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

AP_NavEKF2: Add method to rotate output quaternion history

Browse Source
This commit is contained in:
Paul Riseborough 2015-10-29 10:28:11 +11:00 committed by Randy Mackay
parent a1e32d71ec
commit 844ed95718
2 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -1223,6 +1223,17 @@ void NavEKF2_core::StoreQuatReset()
outputDataDelayed.quat = outputDataNew.quat;
}
// Rotate the stored output quaternion history through a quaternion rotation
void NavEKF2_core::StoreQuatRotate(Quaternion deltaQuat)
{
outputDataNew.quat = outputDataNew.quat*deltaQuat;
// write current measurement to entire table
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
storedOutput[i].quat = storedOutput[i].quat*deltaQuat;
}
outputDataDelayed.quat = outputDataDelayed.quat*deltaQuat;
}
// recall output data from the FIFO
void NavEKF2_core::RecallOutput()
{

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -428,6 +428,9 @@ private:
// Reset the stored output quaternion history to current EKF state
void StoreQuatReset(void);
// Rotate the stored output quaternion history through a quaternion rotation
void StoreQuatRotate(Quaternion deltaQuat);
// recall output data from the FIFO
void RecallOutput();