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

Revert 3 commits about yaw reset core switch

Browse Source 
Revert "AP_NavEKF2: Fix bug in published yaw reset value found during code review"
commit 175faf1e41.

Revert "AP_NavEKF2: use a struct for all yaw step class variables"
commit 77fad065d1.

Partially revert "AP_NavEKF2: Handle yaw jumps due to core switches"
commit 885bfd1b4e.
This commit is contained in:
Francisco Ferreira 2016-09-20 18:59:38 +01:00 committed by Tom Pittenger
parent ff0d0b1aa5
commit 8da22e441b
2 changed files with 1 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -1159,52 +1159,7 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAngDelta)
if (!core) {
return 0;
}
// check for an internal ekf yaw reset
float temp_yaw_delta;
uint32_t ekf_reset_ms = core[primary].getLastYawResetAngle(temp_yaw_delta);
if (ekf_reset_ms != yaw_step_data.last_ekf_reset_ms) {
// record the time of the ekf's internal yaw reset event
yaw_step_data.last_ekf_reset_ms = ekf_reset_ms;
// record the the ekf's internal yaw reset value
yaw_step_data.yaw_delta = temp_yaw_delta;
// record the yaw reset event time
yaw_step_data.yaw_reset_time_ms = imuSampleTime_us/1000;
}
// check for a core switch and if a switch has occurred, set the yaw reset delta
// to the difference in yaw angle between the current and last yaw angle
Vector3f eulers_primary;
core[primary].getEulerAngles(eulers_primary);
if (primary != yaw_step_data.prev_instance) {
// the delta is the difference between the current and previous yaw
// This overwrites any yaw reset value recorded from an internal ekf reset
// that has occured on the same time-step
yaw_step_data.yaw_delta = wrap_PI(eulers_primary.z - yaw_step_data.prev_yaw);
// record the time of the yaw reset event
yaw_step_data.yaw_reset_time_ms = imuSampleTime_us/1000;
// update the time recorded for the last ekf internal yaw reset forthe primary core to
// prevent a yaw ekf reset event being published on the next frame due to the change in time
yaw_step_data.last_ekf_reset_ms = ekf_reset_ms;
}
// record the yaw angle from the primary core
yaw_step_data.prev_yaw = eulers_primary.z;
// record the primary core
yaw_step_data.prev_instance = primary;
// return the yaw delta from the last event
yawAngDelta = yaw_step_data.yaw_delta;
// return the time of the last event
return yaw_step_data.yaw_reset_time_ms;
return core[primary].getLastYawResetAngle(yawAng);
}
// return the amount of NE position change due to the last position reset in metres

10
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -380,14 +380,4 @@ private:
// time at start of current filter update
uint64_t imuSampleTime_us;
// used to keep track of yaw angle steps due to change of primary instance or internal ekf yaw resets
struct {
uint8_t prev_instance; // active core number from the previous time step
uint32_t last_ekf_reset_ms; // last time the active ekf performed a yaw reset (msec)
uint32_t last_lane_switch_ms; // last time there was a lane switch (msec)
uint32_t yaw_reset_time_ms; // last time a yaw reset event was published
float yaw_delta; // the amount of yaw change due to the last published yaw step (rad)
float prev_yaw; // yaw angle published by the active core from the previous time step (rad)
} yaw_step_data;
};