Plane: adding new mode QAUTOTUNE

copter like autotune support for quadplanes and tailsitter in VTOL mode.

cleanup

ArduPlane/ArduPlane.cpp

@@ -647,7 +647,8 @@ void Plane::update_flight_mode(void)
     case QHOVER:
     case QLOITER:
     case QLAND:
-    case QRTL: {
+    case QRTL:
+    case QAUTOTUNE: {
         // set nav_roll and nav_pitch using sticks
         int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max);
         float pitch_input = channel_pitch->norm_input();
@@ -772,6 +773,7 @@ void Plane::update_navigation()
     case QLOITER:
     case QLAND:
     case QRTL:
+    case QAUTOTUNE:
         // nothing to do
         break;
     }

ArduPlane/Attitude.cpp

@@ -155,7 +155,8 @@ void Plane::stabilize_stick_mixing_direct()
         control_mode == QLOITER ||
         control_mode == QLAND ||
         control_mode == QRTL ||
-        control_mode == TRAINING) {
+        control_mode == TRAINING ||
+        control_mode == QAUTOTUNE) {
         return;
     }
     int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
@@ -185,6 +186,7 @@ void Plane::stabilize_stick_mixing_fbw()
         control_mode == QLAND ||
         control_mode == QRTL ||
         control_mode == TRAINING ||
+        control_mode == QAUTOTUNE ||
         (control_mode == AUTO && g.auto_fbw_steer == 42)) {
         return;
     }
@@ -393,7 +395,8 @@ void Plane::stabilize()
                 control_mode == QHOVER ||
                 control_mode == QLOITER ||
                 control_mode == QLAND ||
-                control_mode == QRTL) &&
+                control_mode == QRTL ||
+                control_mode == QAUTOTUNE) &&
                !quadplane.in_tailsitter_vtol_transition()) {
         quadplane.control_run();
     } else {

ArduPlane/GCS_Mavlink.cpp

@@ -34,6 +34,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
     case QLOITER:
     case QLAND:
     case CRUISE:
+    case QAUTOTUNE:
         _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
         break;
     case AUTO:
@@ -1545,6 +1546,7 @@ bool GCS_MAVLINK_Plane::set_mode(const uint8_t mode)
     case QLOITER:
     case QLAND:
     case QRTL:
+    case QAUTOTUNE:
         plane.set_mode((enum FlightMode)mode, MODE_REASON_GCS_COMMAND);
         return true;
     }

ArduPlane/Parameters.cpp

