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

AP_NavEKF2: Fix errors in position reset delta publishing

Browse Source
This commit is contained in:
priseborough 2016-11-22 21:43:32 +11:00 committed by Randy Mackay
parent 52eac34d66
commit 9bfbe4c80a
2 changed files with 21 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -625,6 +625,8 @@ bool NavEKF2::InitialiseFilter(void)
}
// zero the structs used capture reset events
memset(&yaw_reset_data, 0, sizeof(yaw_reset_data));
memset(&pos_reset_data, 0, sizeof(pos_reset_data));
memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data));
check_log_write();
@ -659,7 +661,6 @@ void NavEKF2::UpdateFilter(void)
float primaryErrorScore = core[primary].errorScore();
if (primaryErrorScore > 1.0f || !core[primary].healthy()) {
float lowestErrorScore = primaryErrorScore; // lowest error score from all lanes
bool has_switched = false; // true if a switch has occurred this frame
uint8_t newPrimaryIndex = primary; // index for new primary
for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {
@ -685,11 +686,10 @@ void NavEKF2::UpdateFilter(void)
}
// update the yaw and position reset data to capture changes due to the lane switch
if (newPrimaryIndex != primary) {
updateLaneSwitchYawResetData(has_switched, newPrimaryIndex, primary);
updateLaneSwitchPosResetData(has_switched, newPrimaryIndex, primary);
updateLaneSwitchYawResetData(newPrimaryIndex, primary);
updateLaneSwitchPosResetData(newPrimaryIndex, primary);
updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
primary = newPrimaryIndex;
has_switched = true;
}
}
@ -1189,17 +1189,17 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAngDelta)
yaw_reset_data.last_function_call = imuSampleTime_us / 1000;
yawAngDelta = 0;
uint32_t lastYawReset_ms = 0;
float temp_yawAng;
uint32_t lastCoreYawReset_ms = core[primary].getLastYawResetAngle(temp_yawAng);
// If core has changed (and data not consumed yet) or if the core change was the last yaw reset, return its data
if (yaw_reset_data.core_changed || lastCoreYawReset_ms <= yaw_reset_data.last_primary_change) {
// There has been a change notification in the primary core that the controller has not consumed
if (yaw_reset_data.core_changed) {
yawAngDelta = yaw_reset_data.core_delta;
lastYawReset_ms = yaw_reset_data.last_primary_change;
yaw_reset_data.core_changed = false;
}
// If current core yaw reset event was the last one, add it to the delta
// There has been a reset inside the core since we switched
float temp_yawAng;
uint32_t lastCoreYawReset_ms = core[primary].getLastYawResetAngle(temp_yawAng);
if (lastCoreYawReset_ms > lastYawReset_ms) {
yawAngDelta = wrap_PI(yawAngDelta + temp_yawAng);
lastYawReset_ms = lastCoreYawReset_ms;
@ -1220,17 +1220,17 @@ uint32_t NavEKF2::getLastPosNorthEastReset(Vector2f &posDelta)
pos_reset_data.last_function_call = imuSampleTime_us / 1000;
posDelta.zero();
uint32_t lastPosReset_ms = 0;
Vector2f tempPosDelta;
uint32_t lastCorePosReset_ms = core[primary].getLastPosNorthEastReset(tempPosDelta);
// If core has changed (and data not consumed yet) or if the core change was the last position reset, return its data
if (pos_reset_data.core_changed || lastCorePosReset_ms <= pos_reset_data.last_primary_change) {
// There has been a change in the primary core that the controller has not consumed
if (pos_reset_data.core_changed) {
posDelta = pos_reset_data.core_delta;
lastPosReset_ms = pos_reset_data.last_primary_change;
pos_reset_data.core_changed = false;
}
// If current core position reset event was the last one, add it to the delta
// There has been a reset inside the core since we switched
Vector2f tempPosDelta;
uint32_t lastCorePosReset_ms = core[primary].getLastPosNorthEastReset(tempPosDelta);
if (lastCorePosReset_ms > lastPosReset_ms) {
posDelta = posDelta + tempPosDelta;
lastPosReset_ms = lastCorePosReset_ms;
@ -1316,7 +1316,7 @@ void NavEKF2::updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_prim
}
// update the position reset data to capture changes due to a lane switch
void NavEKF2::updateLaneSwitchPosResetData(bool has_switched, uint8_t new_primary, uint8_t old_primary)
void NavEKF2::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_primary)
{
Vector2f pos_old_primary, pos_new_primary, old_pos_delta;
@ -1326,18 +1326,18 @@ void NavEKF2::updateLaneSwitchPosResetData(bool has_switched, uint8_t new_primar
}
// If current primary has reset position after controller got it, add it to the delta
// Prevent adding the delta if we have already changed primary in this filter update
if (!has_switched && core[old_primary].getLastPosNorthEastReset(old_pos_delta) > pos_reset_data.last_function_call) {
if (core[old_primary].getLastPosNorthEastReset(old_pos_delta) > pos_reset_data.last_function_call) {
pos_reset_data.core_delta += old_pos_delta;
}
// Record the position delta between current core and new primary core and the timestamp of the core change
// Add current delta in case it hasn't been consumed yet
core[old_primary].getLastPosNorthEastReset(pos_old_primary);
core[new_primary].getLastPosNorthEastReset(pos_new_primary);
core[old_primary].getPosNE(pos_old_primary);
core[new_primary].getPosNE(pos_new_primary);
pos_reset_data.core_delta = pos_new_primary - pos_old_primary + pos_reset_data.core_delta;
pos_reset_data.last_primary_change = imuSampleTime_us / 1000;
pos_reset_data.core_changed = true;
}
// Update the vertical position reset data to capture changes due to a core switch

6
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -406,16 +406,14 @@ private:
} pos_down_reset_data;
// update the yaw reset data to capture changes due to a lane switch
// has_switched - true if the primary instance has already been changed during this filter update cycle
// new_primary - index of the ekf instance that we are about to switch to as the primary
// old_primary - index of the ekf instance that we are currently using as the primary
void updateLaneSwitchYawResetData(bool has_switched, uint8_t new_primary, uint8_t old_primary);
void updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_primary);
// update the position reset data to capture changes due to a lane switch
// has_switched - true if the primary instance has already been changed during this filter update cycle
// new_primary - index of the ekf instance that we are about to switch to as the primary
// old_primary - index of the ekf instance that we are currently using as the primary
void updateLaneSwitchPosResetData(bool has_switched, uint8_t new_primary, uint8_t old_primary);
void updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_primary);
// update the position reset data to capture changes due to a lane switch
// new_primary - index of the ekf instance that we are about to switch to as the primary