APM_Control: FixedWing: add ahrs reset method

Author: IamPete1

libraries/APM_Control/AP_FW_Controller.cpp

@@ -337,3 +337,10 @@ void AP_FW_Controller::get_input_shaping(float &angle, float &rate, float &accel
     rate = rate_target_deg;
     accel = accel_target_deg;
 }
+
+// Reset the attitude target to such that a change in attitude due to a ahrs change is smooth
+void AP_FW_Controller::ahrs_reset()
+{
+    // Update the target angle such that the angle error remains the same before and after the change in measured angle from the ahrs
+    angle_target_deg = wrap_180(get_measured_angle() + angle_err_deg);
+}

libraries/APM_Control/AP_FW_Controller.h

@@ -56,6 +56,9 @@ class AP_FW_Controller
     // Get the angle error in degrees
     float get_angle_error_deg() const { return angle_err_deg; }
 
+    // Reset the attitude target to such that a change in attitude due to a ahrs change is smooth
+    void ahrs_reset();
+
 protected:
     const AP_FixedWing &aparm;
     AP_AutoTune::ATGains gains;