Add new tiltrotor flightmodes

ArduPlane/ArduPlane.cpp

@@ -526,6 +526,8 @@ void Plane::update_navigation()
     case Mode::Number::QRTL:
     case Mode::Number::QAUTOTUNE:
     case Mode::Number::QACRO:
+    case Mode::Number::QACRO_FTHR:
+    case Mode::Number::QSTAB_FTHR:
         // nothing to do
         break;
     }

ArduPlane/Attitude.cpp

@@ -144,6 +144,8 @@ void Plane::stabilize_stick_mixing_direct()
         control_mode == &mode_qland ||
         control_mode == &mode_qrtl ||
         control_mode == &mode_qacro ||
+        control_mode == &mode_qafthr ||
+        control_mode == &mode_qsfthr ||
         control_mode == &mode_training ||
         control_mode == &mode_qautotune) {
         return;
@@ -175,6 +177,8 @@ void Plane::stabilize_stick_mixing_fbw()
         control_mode == &mode_qland ||
         control_mode == &mode_qrtl ||
         control_mode == &mode_qacro ||
+        control_mode == &mode_qafthr ||
+        control_mode == &mode_qsfthr ||
         control_mode == &mode_training ||
         control_mode == &mode_qautotune ||
         (control_mode == &mode_auto && g.auto_fbw_steer == 42)) {
@@ -403,6 +407,8 @@ void Plane::stabilize()
                 control_mode == &mode_qland ||
                 control_mode == &mode_qrtl ||
                 control_mode == &mode_qacro ||
+                control_mode == &mode_qafthr ||
+                control_mode == &mode_qsfthr ||
                 control_mode == &mode_qautotune) &&
                !quadplane.in_tailsitter_vtol_transition()) {
         quadplane.control_run();

ArduPlane/GCS_Mavlink.cpp

@@ -24,13 +24,15 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
     case Mode::Number::TRAINING:
     case Mode::Number::ACRO:
     case Mode::Number::QACRO:
+    case Mode::Number::QACRO_FTHR:
         _base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
         break;
     case Mode::Number::STABILIZE:
     case Mode::Number::FLY_BY_WIRE_A:
     case Mode::Number::AUTOTUNE:
     case Mode::Number::FLY_BY_WIRE_B:
     case Mode::Number::QSTABILIZE:
+    case Mode::Number::QSTAB_FTHR:
     case Mode::Number::QHOVER:
     case Mode::Number::QLOITER:
     case Mode::Number::QLAND:

ArduPlane/GCS_Plane.cpp

@@ -46,13 +46,15 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
 
     case Mode::Number::ACRO:
     case Mode::Number::QACRO:
+    case Mode::Number::QACRO_FTHR:
         rate_controlled = true;
         break;
 
     case Mode::Number::STABILIZE:
     case Mode::Number::FLY_BY_WIRE_A:
     case Mode::Number::AUTOTUNE:
     case Mode::Number::QSTABILIZE:
+    case Mode::Number::QSTAB_FTHR:
     case Mode::Number::QHOVER:
     case Mode::Number::QLAND:
     case Mode::Number::QLOITER:

ArduPlane/Plane.h

@@ -155,6 +155,8 @@ class Plane : public AP_Vehicle {
     friend class ModeQLand;
     friend class ModeQRTL;
     friend class ModeQAcro;
+    friend class ModeQAFTHR;
+    friend class ModeQSFTHR;
     friend class ModeQAutotune;
     friend class ModeTakeoff;
 
@@ -280,6 +282,8 @@ class Plane : public AP_Vehicle {
     ModeQLand mode_qland;
     ModeQRTL mode_qrtl;
     ModeQAcro mode_qacro;
+    ModeQAFTHR mode_qafthr;
+    ModeQSFTHR mode_qsfthr;
     ModeQAutotune mode_qautotune;
     ModeTakeoff mode_takeoff;
 

ArduPlane/RC_Channel.cpp

@@ -68,6 +68,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
     case AUX_FUNC::MANUAL:
     case AUX_FUNC::RTL:
     case AUX_FUNC::TAKEOFF:
+    case AUX_FUNC::FWD_THR:
         break;
 
     case AUX_FUNC::REVERSE_THROTTLE:
@@ -85,7 +86,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
         // handle in parent class
         RC_Channel::init_aux_function(ch_option, ch_flag);
         break;
-}
+    }
 }
 
 // do_aux_function - implement the function invoked by auxillary switches
@@ -132,6 +133,9 @@ 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::FWD_THR:
+        break;
+
     default:
         RC_Channel::do_aux_function(ch_option, ch_flag);
         break;

ArduPlane/control_modes.cpp

@@ -67,6 +67,12 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
     case Mode::Number::QACRO:
         ret = &mode_qacro;
         break;
+    case Mode::Number::QACRO_FTHR:
+        ret = &mode_qafthr;
+        break;
+    case Mode::Number::QSTAB_FTHR:
+        ret = &mode_qsfthr;
+        break;
     case Mode::Number::QAUTOTUNE:
         ret = &mode_qautotune;
         break;