@@ -538,49 +538,49 @@ const AP_Param::Info Plane::var_info[] = {
     // @Param: FLTMODE1
     // @DisplayName: FlightMode1
     // @Description: Flight mode for switch position 1 (910 to 1230 and above 2049)
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE
     // @User: Standard
     GSCALAR(flight_mode1,           "FLTMODE1",       FLIGHT_MODE_1),
 
     // @Param: FLTMODE2
     // @DisplayName: FlightMode2
     // @Description: Flight mode for switch position 2 (1231 to 1360)
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE
     // @User: Standard
     GSCALAR(flight_mode2,           "FLTMODE2",       FLIGHT_MODE_2),
 
     // @Param: FLTMODE3
     // @DisplayName: FlightMode3
     // @Description: Flight mode for switch position 3 (1361 to 1490)
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE
     // @User: Standard
     GSCALAR(flight_mode3,           "FLTMODE3",       FLIGHT_MODE_3),
 
     // @Param: FLTMODE4
     // @DisplayName: FlightMode4
     // @Description: Flight mode for switch position 4 (1491 to 1620)
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE
     // @User: Standard
     GSCALAR(flight_mode4,           "FLTMODE4",       FLIGHT_MODE_4),
 
     // @Param: FLTMODE5
     // @DisplayName: FlightMode5
     // @Description: Flight mode for switch position 5 (1621 to 1749)
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE
     // @User: Standard
     GSCALAR(flight_mode5,           "FLTMODE5",       FLIGHT_MODE_5),
 
     // @Param: FLTMODE6
     // @DisplayName: FlightMode6
     // @Description: Flight mode for switch position 6 (1750 to 2049)
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE
     // @User: Standard
     GSCALAR(flight_mode6,           "FLTMODE6",       FLIGHT_MODE_6),
 
     // @Param: INITIAL_MODE
     // @DisplayName: Initial flight mode
     // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver.
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE
     // @User: Advanced
     GSCALAR(initial_mode,        "INITIAL_MODE",     MANUAL),
 

ArduPlane/Plane.h

@@ -151,6 +151,7 @@ public:
     friend class ParametersG2;
     friend class AP_Arming_Plane;
     friend class QuadPlane;
+    friend class QAutoTune;
     friend class AP_Tuning_Plane;
     friend class AP_AdvancedFailsafe_Plane;
     friend class AP_Avoidance_Plane;

ArduPlane/defines.h

@@ -65,7 +65,8 @@ enum FlightMode {
     QHOVER        = 18,
     QLOITER       = 19,
     QLAND         = 20,
-    QRTL          = 21
+    QRTL          = 21,
+    QAUTOTUNE	  = 22
 };
 
 enum mode_reason_t {

ArduPlane/events.cpp

@@ -28,6 +28,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t re
     case QSTABILIZE:
     case QLOITER:
     case QHOVER:
+    case QAUTOTUNE:
         failsafe.saved_mode = control_mode;
         failsafe.saved_mode_set = true;
         set_mode(QLAND, reason);
@@ -90,6 +91,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t rea
     case QSTABILIZE:
     case QHOVER:
     case QLOITER:
+    case QAUTOTUNE:
         set_mode(QLAND, reason);
         break;
         

ArduPlane/qautotune.cpp

@@ -0,0 +1,65 @@
+#include "Plane.h"
+#include "qautotune.h"
+
+#if QAUTOTUNE_ENABLED
+
+/*
+  initialise QAUTOTUNE mode
+ */
+bool QAutoTune::init()
+{
+    if (!plane.quadplane.available()) {
+        return false;
+    }
+
+    // use position hold while tuning if we were in QLOITER
+    bool position_hold = (plane.previous_mode == QLOITER);
+
+    return init_internals(position_hold,
+                          plane.quadplane.attitude_control,
+                          plane.quadplane.pos_control,
+                          plane.quadplane.ahrs_view,
+                          &plane.quadplane.inertial_nav);
+}
+
+float QAutoTune::get_pilot_desired_climb_rate_cms(void) const
+{
+    return plane.quadplane.get_pilot_desired_climb_rate_cms();
+}
+
+void QAutoTune::get_pilot_desired_rp_yrate_cd(int32_t &des_roll_cd, int32_t &des_pitch_cd, int32_t &yaw_rate_cds)
+{
+    if (plane.channel_roll->get_control_in() == 0 && plane.channel_pitch->get_control_in() == 0) {
+        des_roll_cd = 0;
+        des_pitch_cd = 0;
+    } else {
+        des_roll_cd = plane.nav_roll_cd;
+        des_pitch_cd = plane.nav_pitch_cd;
+    }
+    yaw_rate_cds = plane.quadplane.get_desired_yaw_rate_cds();
+}
+
+void QAutoTune::init_z_limits()
+{
+    plane.quadplane.pos_control->set_max_speed_z(-plane.quadplane.pilot_velocity_z_max, plane.quadplane.pilot_velocity_z_max);
+    plane.quadplane.pos_control->set_max_accel_z(plane.quadplane.pilot_accel_z);
+}
+
+
+// Wrote an event packet
+void QAutoTune::Log_Write_Event(enum at_event id)
+{
+    // offset of 30 aligned with ArduCopter autotune events
+    uint8_t ev_id = 30 + (uint8_t)id;
+    DataFlash_Class::instance()->Log_Write(
+        "EVT",
+        "TimeUS,Id",
+        "s-",
+        "F-",
+        "QB",
+        AP_HAL::micros64(),
+        ev_id);
+}
+
+#endif // QAUTOTUNE_ENABLED
+

ArduPlane/qautotune.h

@@ -0,0 +1,29 @@
+/*
+  support for autotune of quadplanes
+ */
+
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+
+#define QAUTOTUNE_ENABLED !HAL_MINIMIZE_FEATURES
+
+#if QAUTOTUNE_ENABLED
+
+#include <AC_AutoTune/AC_AutoTune.h>
+
+class QAutoTune : public AC_AutoTune
+{
+public:
+    friend class QuadPlane;
+
+    bool init() override;
+
+protected:
+    float get_pilot_desired_climb_rate_cms(void) const override;
+    void get_pilot_desired_rp_yrate_cd(int32_t &roll_cd, int32_t &pitch_cd, int32_t &yaw_rate_cds) override;
+    void init_z_limits() override;
+    void Log_Write_Event(enum at_event id) override;
+};
+
+#endif // QAUTOTUNE_ENABLED

ArduPlane/quadplane.cpp

@@ -332,7 +332,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),
 
     AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2),
