Plane: add QACRO mode for quadplanes

use acro_r/p/y_rate params in qacro
port ACRO throttle handling from copter

handle non-tailsitter controls

ArduPlane/ArduPlane.cpp

@@ -679,7 +679,7 @@ void Plane::update_flight_mode(void)
 
         break;
     }
-        
+    case QACRO:
     case INITIALISING:
         // handled elsewhere
         break;
@@ -775,6 +775,7 @@ void Plane::update_navigation()
     case QLAND:
     case QRTL:
     case QAUTOTUNE:
+    case QACRO:
         // nothing to do
         break;
     }

ArduPlane/Attitude.cpp

@@ -155,6 +155,7 @@ void Plane::stabilize_stick_mixing_direct()
         control_mode == QLOITER ||
         control_mode == QLAND ||
         control_mode == QRTL ||
+        control_mode == QACRO ||
         control_mode == TRAINING ||
         control_mode == QAUTOTUNE) {
         return;
@@ -185,6 +186,7 @@ void Plane::stabilize_stick_mixing_fbw()
         control_mode == QLOITER ||
         control_mode == QLAND ||
         control_mode == QRTL ||
+        control_mode == QACRO ||
         control_mode == TRAINING ||
         control_mode == QAUTOTUNE ||
         (control_mode == AUTO && g.auto_fbw_steer == 42)) {
@@ -396,6 +398,7 @@ void Plane::stabilize()
                 control_mode == QLOITER ||
                 control_mode == QLAND ||
                 control_mode == QRTL ||
+                control_mode == QACRO ||
                 control_mode == QAUTOTUNE) &&
                !quadplane.in_tailsitter_vtol_transition()) {
         quadplane.control_run();

ArduPlane/GCS_Mavlink.cpp

@@ -23,6 +23,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
     case MANUAL:
     case TRAINING:
     case ACRO:
+    case QACRO:
         _base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
         break;
     case STABILIZE:

ArduPlane/GCS_Plane.cpp

@@ -77,6 +77,7 @@ void GCS_Plane::update_sensor_status_flags(void)
         break;
 
     case ACRO:
+    case QACRO:
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
         break;
 

ArduPlane/defines.h

@@ -66,7 +66,8 @@ enum FlightMode {
     QLOITER       = 19,
     QLAND         = 20,
     QRTL          = 21,
-    QAUTOTUNE	  = 22
+    QAUTOTUNE     = 22,
+    QACRO         = 23,
 };
 
 enum mode_reason_t {

ArduPlane/quadplane.cpp

@@ -807,6 +807,16 @@ void QuadPlane::check_attitude_relax(void)
     last_att_control_ms = now;
 }
 
+/*
+  init QACRO mode
+ */
+void QuadPlane::init_qacro(void)
+{
+    throttle_wait = false;
+    transition_state = TRANSITION_DONE;
+    attitude_control->relax_attitude_controllers();
+}
+
 // init quadplane hover mode 
 void QuadPlane::init_hover(void)
 {
@@ -858,6 +868,66 @@ void QuadPlane::hold_hover(float target_climb_rate)
     run_z_controller();
 }
 
+/*
+  control QACRO mode
+ */
+void QuadPlane::control_qacro(void)
+{
+    if (throttle_wait) {
+        motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
+        attitude_control->set_throttle_out_unstabilized(0, true, 0);
+    } else {
+        check_attitude_relax();
+
+        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+
+        // convert the input to the desired body frame rate
+        float target_roll = 0;
+        float target_pitch = plane.channel_pitch->norm_input() * plane.g.acro_pitch_rate * 100.0f;
+        float target_yaw = 0;
+        if (is_tailsitter()) {
+            // Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw
+            // acro_roll_rate param applies to yaw in copter frame
+            // no parameter for acro yaw rate; just use the normal one since the default is 100 deg/sec
+            target_roll =  plane.channel_rudder->norm_input() * yaw_rate_max * 100.0f;
+            target_yaw  = -plane.channel_roll->norm_input() * plane.g.acro_roll_rate * 100.0f;
+        } else {
+            target_roll = plane.channel_roll->norm_input() * plane.g.acro_roll_rate * 100.0f;
+            target_yaw  = plane.channel_rudder->norm_input() * yaw_rate_max * 100.0;
+        }
+
+        // get pilot's desired throttle
+        int16_t mid_stick = plane.channel_throttle->get_control_mid();
+        // protect against unlikely divide by zero
+        if (mid_stick <= 0) {
+            mid_stick = 50;
+        }
+        float thr_mid = motors->get_throttle_hover();
+        int16_t throttle_control = plane.channel_throttle->get_control_in();
+        float throttle_in;
+        if (throttle_control < mid_stick) {
+            // below the deadband
+            throttle_in = ((float)throttle_control) * 0.5f / (float)mid_stick;
+        } else if (throttle_control > mid_stick) {
+            // above the deadband
+            throttle_in = 0.5f + ((float)(throttle_control - mid_stick)) * 0.5f / (float)(100 - mid_stick);
+        } else {
+            // must be in the deadband
+            throttle_in = 0.5f;
+        }
+
+        float expo = constrain_float(-(thr_mid - 0.5) / 0.375, -0.5f, 1.0f);
+        // calculate the output throttle using the given expo function
+        float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
+
+        // run attitude controller
+        attitude_control->input_rate_bf_roll_pitch_yaw_3(target_roll, target_pitch, target_yaw);
+
+        // output pilot's throttle without angle boost
+        attitude_control->set_throttle_out(throttle_out, false, 10.0f);
+    }
+}
+
 /*
   control QHOVER mode
  */
@@ -954,6 +1024,9 @@ bool QuadPlane::is_flying_vtol(void) const
         // if we are demanding more than 1% throttle then don't consider aircraft landed
         return true;
     }
+    if (plane.control_mode == QACRO) {
+        return true;
+    }
     if (plane.control_mode == GUIDED && guided_takeoff) {
         return true;
     }
@@ -1703,6 +1776,9 @@ void QuadPlane::control_run(void)
     }
 
     switch (plane.control_mode) {
+    case QACRO:
+        control_qacro();
+        break;
     case QSTABILIZE:
         control_stabilize();
         break;
@@ -1768,6 +1844,9 @@ bool QuadPlane::init_mode(void)
     case QAUTOTUNE:
         return qautotune.init();
 #endif
+    case QACRO:
+        init_qacro();
+        break;
     default:
         break;
     }
@@ -1857,6 +1936,7 @@ bool QuadPlane::in_vtol_mode(void) const
             plane.control_mode == QLOITER ||
             plane.control_mode == QLAND ||
             plane.control_mode == QRTL ||
+            plane.control_mode == QACRO ||
             plane.control_mode == QAUTOTUNE ||
             ((plane.control_mode == GUIDED || plane.control_mode == AVOID_ADSB) && plane.auto_state.vtol_loiter) ||
             in_vtol_auto());

ArduPlane/quadplane.h

@@ -196,6 +196,8 @@ private:
     void control_stabilize(void);
 
     void check_attitude_relax(void);
+    void init_qacro(void);
+    void control_qacro(void);
     void init_hover(void);
     void control_hover(void);
 

ArduPlane/system.cpp

@@ -473,6 +473,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
     case QLAND:
     case QRTL:
     case QAUTOTUNE:
+    case QACRO:
         throttle_allows_nudging = true;
         auto_navigation_mode = false;
         if (!quadplane.init_mode()) {