From a727305a5954ea755a289b5e4a2adc755e3f0509 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 6 Jan 2018 17:16:50 +1030 Subject: [PATCH] Copter: integrate attitude control EKF inertial-frame-reset --- ArduCopter/ekf_check.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 43b007ccf0..c53e3ed9de 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -196,10 +196,10 @@ void Copter::failsafe_ekf_off_event(void) void Copter::check_ekf_reset() { // check for yaw reset - float yaw_angle_change_rad = 0.0f; + float yaw_angle_change_rad; uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad); if (new_ekfYawReset_ms != ekfYawReset_ms) { - attitude_control->shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f); + attitude_control->inertial_frame_reset(); ekfYawReset_ms = new_ekfYawReset_ms; Log_Write_Event(DATA_EKF_YAW_RESET); } @@ -207,6 +207,7 @@ void Copter::check_ekf_reset() #if AP_AHRS_NAVEKF_AVAILABLE // check for change in primary EKF (log only, AC_WPNav handles position target adjustment) if ((EKF2.getPrimaryCoreIndex() != ekf_primary_core) && (EKF2.getPrimaryCoreIndex() != -1)) { + attitude_control->inertial_frame_reset(); ekf_primary_core = EKF2.getPrimaryCoreIndex(); Log_Write_Error(ERROR_SUBSYSTEM_EKF_PRIMARY, ekf_primary_core); gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core);