Plane: fast attitude recovery for quadplanes

ArduPlane/Attitude.cpp

@@ -378,6 +378,10 @@ void Plane::stabilize_yaw()
         SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_output);
     }
 
+#if HAL_QUADPLANE_ENABLED
+    // possibly recover from a spin
+    quadplane.assist.output_spin_recovery();
+#endif
 }
 
 /*

ArduPlane/VTOL_Assist.cpp

@@ -140,4 +140,94 @@ bool VTOL_Assist::should_assist(float aspeed, bool have_airspeed)
     return force_assist || speed_assist || alt_error.is_active() || angle_error.is_active();
 }
 
+/*
+  check if we are in VTOL recovery
+*/
+bool VTOL_Assist::check_VTOL_recovery(void)
+{
+    const bool allow_fw_recovery =
+        !option_is_set(OPTION::FW_FORCE_DISABLED) &&
+        !quadplane.tailsitter.enabled() &&
+        plane.control_mode != &plane.mode_qacro;
+    if (!allow_fw_recovery) {
+        quadplane.force_fw_control_recovery = false;
+        quadplane.in_spin_recovery = false;
+        return false;
+    }
+
+    // see if the attitude is outside twice the Q_ANGLE_MAX
+    const auto &ahrs = plane.ahrs;
+    const int16_t angle_max_cd = quadplane.aparm.angle_max;
+    const float abs_angle_cd = fabsf(Vector2f{float(ahrs.roll_sensor), float(ahrs.pitch_sensor)}.length());
+
+    if (abs_angle_cd > 2*angle_max_cd) {
+        // we are 2x the angle limits, trigger fw recovery
+        quadplane.force_fw_control_recovery = true;
+    }
+
+    if (quadplane.force_fw_control_recovery) {
+        // stop fixed wing recovery if inside Q_ANGLE_MAX
+        if (abs_angle_cd <= angle_max_cd) {
+            quadplane.force_fw_control_recovery = false;
+            quadplane.attitude_control->reset_target_and_rate(false);
+
+            if (ahrs.groundspeed() > quadplane.wp_nav->get_default_speed_xy()*0.01) {
+                /* if moving at high speed also reset position
+                   controller and height controller
+
+                   this avoids an issue where the position
+                   controller may limit pitch after a strong
+                   acceleration event
+                */
+                quadplane.pos_control->init_z_controller();
+                quadplane.pos_control->init_xy_controller();
+            }
+        }
+    }
+
+    if (!option_is_set(OPTION::SPIN_DISABLED) &&
+        quadplane.force_fw_control_recovery) {
+        // additionally check for needing spin recovery
+        const auto &gyro = plane.ahrs.get_gyro();
+        quadplane.in_spin_recovery =
+            fabsf(gyro.z) > radians(10) &&
+            fabsf(gyro.x) > radians(30) &&
+            fabsf(gyro.y) > radians(30) &&
+            gyro.x * gyro.z < 0 &&
+            plane.ahrs.pitch_sensor < -4500;
+    } else {
+        quadplane.in_spin_recovery = false;
+    }
+
+    return quadplane.force_fw_control_recovery;
+}
+
+
+/* if we are in a spin then counter with rudder and elevator
+
+   if roll rate and yaw rate are opposite and yaw rate is
+   significant then put in full rudder to counter the yaw rate
+   for spin recovery
+*/
+void VTOL_Assist::output_spin_recovery(void)
+{
+    if (!quadplane.in_spin_recovery) {
+        return;
+    }
+    if (quadplane.motors->get_desired_spool_state() !=
+        AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
+        // if we and no longer running the VTOL motors we need to
+        // clear the spin flag
+        quadplane.in_spin_recovery = false;
+        return;
+    }
+    const Vector3f &gyro = plane.ahrs.get_gyro();
+
+    // put in opposite rudder to counter yaw, and neutral
+    // elevator until we're out of the spin
+    SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, gyro.z > 0 ? -SERVO_MAX : SERVO_MAX);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 0);
+}
+
+
 #endif  // HAL_QUADPLANE_ENABLED

ArduPlane/VTOL_Assist.h

