Plane: move AHRS reset check up from quadplane and reset fixedwing controllers

ArduPlane/Attitude.cpp

@@ -755,3 +755,55 @@ void Plane::apply_load_factor_roll_limits(void)
     nav_roll_cd = constrain_int32(nav_roll_cd, -lf_roll_limit_deg * 100, lf_roll_limit_deg * 100);
     roll_limit_cd = MIN(roll_limit_cd, lf_roll_limit_deg * 100);
 }
+
+// Check if there has been a change in attitude estimate which the attitude controllers should be told about
+// This allows them to compensate for the change so smooth control is maintained
+void Plane::check_ahrs_reset()
+{
+    bool should_reset = false;
+
+    // Check for change in ahrs type
+    const AP_AHRS::EKFType ahrs_type = ahrs.active_EKF_type();
+    if (ahrs_check.last_ahrs_type != ahrs_type) {
+        should_reset = true;
+    }
+    ahrs_check.last_ahrs_type = ahrs_type;
+
+    // Check for change in core
+    const int8_t primary_core = ahrs.get_primary_core_index();
+    if (ahrs_check.last_primary_core != primary_core) {
+        if (!should_reset) {
+            // Don't report to user if an EKF type change has already been reported
+            gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)primary_core);
+        }
+        should_reset = true;
+        LOGGER_WRITE_ERROR(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(primary_core));
+    }
+    ahrs_check.last_primary_core = primary_core;
+
+    // Check for yaw reset
+    float yaw_angle_change_rad;
+    const uint32_t yaw_reset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
+    if (ahrs_check.last_yaw_reset_ms != yaw_reset_ms) {
+        should_reset = true;
+        LOGGER_WRITE_EVENT(LogEvent::EKF_YAW_RESET);
+    }
+    ahrs_check.last_yaw_reset_ms = yaw_reset_ms;
+
+    // Nothing to do if there are no resets
+    if (!should_reset) {
+        return;
+    }
+
+    // Reset fixed wing controllers
+    rollController.ahrs_reset();
+    pitchController.ahrs_reset();
+
+#if HAL_QUADPLANE_ENABLED
+    // Reset vtol controllers
+    if (quadplane.initialised) {
+        quadplane.attitude_control->inertial_frame_reset();
+    }
+#endif
+
+}

ArduPlane/Plane.cpp

@@ -195,10 +195,10 @@ void Plane::ahrs_update()
     steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
     steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
 
-#if HAL_QUADPLANE_ENABLED
-    // check if we have had a yaw reset from the EKF
-    quadplane.check_yaw_reset();
+    // Check if there has been a change in attitude estimate which the attitude controllers should be told about
+    check_ahrs_reset();
 
+#if HAL_QUADPLANE_ENABLED
     // update inertial_nav for quadplane
     quadplane.inertial_nav.update();
     if (quadplane.available()) {  

ArduPlane/Plane.h

@@ -935,6 +935,14 @@ private:
     int16_t calc_nav_yaw_course(void);
     int16_t calc_nav_yaw_ground(void);
 
+    // Check if there has been a change in attitude estimate which the attitude controllers should be told about
+    void check_ahrs_reset();
+    struct {
+        uint32_t last_yaw_reset_ms;
+        int8_t last_primary_core;
+        AP_AHRS::EKFType last_ahrs_type;
+    } ahrs_check;
+
 #if HAL_LOGGING_ENABLED
 
     // methods for AP_Vehicle:

ArduPlane/quadplane.cpp

@@ -1072,23 +1072,6 @@ void QuadPlane::relax_attitude_control()
     attitude_control->relax_attitude_controllers(!tailsitter.relax_pitch());
 }
 
-/*
-  check for an EKF yaw reset
- */
-void QuadPlane::check_yaw_reset(void)
-{
-    if (!initialised) {
-        return;
-    }
-    float yaw_angle_change_rad = 0.0f;
-    uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
-    if (new_ekfYawReset_ms != ekfYawReset_ms) {
-        attitude_control->inertial_frame_reset();
-        ekfYawReset_ms = new_ekfYawReset_ms;
-        LOGGER_WRITE_EVENT(LogEvent::EKF_YAW_RESET);
-    }
-}
-
 void QuadPlane::set_climb_rate_ms(float target_climb_rate_ms)
 {
     float vel_d_m = -target_climb_rate_ms;

ArduPlane/quadplane.h

@@ -235,9 +235,6 @@ private:
     // return true if airmode should be active
     bool air_mode_active() const;
 
-    // check for an EKF yaw reset
-    void check_yaw_reset(void);
-    
     // hold hover (for transition)
     void hold_hover(float target_climb_rate_cms);
 
@@ -416,9 +413,6 @@ private:
     // return which vfwd method to use
     ActiveFwdThr get_vfwd_method(void) const;
 
-    // time we last got an EKF yaw reset
-    uint32_t ekfYawReset_ms;
-
     struct {
         AP_Float gain;
         float integrator;