Plane: added fast attitude recovery in Q modes

when we start the VTOL motor stabilisation with an attitude beyond
normal attitude limits we set a flag to use fixed wing attitude
control (slaving VTOL rate control to fixed wing rate control) until
we have recovered

this avoids an issue with the shaping in the VTOL attitude controller
and also fixes an issue with the VTOL controller bringing the nose
down, which can cause a lot of height loss in quadplanes

Author: tridge

ArduPlane/quadplane.cpp

@@ -875,7 +875,7 @@ void QuadPlane::run_esc_calibration(void)
  */
 void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
 {
-    bool use_multicopter_control = in_vtol_mode() && !tailsitter.in_vtol_transition();
+    bool use_multicopter_control = in_vtol_mode() && !tailsitter.in_vtol_transition() && !force_fw_control_recovery;
     bool use_yaw_target = false;
 
     float yaw_target_cd = 0.0;
@@ -1937,7 +1937,38 @@ void QuadPlane::motors_output(bool run_rate_controller)
         if (now - last_att_control_ms > 100) {
             // relax if have been inactive
             relax_attitude_control();
+            /*
+              when starting the VTOL motors force use of the fixed
+              wing controller for attitude control if we are outside
+              of the Q_ANGLE_MAX limit. We keep it forced until we
+              have recovered to within the limit
+
+              This avoids an issue with the attitude controller
+              limiting the rate of recovery and also an issue with the
+              quaternion controller trying to fix the attitude in a
+              way that is bad for fixed wing aircraft (eg. putting
+              nose down when inverted, leading to rapid loss of
+              height)
+
+              we don't do this in QACRO or tailsitter modes, as this
+              type of attitude is expected
+            */
+            if (!tailsitter.enabled() &&
+                plane.control_mode != &plane.mode_qacro &&
+                (abs(ahrs_view->roll_sensor) > aparm.angle_max ||
+                 abs(ahrs_view->pitch_sensor) > aparm.angle_max)) {
+                force_fw_control_recovery = true;
+            }
         }
+        if (force_fw_control_recovery) {
+            // see if we have recovered attitude
+            if (abs(ahrs_view->roll_sensor) <= aparm.angle_max &&
+                abs(ahrs_view->pitch_sensor) <= aparm.angle_max) {
+                force_fw_control_recovery = false;
+                relax_attitude_control();
+            }
+        }
+
         // run low level rate controllers that only require IMU data and set loop time
         const float last_loop_time_s = AP::scheduler().get_last_loop_time_s();
         motors->set_dt(last_loop_time_s);
@@ -4192,7 +4223,8 @@ bool QuadPlane::use_fw_attitude_controllers(void) const
         motors->get_desired_spool_state() >= AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED &&
         in_vtol_mode() &&
         !tailsitter.enabled() &&
-        poscontrol.get_state() != QPOS_AIRBRAKE) {
+        poscontrol.get_state() != QPOS_AIRBRAKE &&
+        !force_fw_control_recovery) {
         // we want the desired rates for fixed wing slaved to the
         // multicopter rates
         return false;

ArduPlane/quadplane.h

@@ -632,6 +632,9 @@ class QuadPlane
     // ignored unless OPTION_DELAY_ARMING or OPTION_TILT_DISARMED is set
     bool delay_arming;
 
+    // should we force use of fixed wing controller for attitude upset recovery?
+    bool force_fw_control_recovery;
+
     /*
       return true if current mission item is a vtol takeoff
      */