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

AP_NavEKF: Compensate optical flow fusion for GPS glitch recovery offset

Browse Source
This commit is contained in:
priseborough 2014-12-21 11:31:46 +11:00 committed by Randy Mackay
parent e4c969084d
commit b160f4c03b
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2723,8 +2723,9 @@ void NavEKF::RunAuxiliaryEKF()
q1 = statesAtFlowTime.quat[1]; q1 = statesAtFlowTime.quat[1];
q2 = statesAtFlowTime.quat[2]; q2 = statesAtFlowTime.quat[2];
q3 = statesAtFlowTime.quat[3]; q3 = statesAtFlowTime.quat[3];
vel.x = statesAtFlowTime.velocity[0]; // Correct velocities for GPS glitch recovery offset
vel.y = statesAtFlowTime.velocity[1]; vel.x = statesAtFlowTime.velocity[0] - gpsVelGlitchOffset.x;
vel.y = statesAtFlowTime.velocity[1] - gpsVelGlitchOffset.y;
vel.z = statesAtFlowTime.velocity[2]; vel.z = statesAtFlowTime.velocity[2];
// constrain terrain height to be below the vehicle // constrain terrain height to be below the vehicle
@ -2857,8 +2858,9 @@ void NavEKF::FuseOptFlow()
ve = statesAtFlowTime.velocity[1]; ve = statesAtFlowTime.velocity[1];
vd = statesAtFlowTime.velocity[2]; vd = statesAtFlowTime.velocity[2];
pd = statesAtFlowTime.position[2]; pd = statesAtFlowTime.position[2];
velNED_local.x = vn; // Correct velocities for GPS glitch recovery offset
velNED_local.y = ve; velNED_local.x = vn - gpsVelGlitchOffset.x;
velNED_local.y = ve - gpsVelGlitchOffset.y;
velNED_local.z = vd; velNED_local.z = vd;
// constrain terrain to be below vehicle // constrain terrain to be below vehicle