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

AP_NavEKF2: Report position jumps due to lane switches 

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);
};