Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: correct extnav position as it is recalled from the buffer 

This makes extnav position correction consistent with the velocity correction
This commit is contained in:
Randy Mackay 2020-07-04 11:51:15 +09:00 committed by Andrew Tridgell
parent 225de23950
commit a16de76f57
1 changed files with 5 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -123,10 +123,8 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
lastRngBcnPassTime_ms = imuSampleTime_ms; lastRngBcnPassTime_ms = imuSampleTime_ms;
} else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::EXTNAV) { } else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::EXTNAV) {
// use external nav data as the third preference // use external nav data as the third preference
ext_nav_elements extNavCorrected = extNavDataDelayed; stateStruct.position.x = extNavDataDelayed.pos.x;
CorrectExtNavForSensorOffset(extNavCorrected.pos); stateStruct.position.y = extNavDataDelayed.pos.y;
stateStruct.position.x = extNavCorrected.pos.x;
stateStruct.position.y = extNavCorrected.pos.y;
// set the variances as received from external nav system data // set the variances as received from external nav system data
P[7][7] = P[8][8] = sq(extNavDataDelayed.posErr); P[7][7] = P[8][8] = sq(extNavDataDelayed.posErr);
// clear the timeout flags and counters // clear the timeout flags and counters
@ -402,6 +400,9 @@ void NavEKF3_core::SelectVelPosFusion()
// Check for data at the fusion time horizon // Check for data at the fusion time horizon
extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms); extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
if (extNavDataToFuse) {
CorrectExtNavForSensorOffset(extNavDataDelayed.pos);
}
extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms); extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms);
if (extNavVelToFuse) { if (extNavVelToFuse) {
CorrectExtNavVelForSensorOffset(extNavVelDelayed.vel); CorrectExtNavVelForSensorOffset(extNavVelDelayed.vel);
@ -445,10 +446,6 @@ void NavEKF3_core::SelectVelPosFusion()
// use external nav system for horizontal position // use external nav system for horizontal position
extNavUsedForPos = true; extNavUsedForPos = true;
fusePosData = true; fusePosData = true;
// correct for external navigation sensor position
CorrectExtNavForSensorOffset(extNavDataDelayed.pos);
velPosObs[3] = extNavDataDelayed.pos.x; velPosObs[3] = extNavDataDelayed.pos.x;
velPosObs[4] = extNavDataDelayed.pos.y; velPosObs[4] = extNavDataDelayed.pos.y;
} }