From 99cd9c8ade70e442a1affdd0f5b48cccbd4837de Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sun, 28 Jun 2026 12:07:45 +0100
Subject: [PATCH] Plane: move AHRS reset check up from quadplane and reset
 fixedwing controllers

---
 ArduPlane/Attitude.cpp  | 52 +++++++++++++++++++++++++++++++++++++++++
 ArduPlane/Plane.cpp     |  6 ++---
 ArduPlane/Plane.h       |  8 +++++++
 ArduPlane/quadplane.cpp | 17 --------------
 ArduPlane/quadplane.h   |  6 -----
 5 files changed, 63 insertions(+), 26 deletions(-)

diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index e8441290713..331eec81e14 100644
--- a/ArduPlane/Attitude.cpp
+++ b/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
+
+}
diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp
index 9f2941ded34..ca64bd2e589 100644
--- a/ArduPlane/Plane.cpp
+++ b/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()) {  
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 56447fd7499..dd17bc936dd 100644
--- a/ArduPlane/Plane.h
+++ b/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:
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index c770bcc497a..52130620416 100644
--- a/ArduPlane/quadplane.cpp
+++ b/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;
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index eb0601874b5..e3cee3c19bb 100644
--- a/ArduPlane/quadplane.h
+++ b/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;