From 8da22e441bf6fff701de952c1c54507e3fdf149f Mon Sep 17 00:00:00 2001 From: Francisco Ferreira Date: Tue, 20 Sep 2016 18:59:38 +0100 Subject: [PATCH] Revert 3 commits about yaw reset core switch Revert "AP_NavEKF2: Fix bug in published yaw reset value found during code review" commit 175faf1e4184f9000aaaf77f56ae0877ab2ffefe. Revert "AP_NavEKF2: use a struct for all yaw step class variables" commit 77fad065d15ea86cb309a6824c662f952070b5c7. Partially revert "AP_NavEKF2: Handle yaw jumps due to core switches" commit 885bfd1b4e8fb5136af3880dadb731081d294cd0. --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 47 +---------------------------- libraries/AP_NavEKF2/AP_NavEKF2.h | 10 ------ 2 files changed, 1 insertion(+), 56 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index e6072009b1..42b5a74a9e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index af502ab004..7f15322d7e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -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; };