Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix bug in reporting of horizontal position reset

Browse Source 
Fixes a bug that allows the last reported reset time to to wind back and an out of date reset delta to be reported if a switch to a core that has previously been reset occurs.
Allows multiple consumers provided they access on the same frame.
This commit is contained in:
priseborough 2016-11-30 11:02:41 +11:00 committed by Randy Mackay
parent 7c9d2b6ca7
commit 56262f44f4
1 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -1215,27 +1215,34 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAngDelta)
return lastYawReset_ms; return lastYawReset_ms;
} }
// return the amount of NE position change due to the last position reset in metres // Returns the amount of NE position change due to the last position reset or core switch in metres
// returns the time of the last reset or 0 if no reset has ever occurred // Returns the time of the last reset or 0 if no reset or core switch has ever occurred
// Where there are multiple consumers, they must access this function on the same frame as each other
uint32_t NavEKF2::getLastPosNorthEastReset(Vector2f &posDelta) uint32_t NavEKF2::getLastPosNorthEastReset(Vector2f &posDelta)
{ {
if (!core) { if (!core) {
return 0; return 0;
} }
// Record last time controller got the position reset
pos_reset_data.last_function_call = imuSampleTime_us / 1000;
posDelta.zero(); posDelta.zero();
uint32_t lastPosReset_ms = 0;
// Do the conversion to msec in one place
uint32_t now_time_ms = imuSampleTime_us / 1000;
// The last time we switched to the current primary core is the first reset event
uint32_t lastPosReset_ms = pos_reset_data.last_primary_change;
// There has been a change in the primary core that the controller has not consumed // There has been a change in the primary core that the controller has not consumed
if (pos_reset_data.core_changed) { // allow for multiple consumers on the same frame
if (pos_reset_data.core_changed || pos_reset_data.last_function_call == now_time_ms) {
posDelta = pos_reset_data.core_delta; posDelta = pos_reset_data.core_delta;
lastPosReset_ms = pos_reset_data.last_primary_change;
pos_reset_data.core_changed = false; pos_reset_data.core_changed = false;
} }
// There has been a reset inside the core since we switched // Record last time controller got the position reset
pos_reset_data.last_function_call = now_time_ms;
// There has been a reset inside the core since we switched so update the time and delta
Vector2f tempPosDelta; Vector2f tempPosDelta;
uint32_t lastCorePosReset_ms = core[primary].getLastPosNorthEastReset(tempPosDelta); uint32_t lastCorePosReset_ms = core[primary].getLastPosNorthEastReset(tempPosDelta);
if (lastCorePosReset_ms > lastPosReset_ms) { if (lastCorePosReset_ms > lastPosReset_ms) {