From e83b598040e66eb57c1880b10ff522275b86c1dc Mon Sep 17 00:00:00 2001
From: Peter Hall <iampete@hotmail.co.uk>
Date: Thu, 23 Sep 2021 23:55:59 +0100
Subject: [PATCH] Plane: Quadplane: check attitude relax before running
 controllers, don't run controls if not going to output

---
 ArduPlane/mode_qacro.cpp   |  2 --
 ArduPlane/mode_qloiter.cpp |  2 --
 ArduPlane/quadplane.cpp    | 40 +++++++++++---------------------------
 ArduPlane/quadplane.h      |  1 -
 4 files changed, 11 insertions(+), 34 deletions(-)

diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp
index 32cc739e13..c8eecfad3a 100644
--- a/ArduPlane/mode_qacro.cpp
+++ b/ArduPlane/mode_qacro.cpp
@@ -31,8 +31,6 @@ void ModeQAcro::run()
         attitude_control->set_throttle_out(0, true, 0);
         quadplane.relax_attitude_control();
     } else {
-        quadplane.check_attitude_relax();
-
         quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
         // convert the input to the desired body frame rate
diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp
index 71a08e52e2..c9e8273753 100644
--- a/ArduPlane/mode_qloiter.cpp
+++ b/ArduPlane/mode_qloiter.cpp
@@ -44,8 +44,6 @@ void ModeQLoiter::run()
         plane.mode_qloiter._enter();
     }
 
-    quadplane.check_attitude_relax();
-
     if (quadplane.should_relax()) {
         loiter_nav->soften_for_landing();
     }
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 43801aa6ea..8c39413b2f 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -755,8 +755,6 @@ void QuadPlane::run_esc_calibration(void)
  */
 void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
 {
-    check_attitude_relax();
-
     bool use_multicopter_control = in_vtol_mode() && !tailsitter.in_vtol_transition();
     bool use_multicopter_eulers = false;
 
@@ -889,19 +887,6 @@ void QuadPlane::relax_attitude_control()
     attitude_control->relax_attitude_controllers(tailsitter._is_vectored);
 }
 
-/*
-  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)
-{
-    if (AP_HAL::millis() - last_att_control_ms > 100) {
-        relax_attitude_control();
-    }
-}
-
 /*
   check for an EKF yaw reset
  */
@@ -1587,7 +1572,6 @@ void QuadPlane::update_transition(void)
         // multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees
         plane.nav_pitch_cd = constrain_float(transition_initial_pitch - (tailsitter.transition_rate_fw * dt) * 0.1f * (plane.fly_inverted()?-1.0f:1.0f), -8500, 8500);
         plane.nav_roll_cd = 0;
-        check_attitude_relax();
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
                                                                       plane.nav_pitch_cd,
                                                                       0);
@@ -1859,12 +1843,6 @@ void QuadPlane::update_throttle_hover()
  */
 void QuadPlane::motors_output(bool run_rate_controller)
 {
-    const uint32_t now = AP_HAL::millis();
-    if (run_rate_controller) {
-        attitude_control->rate_controller_run();
-        last_att_control_ms = now;
-    }
-
     /* Delay for ARMING_DELAY_MS after arming before allowing props to spin:
        1) for safety (OPTION_DELAY_ARMING)
        2) to allow motors to return to vertical (OPTION_DISARMED_TILT)
@@ -1903,9 +1881,19 @@ void QuadPlane::motors_output(bool run_rate_controller)
         return;
     }
 
+    const uint32_t now = AP_HAL::millis();
+    if (run_rate_controller) {
+        if (now - last_att_control_ms > 100) {
+            // relax if have been inactive
+            relax_attitude_control();
+        }
+        attitude_control->rate_controller_run();
+        last_att_control_ms = now;
+    }
+
     // see if motors should be shut down
     update_throttle_suppression();
-    
+
     motors->output();
 
     // remember when motors were last active for throttle suppression
@@ -2151,8 +2139,6 @@ void QuadPlane::vtol_position_controller(void)
     // target speed when we reach position2 threshold
     const float position2_target_speed = 2.0;
 
-    check_attitude_relax();
-
     // horizontal position control
     switch (poscontrol.get_state()) {
 
@@ -2634,8 +2620,6 @@ void QuadPlane::takeoff_controller(void)
     /*
       for takeoff we use the position controller
     */
-    check_attitude_relax();
-
     setup_target_position();
 
     // set position controller desired velocity and acceleration to zero
@@ -2685,8 +2669,6 @@ 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 806f1b202d..eb5a9426e6 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -233,7 +233,6 @@ private:
     // use multicopter rate controller
     void multicopter_attitude_rate_update(float yaw_rate_cds);
 
-    void check_attitude_relax(void);
     float get_pilot_throttle(void);
     void control_hover(void);
     void relax_attitude_control();