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

AP_NavEKF2: Report position jumps due to lane switches

Browse Source 
Also moves code required to update reset data due to lane switches into separate functionscto improve readability.
This commit is contained in:
priseborough 2016-10-10 08:18:54 +11:00 committed by Randy Mackay
parent 337461c16c
commit ab55991b33
2 changed files with 101 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -657,35 +657,16 @@ void NavEKF2::UpdateFilter(void)
// If the current core selected has a bad fault score or is unhealthy, switch to a healthy core with the lowest fault score
if (core[primary].faultScore() > 0.0f || !core[primary].healthy()) {
float score = 1e9f;
bool changed = false;
bool has_switched = false; // true if a switch has occurred this frame
for (uint8_t i=0; i<num_cores; i++) {
if (core[i].healthy()) {
float tempScore = core[i].faultScore();
if (tempScore < score) {
Vector3f eulers_old_primary, eulers_new_primary;
float old_yaw_delta;
// update the yaw and position reset data to capture changes due to the lane switch
updateLaneSwitchYawResetData(has_switched, i, primary);
updateLaneSwitchPosResetData(has_switched, i, primary);
// If core yaw reset data has been consumed reset delta to zero
if (!yaw_reset_data.core_changed) {
yaw_reset_data.core_delta = 0;
}
// If current primary has reset yaw after controller got it, add it to the delta
// Prevent adding the delta if we have already changed primary in this filter update
if (!changed && core[primary].getLastYawResetAngle(old_yaw_delta) > yaw_reset_data.last_function_call) {
yaw_reset_data.core_delta += old_yaw_delta;
}
core[primary].getEulerAngles(eulers_old_primary);
core[i].getEulerAngles(eulers_new_primary);
// Record the yaw 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
yaw_reset_data.core_delta = wrap_PI(eulers_new_primary.z - eulers_old_primary.z + yaw_reset_data.core_delta);
yaw_reset_data.last_primary_change = imuSampleTime_us / 1000;
yaw_reset_data.core_changed = true;
changed = true;
has_switched = true;
primary = i;
score = tempScore;
}
@ -1210,12 +1191,33 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAngDelta)
// return the amount of NE position change due to the last position reset in metres
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t NavEKF2::getLastPosNorthEastReset(Vector2f &pos) const
uint32_t NavEKF2::getLastPosNorthEastReset(Vector2f &posDelta)
{
if (!core) {
return 0;
}
return core[primary].getLastPosNorthEastReset(pos);
// Record last time controller got the position reset
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) {
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
if (lastCorePosReset_ms > lastPosReset_ms) {
posDelta = posDelta + tempPosDelta;
lastPosReset_ms = lastCorePosReset_ms;
}
return lastPosReset_ms;
}
// return the amount of NE velocity change due to the last velocity reset in metres/sec
@ -1237,4 +1239,56 @@ const char *NavEKF2::prearm_failure_reason(void) const
return core[primary].prearm_failure_reason();
}
// update the yaw reset data to capture changes due to a lane switch
void NavEKF2::updateLaneSwitchYawResetData(bool has_switched, uint8_t new_primary, uint8_t old_primary)
{
Vector3f eulers_old_primary, eulers_new_primary;
float old_yaw_delta;
// If core yaw reset data has been consumed reset delta to zero
if (!yaw_reset_data.core_changed) {
yaw_reset_data.core_delta = 0;
}
// If current primary has reset yaw 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].getLastYawResetAngle(old_yaw_delta) > yaw_reset_data.last_function_call) {
yaw_reset_data.core_delta += old_yaw_delta;
}
// Record the yaw 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].getEulerAngles(eulers_old_primary);
core[new_primary].getEulerAngles(eulers_new_primary);
yaw_reset_data.core_delta = wrap_PI(eulers_new_primary.z - eulers_old_primary.z + yaw_reset_data.core_delta);
yaw_reset_data.last_primary_change = imuSampleTime_us / 1000;
yaw_reset_data.core_changed = true;
}
// 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)
{
Vector2f pos_old_primary, pos_new_primary, old_pos_delta;
// If core position reset data has been consumed reset delta to zero
if (!pos_reset_data.core_changed) {
pos_reset_data.core_delta.zero();
}
// 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) {
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);
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;
}
#endif //HAL_CPU_CLASS

23
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -274,7 +274,7 @@ public:
// return the amount of NE position change due to the last position reset in metres
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosNorthEastReset(Vector2f &pos) const;
uint32_t getLastPosNorthEastReset(Vector2f &posDelta);
// return the amount of NE velocity change due to the last velocity reset in metres/sec
// returns the time of the last reset or 0 if no reset has ever occurred
@ -385,6 +385,25 @@ private:
uint32_t last_function_call; // last time getLastYawYawResetAngle was called
bool core_changed; // true when a core change happened and hasn't been consumed, false otherwise
uint32_t last_primary_change; // last time a primary has changed
float core_delta; // the ammount of yaw change between cores when a change happened
float core_delta; // the amount of yaw change between cores when a change happened
} yaw_reset_data;
struct {
uint32_t last_function_call; // last time getLastPosNorthEastReset was called
bool core_changed; // true when a core change happened and hasn't been consumed, false otherwise
uint32_t last_primary_change; // last time a primary has changed
Vector2f core_delta; // the amount of NE position change between cores when a change happened
} pos_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);
// 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);
};