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

AP_NavEKF2: Handle yaw jumps due to core switches

Browse Source 
Based on work from Paul Riseborough (priseborough)
This commit is contained in:
Francisco Ferreira 2016-09-20 19:23:50 +01:00 committed by Tom Pittenger
parent 8da22e441b
commit 42cd8e9721
2 changed files with 55 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -657,10 +657,35 @@ 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;
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;
// 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;
primary = i;
score = tempScore;
}
@ -1159,7 +1184,28 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAngDelta)
if (!core) {
return 0;
}
return core[primary].getLastYawResetAngle(yawAng);
// Record last time controller got the yaw reset
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) {
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
if (lastCoreYawReset_ms > lastYawReset_ms) {
yawAngDelta = wrap_PI(yawAngDelta + temp_yawAng);
lastYawReset_ms = lastCoreYawReset_ms;
}
return lastYawReset_ms;
}
// return the amount of NE position change due to the last position reset in metres

9
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -268,7 +268,7 @@ public:
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation
bool getHeightControlLimit(float &height) const;
// return the amount of yaw angle change due to the last yaw angle reset in radians
// return the amount of yaw angle change (in radians) due to the last yaw angle reset or core selection switch
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t getLastYawResetAngle(float &yawAngDelta);
@ -380,4 +380,11 @@ private:
// time at start of current filter update
uint64_t imuSampleTime_us;
struct {
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
} yaw_reset_data;
};