-    
+
     AP_GROUPEND
 };
 
@@ -377,6 +377,12 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @User: Standard
     AP_GROUPINFO("TAILSIT_RLL_MX", 5, QuadPlane, tailsitter.max_roll_angle, 0),
 
+#if QAUTOTUNE_ENABLED
+    // @Group: AUTOTUNE_
+    // @Path: qautotune.cpp
+    AP_SUBGROUPINFO(qautotune, "AUTOTUNE_",  6, QuadPlane, QAutoTune),
+#endif
+
     AP_GROUPEND
 };
 
@@ -930,7 +936,7 @@ bool QuadPlane::is_flying_vtol(void) const
     if (plane.control_mode == GUIDED && guided_takeoff) {
         return true;
     }
-    if (plane.control_mode == QSTABILIZE || plane.control_mode == QHOVER || plane.control_mode == QLOITER) {
+    if (plane.control_mode == QSTABILIZE || plane.control_mode == QHOVER || plane.control_mode == QLOITER || plane.control_mode == QAUTOTUNE) {
         // in manual flight modes only consider aircraft landed when pilot demanded throttle is zero
         return plane.get_throttle_input() > 0;
     }
@@ -1630,6 +1636,11 @@ void QuadPlane::control_run(void)
     case QRTL:
         control_qrtl();
         break;
+#if QAUTOTUNE_ENABLED
+    case QAUTOTUNE:
+        qautotune.run();
+        break;
+#endif
     default:
         break;
     }
@@ -1673,6 +1684,10 @@ bool QuadPlane::init_mode(void)
     case GUIDED:
         guided_takeoff = false;
         break;
+#if QAUTOTUNE_ENABLED
+    case QAUTOTUNE:
+        return qautotune.init();
+#endif
     default:
         break;
     }
@@ -1762,6 +1777,7 @@ bool QuadPlane::in_vtol_mode(void) const
             plane.control_mode == QLOITER ||
             plane.control_mode == QLAND ||
             plane.control_mode == QRTL ||
+            plane.control_mode == QAUTOTUNE ||
             ((plane.control_mode == GUIDED || plane.control_mode == AVOID_ADSB) && plane.auto_state.vtol_loiter) ||
             in_vtol_auto());
 }
@@ -2338,7 +2354,8 @@ int8_t QuadPlane::forward_throttle_pct(void)
         !motors->armed() ||
         vel_forward.gain <= 0 ||
         plane.control_mode == QSTABILIZE ||
-        plane.control_mode == QHOVER) {
+        plane.control_mode == QHOVER ||
+        plane.control_mode == QAUTOTUNE) {
         return 0;
     }
 
@@ -2415,7 +2432,8 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void)
         !motors->armed() ||
         weathervane.gain <= 0 ||
         plane.control_mode == QSTABILIZE ||
-        plane.control_mode == QHOVER) {
+        plane.control_mode == QHOVER ||
+        plane.control_mode == QAUTOTUNE) {
         weathervane.last_output = 0;
         return 0;
     }

ArduPlane/quadplane.h

