Plane: add Qassist switch

ArduPlane/RC_Channel.cpp

@@ -58,6 +58,26 @@ void RC_Channel_Plane::do_aux_function_change_mode(const Mode::Number number,
     }
 }
 
+void RC_Channel_Plane::do_aux_function_q_assist_state(aux_switch_pos_t ch_flag)
+{
+    switch(ch_flag) {
+        case HIGH:
+            gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Force enabled");
+            plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE);
+            break;
+
+        case MIDDLE:
+            gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Enabled");
+            plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED);
+            break;
+
+        case LOW:
+            gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Disabled");
+            plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED);
+            break;
+    }
+}
+
 void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
                                          const RC_Channel::aux_switch_pos_t ch_flag)
 {
@@ -74,6 +94,10 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
     case AUX_FUNC::TAKEOFF:
         break;
 
+    case AUX_FUNC::Q_ASSIST:
+        do_aux_function(ch_option, ch_flag);
+        break;
+
     case AUX_FUNC::REVERSE_THROTTLE:
         plane.have_reverse_throttle_rc_option = true;
         // setup input throttle as a range. This is needed as init_aux_function is called
@@ -136,6 +160,10 @@ void RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const aux_swi
     case AUX_FUNC::FLAP:
         break; // flap input label, nothing to do
 
+    case AUX_FUNC::Q_ASSIST:
+        do_aux_function_q_assist_state(ch_flag);
+        break;
+
     default:
         RC_Channel::do_aux_function(ch_option, ch_flag);
         break;

ArduPlane/RC_Channel.h

@@ -17,6 +17,8 @@ private:
 
     void do_aux_function_change_mode(Mode::Number number,
                                      aux_switch_pos_t ch_flag);
+
+    void do_aux_function_q_assist_state(aux_switch_pos_t ch_flag);
 };
 
 class RC_Channels_Plane : public RC_Channels

ArduPlane/quadplane.cpp

@@ -1436,7 +1436,7 @@ float QuadPlane::desired_auto_yaw_rate_cds(void) const
 /*
   return true if the quadplane should provide stability assistance
  */
-bool QuadPlane::assistance_needed(float aspeed)
+bool QuadPlane::assistance_needed(float aspeed, bool have_airspeed)
 {
     if (assist_speed <= 0) {
         // assistance disabled
@@ -1445,7 +1445,7 @@ bool QuadPlane::assistance_needed(float aspeed)
         return false;
     }
 
-    if (aspeed < assist_speed) {
+    if (have_airspeed && aspeed < assist_speed) {
         // assistance due to Q_ASSIST_SPEED
         in_angle_assist = false;
         angle_error_start_ms = 0;
@@ -1559,26 +1559,28 @@ void QuadPlane::update_transition(void)
     if (is_tailsitter() && transition_state == TRANSITION_AIRSPEED_WAIT) {
         transition_state = TRANSITION_ANGLE_WAIT_FW;
     }
-    
+
     /*
       see if we should provide some assistance
      */
-    if (have_airspeed &&
+    bool q_assist_safe = hal.util->get_soft_armed() && ((plane.auto_throttle_mode && !plane.throttle_suppressed) ||
-        assistance_needed(aspeed) &&
+                                                         plane.get_throttle_input()>0 ||
-        !is_tailsitter() &&
+                                                         plane.is_flying());
-        hal.util->get_soft_armed() &&
+    if (q_assist_safe &&
-        ((plane.auto_throttle_mode && !plane.throttle_suppressed) ||
+        (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE ||
-         plane.get_throttle_input()>0 ||
+        (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed)))) {
-         plane.is_flying())) {
         // the quad should provide some assistance to the plane
-        if (transition_state != TRANSITION_AIRSPEED_WAIT) {
-            gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed);
-        }
-        transition_state = TRANSITION_AIRSPEED_WAIT;
-        if (transition_start_ms == 0) {
-            transition_start_ms = now;
-        }
         assisted_flight = true;
+        if (!is_tailsitter()) {
+            // update tansition state for vehicles using airspeed wait
+            if (transition_state != TRANSITION_AIRSPEED_WAIT) {
+                gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed);
+            }
+            transition_state = TRANSITION_AIRSPEED_WAIT;
+            if (transition_start_ms == 0) {
+                transition_start_ms = now;
+            }
+        }
     } else {
         assisted_flight = false;
     }

ArduPlane/quadplane.h

@@ -155,6 +155,13 @@ public:
 
     MAV_TYPE get_mav_type(void) const;
 
+    enum Q_ASSIST_STATE_ENUM {
+        Q_ASSIST_DISABLED,
+        Q_ASSIST_ENABLED,
+        Q_ASSIST_FORCE,
+    };
+    void set_q_assist_state(Q_ASSIST_STATE_ENUM state) {q_assist_state = state;};
+
 private:
     AP_AHRS_NavEKF &ahrs;
     AP_Vehicle::MultiCopter aparm;
@@ -179,7 +186,7 @@ private:
     AP_Int16 pilot_accel_z;
 
     // check for quadplane assistance needed
-    bool assistance_needed(float aspeed);
+    bool assistance_needed(float aspeed, bool have_airspeed);
 
     // update transition handling
     void update_transition(void);
@@ -579,7 +586,10 @@ private:
       are we in any of the phases of a VTOL landing?
      */
     bool in_vtol_land_sequence(void) const;
-    
+
+    // Q assist state, can be enabled, disabled or force. Default to enabled
+    Q_ASSIST_STATE_ENUM q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED;
+
 public:
     void motor_test_output();
     MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,