@@ -25,6 +25,19 @@ class VTOL_Assist {
     // Time hysteresis for triggering of assistance
     AP_Float delay;
 
+    // special options
+    AP_Int16 options;
+
+    // assist options
+    enum class OPTION {
+        FW_FORCE_DISABLED=(1U<<0),
+        SPIN_DISABLED=(1U<<1),
+    };
+
+    bool option_is_set(OPTION option) const {
+        return (options.get() & int32_t(option)) != 0;
+    }
+
     // State from pilot
     enum class STATE {
         ASSIST_DISABLED,
@@ -39,6 +52,12 @@ class VTOL_Assist {
     bool in_alt_assist() const { return alt_error.is_active(); }
     bool in_angle_assist() const { return angle_error.is_active(); }
 
+    // check if we are in VTOL recovery
+    bool check_VTOL_recovery(void);
+
+    // output rudder and elevator for spin recovery
+    void output_spin_recovery(void);
+
 private:
 
     // Default to enabled

ArduPlane/mode.cpp

@@ -156,6 +156,12 @@ bool Mode::enter()
             bool have_airspeed = quadplane.ahrs.airspeed_estimate(aspeed);
             quadplane.assisted_flight = quadplane.assist.should_assist(aspeed, have_airspeed);
         }
+
+        if (is_vtol_mode() && !quadplane.tailsitter.enabled()) {
+            // if flying inverted and entering a VTOL mode cancel
+            // inverted flight
+            plane.inverted_flight = false;
+        }
 #endif
     }
 

ArduPlane/mode_qhover.cpp

@@ -24,6 +24,8 @@ void ModeQHover::update()
  */
 void ModeQHover::run()
 {
+    quadplane.assist.check_VTOL_recovery();
+
     const uint32_t now = AP_HAL::millis();
     if (quadplane.tailsitter.in_vtol_transition(now)) {
         // Tailsitters in FW pull up phase of VTOL transition run FW controllers
@@ -47,6 +49,9 @@ void ModeQHover::run()
 
     // Center rudder
     output_rudder_and_steering(0.0);
+
+    // possibly apply spin recovery
+    quadplane.assist.output_spin_recovery();
 }
 
 #endif

ArduPlane/mode_qloiter.cpp

@@ -32,6 +32,13 @@ void ModeQLoiter::update()
 // run quadplane loiter controller
 void ModeQLoiter::run()
 {
+    if (quadplane.assist.check_VTOL_recovery()) {
+        // use QHover to recover from extreme attitudes, this allows
+        // for the fixed wing controller to handle the recovery
+        plane.mode_qhover.run();
+        return;
+    }
+
     const uint32_t now = AP_HAL::millis();
 
 #if AC_PRECLAND_ENABLED

ArduPlane/quadplane.cpp

@@ -238,7 +238,15 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("ASSIST_ANGLE", 45, QuadPlane, assist.angle, 30),
 
-    // 47: TILT_TYPE
+    // @Param: ASSIST_OPTIONS
+    // @DisplayName: Quadplane assistance options
+    // @Description: Options for special QAssist features
+    // @Bitmask: 0: Disable force fixed wing controller recovery
+    // @Bitmask: 1: Disable quadplane spin recovery
+    // @User: Standard
+    AP_GROUPINFO("ASSIST_OPTIONS", 47, QuadPlane, assist.options, 0),
+
+    // 47: TILT_TYPE // was AP_Int8, re-used by AP_Int16 ASSIST_OPTIONS
     // 48: TAILSIT_ANGLE
     // 61: TAILSIT_ANG_VT
     // 49: TILT_RATE_DN
@@ -875,11 +883,12 @@ 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;
-    if (!use_multicopter_control && transition->update_yaw_target(yaw_target_cd)) {
+    if (!use_multicopter_control && transition->update_yaw_target(yaw_target_cd) &&
+        !force_fw_control_recovery) {
         use_multicopter_control = true;
         use_yaw_target = true;
     }
@@ -968,7 +977,7 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
         // disable yaw time constant for 1:1 match of desired rates
         disable_yaw_rate_time_constant();
 
-        attitude_control->input_rate_bf_roll_pitch_yaw_2(bf_input_cd.x, bf_input_cd.y, bf_input_cd.z);
+        attitude_control->input_rate_bf_roll_pitch_yaw_no_shaping(bf_input_cd.x, bf_input_cd.y, bf_input_cd.z);
     }
 }
 
@@ -1955,6 +1964,10 @@ void QuadPlane::motors_output(bool run_rate_controller)
             // relax if have been inactive
             relax_attitude_control();
         }
+
+        // see if we need to be in VTOL recovery
+        assist.check_VTOL_recovery();
+
         // 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);
@@ -3614,6 +3627,8 @@ void QuadPlane::Log_Write_QControl_Tuning()
         speed              = 1U<<2, // true if assistance due to low airspeed
         alt                = 1U<<3, // true if assistance due to low altitude
         angle              = 1U<<4, // true if assistance due to attitude error
+        fw_force           = 1U<<5, // true if forcing use of fixed wing controllers
+        spin_recovery      = 1U<<6, // true if recovering from a spin
     };
 
     uint8_t assist_flags = 0;