@@ -10,6 +10,7 @@
 #include <AC_Fence/AC_Fence.h>
 #include <AC_Avoidance/AC_Avoid.h>
 #include <AP_Proximity/AP_Proximity.h>
+#include "qautotune.h"
 
 /*
   QuadPlane specific functionality
@@ -21,7 +22,8 @@ public:
     friend class AP_Tuning_Plane;
     friend class GCS_MAVLINK_Plane;
     friend class AP_AdvancedFailsafe_Plane;
-    
+    friend class QAutoTune;
+
     QuadPlane(AP_AHRS_NavEKF &_ahrs);
 
     // var_info for holding Parameter information
@@ -471,7 +473,12 @@ private:
       return true if current mission item is a vtol landing
      */
     bool is_vtol_land(uint16_t id) const;
-    
+
+#if QAUTOTUNE_ENABLED
+    // qautotune mode
+    QAutoTune qautotune;
+#endif
+
 public:
     void motor_test_output();
     MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,

ArduPlane/sensors.cpp

@@ -162,6 +162,7 @@ void Plane::update_sensor_status_flags(void)
     case QHOVER:
     case QLAND:
     case QLOITER:
+    case QAUTOTUNE:
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
         break;

ArduPlane/system.cpp

@@ -278,6 +278,13 @@ enum FlightMode Plane::get_previous_mode() {
 
 void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
 {
+#if !QAUTOTUNE_ENABLED
+    if (mode == QAUTOTUNE) {
+        gcs().send_text(MAV_SEVERITY_INFO,"QAUTOTUNE disabled");
+        mode = QHOVER;
+    }
+#endif
+
     if(control_mode == mode) {
         // don't switch modes if we are already in the correct mode.
         return;
@@ -481,6 +488,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
     case QLOITER:
     case QLAND:
     case QRTL:
+    case QAUTOTUNE:
         throttle_allows_nudging = true;
         auto_navigation_mode = false;
         if (!quadplane.init_mode()) {
@@ -510,7 +518,8 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
 void Plane::exit_mode(enum FlightMode mode)
 {
     // stop mission when we leave auto
-    if (mode == AUTO) {
+    switch (mode) {
+    case AUTO:
         if (mission.state() == AP_Mission::MISSION_RUNNING) {
             mission.stop();
 
@@ -521,6 +530,14 @@ void Plane::exit_mode(enum FlightMode mode)
             }
         }
         auto_state.started_flying_in_auto_ms = 0;
+        break;
+#if QAUTOTUNE_ENABLED
+    case QAUTOTUNE:
+        quadplane.qautotune.stop();
+        break;
+#endif
+    default:
+        break;
     }
 }
 
@@ -703,6 +720,9 @@ void Plane::notify_flight_mode(enum FlightMode mode)
     case QRTL:
         notify.set_flight_mode_str("QRTL");
         break;
+    case QAUTOTUNE:
+        notify.set_flight_mode_str("QAUTOTUNE");
+        break;
     default:
         notify.set_flight_mode_str("----");
         break;
@@ -781,5 +801,10 @@ bool Plane::disarm_motors(void)
     // reload target airspeed which could have been modified by a mission
     plane.aparm.airspeed_cruise_cm.load();
     
+#if QAUTOTUNE_ENABLED
+    //save qautotune gains if enabled and success
+    quadplane.qautotune.save_tuning_gains();
+#endif
+
     return true;
 }

ArduPlane/tiltrotor.cpp

@@ -110,7 +110,8 @@ void QuadPlane::tiltrotor_continuous_update(void)
          to forward flight and should put the rotors all the way forward
     */
     if (plane.control_mode == QSTABILIZE ||
-        plane.control_mode == QHOVER) {
+        plane.control_mode == QHOVER ||
+        plane.control_mode == QAUTOTUNE) {
         tiltrotor_slew(0);
         return;
     }

ArduPlane/wscript

@@ -32,6 +32,7 @@ def build(bld):
             'AP_Soaring',
             'AP_Devo_Telem',
             'AP_OSD',
+            'AC_AutoTune',
         ],
     )