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

AP_NavEKF2: use a struct for all yaw step class variables

Browse Source
This commit is contained in:
priseborough 2016-08-26 12:20:13 +10:00 committed by Randy Mackay
parent 885bfd1b4e
commit 77fad065d1
2 changed files with 21 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -1155,15 +1155,15 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAngDelta)
// check for an internal ekf yaw reset // check for an internal ekf yaw reset
float temp_yaw_delta; float temp_yaw_delta;
uint32_t ekf_reset_ms = core[primary].getLastYawResetAngle(temp_yaw_delta); uint32_t ekf_reset_ms = core[primary].getLastYawResetAngle(temp_yaw_delta);
if (ekf_reset_ms != last_ekf_reset_ms) { if (ekf_reset_ms != yaw_step_data.last_ekf_reset_ms) {
// record the time of the ekf's internal yaw reset event // record the time of the ekf's internal yaw reset event
last_ekf_reset_ms = ekf_reset_ms; yaw_step_data.last_ekf_reset_ms = ekf_reset_ms;
// record the the ekf's internal yaw reset value // record the the ekf's internal yaw reset value
yaw_reset_delta = temp_yaw_delta; yaw_step_data.yaw_delta = temp_yaw_delta;
// record the yaw reset event time // record the yaw reset event time
yaw_reset_time_ms = imuSampleTime_us/1000; yaw_step_data.yaw_reset_time_ms = imuSampleTime_us/1000;
} }
@ -1171,31 +1171,31 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAngDelta)
// to the difference in yaw angle between the current and last yaw angle // to the difference in yaw angle between the current and last yaw angle
Vector3f eulers_primary; Vector3f eulers_primary;
core[primary].getEulerAngles(eulers_primary); core[primary].getEulerAngles(eulers_primary);
if (primary != prev_instance) { if (primary != yaw_step_data.prev_instance) {
// the delta is the difference between the current and previous yaw // the delta is the difference between the current and previous yaw
// This overwrites any yaw reset value recorded from an internal eff reset // This overwrites any yaw reset value recorded from an internal eff reset
yaw_reset_delta += wrap_PI(eulers_primary.z - prev_yaw); yaw_step_data.yaw_delta += wrap_PI(eulers_primary.z - yaw_step_data.prev_yaw);
// record the time of the yaw reset event // record the time of the yaw reset event
yaw_reset_time_ms = imuSampleTime_us/1000; 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 // 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 // prevent a yaw ekf reset event being published on the next frame due to the change in time
last_ekf_reset_ms = ekf_reset_ms; yaw_step_data.last_ekf_reset_ms = ekf_reset_ms;
} }
// record the yaw angle from the primary core // record the yaw angle from the primary core
prev_yaw = eulers_primary.z; yaw_step_data.prev_yaw = eulers_primary.z;
// record the primary core // record the primary core
prev_instance = primary; yaw_step_data.prev_instance = primary;
// return the yaw delta from the last event // return the yaw delta from the last event
yawAngDelta = yaw_reset_delta; yawAngDelta = yaw_step_data.yaw_delta;
// return the time of the last event // return the time of the last event
return yaw_reset_time_ms; return yaw_step_data.yaw_reset_time_ms;
} }
// return the amount of NE position change due to the last position reset in metres // return the amount of NE position change due to the last position reset in metres

16
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -380,11 +380,13 @@ private:
// time at start of current filter update // time at start of current filter update
uint64_t imuSampleTime_us; uint64_t imuSampleTime_us;
// used to keep track of yaw angle changes due to change of primary instance // used to keep track of yaw angle steps due to change of primary instance or internal ekf yaw resets
uint8_t prev_instance = 0; // active core number from the previous time step struct {
uint32_t last_ekf_reset_ms= 0; // last time the active ekf performed a yaw reset (msec) uint8_t prev_instance; // active core number from the previous time step
uint32_t last_lane_switch_ms = 0; // last time there was a lane switch (msec) uint32_t last_ekf_reset_ms; // last time the active ekf performed a yaw reset (msec)
uint32_t yaw_reset_time_ms = 0; // last time a yaw reset event was published uint32_t last_lane_switch_ms; // last time there was a lane switch (msec)
float yaw_reset_delta = 0.0f; // the amount of yaw change due to the last published yaw reset (rad) uint32_t yaw_reset_time_ms; // last time a yaw reset event was published
float prev_yaw = 0.0f; // yaw angle published by the active core from the previous time step (rad) 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;
}; };