From 822c26f811a454109b831fd58a9744c26d955483 Mon Sep 17 00:00:00 2001
From: MatthewHampsey <matthewjohnhampsey@gmail.com>
Date: Thu, 15 Sep 2022 13:51:06 +1000
Subject: [PATCH] APM_Control: fixed yaw PID reset

---
 libraries/APM_Control/AP_YawController.cpp | 12 ++++++++++--
 libraries/APM_Control/AP_YawController.h   |  3 +++
 2 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp
index b49befbd1c..493d70636e 100644
--- a/libraries/APM_Control/AP_YawController.cpp
+++ b/libraries/APM_Control/AP_YawController.cpp
@@ -164,6 +164,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
     uint32_t dt = tnow - _last_t;
     if (_last_t == 0 || dt > 1000) {
         dt = 0;
+        _pid_info.I = 0;
     }
     _last_t = tnow;
 
@@ -343,8 +344,15 @@ float AP_YawController::get_rate_out(float desired_rate, float scaler, bool disa
 
 void AP_YawController::reset_I()
 {
-    _integrator = 0.0;
-    _pid_info.I = 0.0;
+    _pid_info.I = 0;
+    rate_pid.reset_I();
+    _integrator = 0;
+}
+
+void AP_YawController::reset_rate_PID()
+{
+    rate_pid.reset_I();
+    rate_pid.reset_filter();
 }
 
 /*
diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h
index 7c449e853e..667f1beec8 100644
--- a/libraries/APM_Control/AP_YawController.h
+++ b/libraries/APM_Control/AP_YawController.h
@@ -29,6 +29,8 @@ public:
 
     void reset_I();
 
+    void reset_rate_PID();
+
     /*
       reduce the integrator, used when we have a low scale factor in a quadplane hover
     */
@@ -46,6 +48,7 @@ public:
     // start/stop auto tuner
     void autotune_start(void);
     void autotune_restore(void);
+    
 
     static const struct AP_Param::GroupInfo var_info[];