From ef154cf3fa948de1bf795c82ee6d61c4ce2d2afe Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 14 Sep 2018 10:02:18 +1000
Subject: [PATCH] Plane: fixed relaxing of attitude controller on transition

this fixes an issue found by Leonard where the attitude controller
could have residual control left over from a previous transition when
engaging the VTOL attitude controller
---
 ArduPlane/quadplane.cpp | 28 +++++++++++++++++++++++++++-
 ArduPlane/quadplane.h   |  4 ++++
 2 files changed, 31 insertions(+), 1 deletion(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index aed66d1c73..20d02b5b70 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -693,6 +693,8 @@ void QuadPlane::init_stabilize(void)
  */
 void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
 {
+    check_attitude_relax();
+
     if (in_vtol_mode() || is_tailsitter()) {
         // use euler angle attitude control
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
@@ -769,6 +771,22 @@ void QuadPlane::run_z_controller(void)
     pos_control->update_z_controller();    
 }
 
+/*
+  check if we should relax the attitude controllers
+
+  We relax them whenever we will be using them after a period of
+  inactivity
+ */
+void QuadPlane::check_attitude_relax(void)
+{
+    uint32_t now = AP_HAL::millis();
+    if (now - last_att_control_ms > 500) {
+        attitude_control->relax_attitude_controllers();
+        attitude_control->reset_rate_controller_I_terms();
+    }
+    last_att_control_ms = now;
+}
+
 // init quadplane hover mode 
 void QuadPlane::init_hover(void)
 {
@@ -948,6 +966,7 @@ void QuadPlane::control_loiter()
         return;
     }
 
+    check_attitude_relax();
 
     if (should_relax()) {
         loiter_nav->soften_for_landing();
@@ -1342,7 +1361,8 @@ void QuadPlane::update_transition(void)
         uint32_t dt = AP_HAL::millis() - transition_start_ms;
         plane.nav_pitch_cd = constrain_float((-transition_rate * dt)*100, -8500, 0);
         plane.nav_roll_cd = 0;
-        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, 
+        check_attitude_relax();
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
                                                                       plane.nav_pitch_cd,
                                                                       0);
         attitude_control->set_throttle_out(motors->get_throttle_hover(), true, 0);
@@ -1746,6 +1766,8 @@ void QuadPlane::vtol_position_controller(void)
     float ekfGndSpdLimit, ekfNavVelGainScaler;    
     ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
 
+    check_attitude_relax();
+
     // horizontal position control
     switch (poscontrol.state) {
 
@@ -1971,6 +1993,8 @@ void QuadPlane::takeoff_controller(void)
     float ekfGndSpdLimit, ekfNavVelGainScaler;    
     ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
 
+    check_attitude_relax();
+
     setup_target_position();
 
     // set position controller desired velocity and acceleration to zero
@@ -2000,6 +2024,8 @@ void QuadPlane::waypoint_controller(void)
 {
     setup_target_position();
 
+    check_attitude_relax();
+
     /*
       this is full copter control of auto flight
     */
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 6cdd127eff..57737edc14 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -186,6 +186,7 @@ private:
     void init_stabilize(void);
     void control_stabilize(void);
 
+    void check_attitude_relax(void);
     void init_hover(void);
     void control_hover(void);
     void run_rate_controller(void);
@@ -305,6 +306,9 @@ private:
     // pitch when we enter loiter mode
     int32_t loiter_initial_pitch_cd;
 
+    // when did we last run the attitude controller?
+    uint32_t last_att_control_ms;
+
     // true if we have reached the airspeed threshold for transition
     enum {
         TRANSITION_AIRSPEED_WAIT,