ArduPlane/events.cpp

@@ -30,6 +30,8 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
     case Mode::Number::QHOVER:
     case Mode::Number::QAUTOTUNE:
     case Mode::Number::QACRO:
+    case Mode::Number::QACRO_FTHR:
+    case Mode::Number::QSTAB_FTHR:
         failsafe.saved_mode_number = control_mode->mode_number();
         failsafe.saved_mode_set = true;
         if (quadplane.options & QuadPlane::OPTION_FS_QRTL) {
@@ -98,6 +100,8 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
     case Mode::Number::QHOVER:
     case Mode::Number::QLOITER:
     case Mode::Number::QACRO:
+    case Mode::Number::QACRO_FTHR:
+    case Mode::Number::QSTAB_FTHR:
     case Mode::Number::QAUTOTUNE:
         if (quadplane.options & QuadPlane::OPTION_FS_QRTL) {
             set_mode(mode_qrtl, reason);

ArduPlane/mode.h

@@ -38,6 +38,8 @@ class Mode
         QRTL          = 21,
         QAUTOTUNE     = 22,
         QACRO         = 23,
+        QACRO_FTHR    = 24,
+        QSTAB_FTHR    = 25,
     };
 
     // Constructor
@@ -420,6 +422,8 @@ class ModeQAcro : public Mode
 {
 public:
 
+    friend class ModeQAFTHR;
+
     Number mode_number() const override { return Number::QACRO; }
     const char *name() const override { return "QACO"; }
     const char *name4() const override { return "QACRO"; }
@@ -480,3 +484,39 @@ class ModeTakeoff: public Mode
 
     bool _enter() override;
 };
+
+class ModeQAFTHR : public Mode
+{
+public:
+
+    Number mode_number() const override { return Number::QACRO_FTHR; }
+    const char *name() const override { return "QACRO_FTHR"; }
+    const char *name4() const override { return "QAFT"; }
+
+    bool is_vtol_mode() const override { return true; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+protected:
+
+    bool _enter() override;
+};
+
+class ModeQSFTHR : public Mode
+{
+public:
+
+    Number mode_number() const override { return Number::QSTAB_FTHR; }
+    const char *name() const override { return "QSTAB_FTHR"; }
+    const char *name4() const override { return "QSFT"; }
+
+    bool is_vtol_mode() const override { return true; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+protected:
+
+    bool _enter() override;
+};

ArduPlane/mode_qacro.cpp

@@ -21,6 +21,9 @@ void ModeQAcro::update()
     Vector3f att_target = plane.quadplane.attitude_control->get_att_target_euler_cd();
     plane.nav_pitch_cd = att_target.y;
     plane.nav_roll_cd = att_target.x;
+
+    // set rudder using copter controller
+    plane.steering_control.rudder = plane.quadplane.motors->get_yaw() * SERVO_MAX;
     return;
 }
 

ArduPlane/mode_qafthr.cpp

@@ -0,0 +1,12 @@
+#include "mode.h"
+#include "Plane.h"
+
+bool ModeQAFTHR::_enter()
+{
+    return plane.mode_qacro._enter();
+}
+
+void ModeQAFTHR::update()
+{
+    return plane.mode_qacro.update();
+}

ArduPlane/mode_qsfthr.cpp

@@ -0,0 +1,43 @@
+#include "mode.h"
+#include "Plane.h"
+
+bool ModeQSFTHR::_enter()
+{
+    return plane.mode_qstabilize._enter();
+}
+
+// this update method differs from ModeQStabilize::update()
+// only in the roll_limit setting; it is higher since this is a more aerobatic
+// use case than qstabilize
+// TODO: add an argument to ModeQStabilize::update() instead of duplicating code
+void ModeQSFTHR::update()
+{
+    // set nav_roll and nav_pitch using sticks
+
+    // use larger of Plane and quadplane roll limits (or just the plane limit)
+    int16_t roll_limit = MAX(plane.roll_limit_cd, plane.quadplane.aparm.angle_max);
+
+    float pitch_input = plane.channel_pitch->norm_input();
+    // Scale from normalized input [-1,1] to centidegrees
+    if (plane.quadplane.tailsitter_active()) {
+        // separate limit for tailsitter roll, if set
+        if (plane.quadplane.tailsitter.max_roll_angle > 0) {
+            roll_limit = plane.quadplane.tailsitter.max_roll_angle * 100.0f;
+        }
+
+        // angle max for tailsitter pitch
+        plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max;
+    } else {
+        // pitch is further constrained by LIM_PITCH_MIN/MAX which may impose
+        // tighter (possibly asymmetrical) limits than Q_ANGLE_MAX
+        if (pitch_input > 0) {
+            plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max_cd, plane.quadplane.aparm.angle_max);
+        } else {
+            plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min_cd, plane.quadplane.aparm.angle_max);
+        }
+        plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get());
+    }
+
+    plane.nav_roll_cd = (plane.channel_roll->get_control_in() / 4500.0) * roll_limit;
+    plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -roll_limit, roll_limit);
+}