From 77fad065d15ea86cb309a6824c662f952070b5c7 Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 26 Aug 2016 12:20:13 +1000 Subject: [PATCH] AP_NavEKF2: use a struct for all yaw step class variables --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 24 ++++++++++++------------ libraries/AP_NavEKF2/AP_NavEKF2.h | 16 +++++++++------- 2 files changed, 21 insertions(+), 19 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index d4dcfaf823..7ce29eba0e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -1155,15 +1155,15 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAngDelta) // 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 != 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 - 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 - yaw_reset_delta = temp_yaw_delta; + yaw_step_data.yaw_delta = temp_yaw_delta; // 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 Vector3f 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 // 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 - 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 // 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 - prev_yaw = eulers_primary.z; + yaw_step_data.prev_yaw = eulers_primary.z; // record the primary core - prev_instance = primary; + yaw_step_data.prev_instance = primary; // 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 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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 5ba4eb8a07..2ef4936901 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -380,11 +380,13 @@ private: // time at start of current filter update uint64_t imuSampleTime_us; - // used to keep track of yaw angle changes due to change of primary instance - uint8_t prev_instance = 0; // active core number from the previous time step - uint32_t last_ekf_reset_ms= 0; // last time the active ekf performed a yaw reset (msec) - uint32_t last_lane_switch_ms = 0; // last time there was a lane switch (msec) - uint32_t yaw_reset_time_ms = 0; // last time a yaw reset event was published - float yaw_reset_delta = 0.0f; // the amount of yaw change due to the last published yaw reset (rad) - float prev_yaw = 0.0f; // yaw angle published by the active core from the previous time step (rad) + // 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; };