@@ -3632,6 +3647,12 @@ void QuadPlane::Log_Write_QControl_Tuning()
     if (assist.in_angle_assist()) {
         assist_flags |= (uint8_t)log_assistance_flags::angle;
     }
+    if (force_fw_control_recovery) {
+        assist_flags |= (uint8_t)log_assistance_flags::fw_force;
+    }
+    if (in_spin_recovery) {
+        assist_flags |= (uint8_t)log_assistance_flags::spin_recovery;
+    }
 
     struct log_QControl_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_QTUN_MSG),
@@ -4177,7 +4198,7 @@ bool QuadPlane::in_vtol_airbrake(void) const
 // return true if we should show VTOL view
 bool QuadPlane::show_vtol_view() const
 {
-    return available() && transition->show_vtol_view();
+    return available() && transition->show_vtol_view() && !force_fw_control_recovery;
 }
 
 // return true if we should show VTOL view
@@ -4207,13 +4228,27 @@ bool QuadPlane::use_fw_attitude_controllers(void) const
     if (available() &&
         motors->armed() &&
         motors->get_desired_spool_state() >= AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED &&
-        in_vtol_mode() &&
         !tailsitter.enabled() &&
-        poscontrol.get_state() != QPOS_AIRBRAKE) {
-        // we want the desired rates for fixed wing slaved to the
-        // multicopter rates
-        return false;
+        poscontrol.get_state() != QPOS_AIRBRAKE &&
+        !force_fw_control_recovery) {
+
+        if (in_vtol_mode()) {
+            // in VTOL modes always slave fixed wing to VTOL rate control
+            return false;
+        }
+
+        if (transition->use_multirotor_control_in_fwd_transition()) {
+            /*
+              special case for vectored yaw tiltrotors in forward
+              transition, keep multicopter control until we reach
+              target transition airspeed. This can result in loss of
+              yaw control on some tilt-vectored airframes without
+              strong VTOL yaw control
+            */
+            return false;
+        }
     }
+
     return true;
 }
 
@@ -4597,6 +4632,9 @@ void QuadPlane::mode_enter(void)
 
     q_fwd_throttle = 0.0f;
     q_fwd_pitch_lim_cd = 100.0f * q_fwd_pitch_lim;
+
+    force_fw_control_recovery = false;
+    in_spin_recovery = false;
 }
 
 // Set attitude control yaw rate time constant to pilot input command model value

ArduPlane/quadplane.h

@@ -635,6 +635,12 @@ 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;
+
+    // are we in spin recovery?
+    bool in_spin_recovery;
+
     /*
       return true if current mission item is a vtol takeoff
      */

ArduPlane/tailsitter.cpp

@@ -902,6 +902,10 @@ void Tailsitter_Transition::VTOL_update()
             vtol_limit_start_ms = now;
             vtol_limit_initial_pitch = quadplane.ahrs_view->pitch_sensor;
         }
+
+        // clear inverted flight flag to make behaviour consistent
+        // with other quadplane types
+        plane.inverted_flight = false;
     } else {
         // Keep assistance reset while not checking
         quadplane.assist.reset();

ArduPlane/tiltrotor.cpp

@@ -742,10 +742,18 @@ void Tiltrotor::update_yaw_target(void)
     transition_yaw_set_ms = now;
 }
 
+
+/*
+  control use of multirotor rate control in forward transition
+ */
+bool Tiltrotor_Transition::use_multirotor_control_in_fwd_transition() const
+{
+    return tiltrotor.is_vectored() && transition_state <= TRANSITION_TIMER;
+}
+
 bool Tiltrotor_Transition::update_yaw_target(float& yaw_target_cd)
 {
-    if (!(tiltrotor.is_vectored() &&
-        transition_state <= TRANSITION_TIMER)) {
+    if (!use_multirotor_control_in_fwd_transition()) {
         return false;
     }
     tiltrotor.update_yaw_target();

ArduPlane/tiltrotor.h

@@ -143,6 +143,8 @@ friend class Tiltrotor;
 
     bool show_vtol_view() const override;
 
+    bool use_multirotor_control_in_fwd_transition() const override;
+
 private:
 
     Tiltrotor& tiltrotor;

ArduPlane/transition.h

@@ -58,6 +58,8 @@ class Transition
 
     virtual bool allow_stick_mixing() const { return true; }
 
+    virtual bool use_multirotor_control_in_fwd_transition() const { return false; }
+
 protected:
 
     // refences for convenience