Quicktune: backport for 4.6

ArduPlane/ArduPlane.cpp

@@ -139,6 +139,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
 #if AC_PRECLAND_ENABLED
     SCHED_TASK(precland_update, 400, 50, 160),
 #endif
+#if AP_QUICKTUNE_ENABLED
+    SCHED_TASK(update_quicktune, 40, 100, 163),
+#endif
 };
 
 void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
@@ -1025,4 +1028,16 @@ void Plane::precland_update(void)
 }
 #endif
 
+#if AP_QUICKTUNE_ENABLED
+/*
+  update AP_Quicktune object. We pass the supports_quicktune() method
+  in so that quicktune can detect if the user changes to a
+  non-quicktune capable mode while tuning and the gains can be reverted
+ */
+void Plane::update_quicktune(void)
+{
+    quicktune.update(control_mode->supports_quicktune());
+}
+#endif
+
 AP_HAL_MAIN_CALLBACKS(&plane);

ArduPlane/Parameters.cpp

@@ -1029,6 +1029,12 @@ const AP_Param::Info Plane::var_info[] = {
     // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp
     PARAM_VEHICLE_INFO,
 
+#if AP_QUICKTUNE_ENABLED
+    // @Group: QWIK_
+    // @Path: ../libraries/AP_Quicktune/AP_Quicktune.cpp
+    GOBJECT(quicktune, "QWIK_",  AP_Quicktune),
+#endif
+
     AP_VAREND
 };
 

ArduPlane/Parameters.h

@@ -361,6 +361,7 @@ class Parameters {
         k_param_takeoff_options,
 
         k_param_pullup = 270,
+        k_param_quicktune,
     };
 
     AP_Int16 format_version;

ArduPlane/Plane.h

@@ -330,6 +330,10 @@ class Plane : public AP_Vehicle {
     ModeThermal mode_thermal;
 #endif
 
+#if AP_QUICKTUNE_ENABLED
+    AP_Quicktune quicktune;
+#endif
+
     // This is the state of the flight control system
     // There are multiple states defined such as MANUAL, FBW-A, AUTO
     Mode *control_mode = &mode_initializing;
@@ -875,6 +879,10 @@ class Plane : public AP_Vehicle {
     static const TerrainLookupTable Terrain_lookup[];
 #endif
 
+#if AP_QUICKTUNE_ENABLED
+    void update_quicktune(void);
+#endif
+
     // Attitude.cpp
     void adjust_nav_pitch_throttle(void);
     void update_load_factor(void);

ArduPlane/RC_Channel.cpp

@@ -1,6 +1,7 @@
 #include "Plane.h"
 
 #include "RC_Channel.h"
+#include "qautotune.h"
 
 // defining these two macros and including the RC_Channels_VarInfo
 // header defines the parameter information common to all vehicle
@@ -176,6 +177,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option,
 #endif
 #if QAUTOTUNE_ENABLED
     case AUX_FUNC::AUTOTUNE_TEST_GAINS:
+#endif
+#if AP_QUICKTUNE_ENABLED
+    case AUX_FUNC::QUICKTUNE:
 #endif
         break;
 
@@ -458,6 +462,12 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch
         break;
 #endif
 
+#if AP_QUICKTUNE_ENABLED
+    case AUX_FUNC::QUICKTUNE:
+        plane.quicktune.update_switch_pos(ch_flag);
+        break;
+#endif
+
     default:
         return RC_Channel::do_aux_function(ch_option, ch_flag);
     }

ArduPlane/RC_Channel.h

@@ -30,7 +30,6 @@ class RC_Channel_Plane : public RC_Channel
     void do_aux_function_soaring_3pos(AuxSwitchPos ch_flag);
 
     void do_aux_function_flare(AuxSwitchPos ch_flag);
-
 };
 
 class RC_Channels_Plane : public RC_Channels

ArduPlane/mode.h

@@ -11,6 +11,12 @@
 #include <AP_Mission/AP_Mission.h>
 #include "pullup.h"
 
+#ifndef AP_QUICKTUNE_ENABLED
+#define AP_QUICKTUNE_ENABLED HAL_QUADPLANE_ENABLED
+#endif
+
+#include <AP_Quicktune/AP_Quicktune.h>
+
 class AC_PosControl;
 class AC_AttitudeControl_Multi;
 class AC_Loiter;
@@ -142,6 +148,11 @@ class Mode
     // true if voltage correction should be applied to throttle
     virtual bool use_battery_compensation() const;
 
+#if AP_QUICKTUNE_ENABLED
+    // does this mode support VTOL quicktune?
+    virtual bool supports_quicktune() const { return false; }
+#endif
+
 protected:
 
     // subclasses override this to perform checks before entering the mode
@@ -322,6 +333,9 @@ class ModeGuided : public Mode
 
     bool _enter() override;
     bool _pre_arm_checks(size_t buflen, char *buffer) const override { return true; }
+#if AP_QUICKTUNE_ENABLED
+    bool supports_quicktune() const override { return true; }
+#endif
 
 private:
     float active_radius_m;
@@ -659,6 +673,9 @@ class ModeQHover : public Mode
 protected:
 
     bool _enter() override;
+#if AP_QUICKTUNE_ENABLED
+    bool supports_quicktune() const override { return true; }
+#endif
 };
 
 class ModeQLoiter : public Mode
@@ -685,6 +702,10 @@ friend class Plane;
 
     bool _enter() override;
     uint32_t last_target_loc_set_ms;
+
+#if AP_QUICKTUNE_ENABLED
+    bool supports_quicktune() const override { return true; }
+#endif
 };
 
 class ModeQLand : public Mode

ArduPlane/qautotune.h

@@ -8,7 +8,7 @@
 
 #include "quadplane.h"
 #ifndef QAUTOTUNE_ENABLED
-  #define QAUTOTUNE_ENABLED HAL_QUADPLANE_ENABLED
+#define QAUTOTUNE_ENABLED (HAL_QUADPLANE_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif
 
 #if QAUTOTUNE_ENABLED

ArduPlane/wscript

@@ -27,6 +27,7 @@ def build(bld):
             'AP_Follow',
             'AC_PrecLand',
             'AP_IRLock',
+            'AP_Quicktune',
         ],
     )
 

Tools/autotest/default_params/quadplane.parm

@@ -70,10 +70,16 @@ Q_M_PWM_MIN      1000
 Q_M_PWM_MAX      2000
 Q_M_BAT_VOLT_MAX 12.8
 Q_M_BAT_VOLT_MIN 9.6
-Q_A_RAT_RLL_P 0.15
-Q_A_RAT_PIT_P 0.15
-Q_A_RAT_RLL_D 0.002
-Q_A_RAT_PIT_D 0.002
+Q_A_RAT_RLL_P 0.108
+Q_A_RAT_RLL_I 0.108
+Q_A_RAT_RLL_D 0.001
+Q_A_RAT_PIT_P 0.103
+Q_A_RAT_PIT_I 0.103
+Q_A_RAT_PIT_D 0.001
+Q_A_RAT_YAW_P 0.2
+Q_A_RAT_YAW_P 0.02
+Q_A_ANG_RLL_P 6
+Q_A_ANG_PIT_P 6
 RTL_ALTITUDE 20.00
 PTCH_RATE_FF 1.407055
 RLL_RATE_FF 0.552741

Tools/autotest/quadplane.py

@@ -363,6 +363,20 @@ def EXTENDED_SYS_STATE(self):
     def QAUTOTUNE(self):
         '''test Plane QAutoTune mode'''
 
+        # adjust tune so QAUTOTUNE can cope
+        self.set_parameters({
+            "Q_A_RAT_RLL_P" : 0.15,
+            "Q_A_RAT_RLL_I" : 0.25,
+            "Q_A_RAT_RLL_D" : 0.002,
+            "Q_A_RAT_PIT_P" : 0.15,
+            "Q_A_RAT_PIT_I" : 0.25,
+            "Q_A_RAT_PIT_D" : 0.002,
+            "Q_A_RAT_YAW_P" : 0.18,
+            "Q_A_RAT_YAW_I" : 0.018,
+            "Q_A_ANG_RLL_P" : 4.5,
+            "Q_A_ANG_PIT_P" : 4.5,
+            })
+
         # this is a list of all parameters modified by QAUTOTUNE.  Set
         # them so that when the context is popped we get the original
         # values back:
@@ -1411,6 +1425,81 @@ def VTOLQuicktune(self):
 
         self.wait_disarmed(timeout=120)
 
+    def VTOLQuicktune_CPP(self):
+        '''VTOL Quicktune in C++'''
+        self.set_parameters({
+            "RC7_OPTION": 181,
+            "QWIK_ENABLE" : 1,
+            "QWIK_DOUBLE_TIME" : 5, # run faster for autotest
+        })
+
+        self.context_push()
+        self.context_collect('STATUSTEXT')
+
+        # reduce roll/pitch gains by 2
+        gain_mul = 0.5
+        soften_params = ['Q_A_RAT_RLL_P', 'Q_A_RAT_RLL_I', 'Q_A_RAT_RLL_D',
+                         'Q_A_RAT_PIT_P', 'Q_A_RAT_PIT_I', 'Q_A_RAT_PIT_D',
+                         'Q_A_RAT_YAW_P', 'Q_A_RAT_YAW_I']
+
+        original_values = self.get_parameters(soften_params)
+
+        softened_values = {}
+        for p in original_values.keys():
+            softened_values[p] = original_values[p] * gain_mul
+        self.set_parameters(softened_values)
+
+        self.wait_ready_to_arm()
+        self.change_mode("QLOITER")
+        self.set_rc(7, 1000)
+        self.arm_vehicle()
+        self.takeoff(20, 'QLOITER')
+
+        # use rc switch to start tune
+        self.set_rc(7, 1500)
+
+        self.wait_text("Quicktune: starting tune", check_context=True)
+        for axis in ['Roll', 'Pitch', 'Yaw']:
+            self.wait_text("Starting %s tune" % axis, check_context=True)
+            self.wait_text("Quicktune: %s D done" % axis, check_context=True, timeout=120)
+            self.wait_text("Quicktune: %s P done" % axis, check_context=True, timeout=120)
+            self.wait_text("Quicktune: %s done" % axis, check_context=True, timeout=120)
+
+        new_values = self.get_parameters(soften_params)
+        for p in original_values.keys():
+            threshold = 0.8 * original_values[p]
+            self.progress("tuned param %s %.4f need %.4f" % (p, new_values[p], threshold))
+            if new_values[p] < threshold:
+                raise NotAchievedException(
+                    "parameter %s %.4f not increased over %.4f" %
+                    (p, new_values[p], threshold))
+
+        self.progress("ensure we are not overtuned")
+        self.set_parameter('SIM_ENGINE_MUL', 0.9)
+
+        self.delay_sim_time(5)
+
+        # and restore it
+        self.set_parameter('SIM_ENGINE_MUL', 1)
+
+        for i in range(5):
+            self.wait_heartbeat()
+
+        if self.statustext_in_collections("ABORTING"):
+            raise NotAchievedException("tune has aborted, overtuned")
+
+        self.progress("using aux fn for save tune")
+
+        # to test aux function method, use aux fn for save
+        self.run_auxfunc(181, 2)
+        self.wait_text("Quicktune: saved", check_context=True)
+        self.change_mode("QLAND")
+
+        self.wait_disarmed(timeout=120)
+        self.set_parameter("QWIK_ENABLE", 0)
+        self.context_pop()
+        self.reboot_sitl()
+
     def PrecisionLanding(self):
         '''VTOL precision landing'''
 
@@ -2104,6 +2193,7 @@ def tests(self):
             self.LoiterAltQLand,
             self.VTOLLandSpiral,
             self.VTOLQuicktune,
+            self.VTOLQuicktune_CPP,
             self.PrecisionLanding,
             self.ShipLanding,
             Test(self.MotorTest, kwargs={  # tests motors 4 and 2

Tools/scripts/build_options.py

@@ -223,6 +223,7 @@ def config_option(self):
     Feature('Plane', 'AP_TX_TUNING', 'AP_TUNING_ENABLED', 'Enable TX-based tuning parameter adjustments', 0, None),
     Feature('Plane', 'PLANE_GUIDED_SLEW', 'AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', 'Enable Offboard-guided slew commands', 0, None),  # noqa:401
     Feature('Plane', 'PLANE_GLIDER_PULLUP', 'AP_PLANE_GLIDER_PULLUP_ENABLED', 'Enable Glider pullup support', 0, None),
+    Feature('Plane', 'QUICKTUNE', 'AP_QUICKTUNE_ENABLED', 'Enable VTOL quicktune', 0, None),
 
     Feature('RC', 'RC_Protocol', 'AP_RCPROTOCOL_ENABLED', "Enable Serial RC Protocols", 0, None),   # NOQA: E501
     Feature('RC', 'RC_CRSF', 'AP_RCPROTOCOL_CRSF_ENABLED', "Enable CRSF", 0, "RC_Protocol"),   # NOQA: E501

Tools/scripts/extract_features.py

@@ -281,6 +281,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
 
             ('AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', r'GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands'),
             ('AP_SERIALMANAGER_REGISTER_ENABLED', r'AP_SerialManager::register_port'),
+            ('AP_QUICKTUNE_ENABLED', r'AP_Quicktune::update'),
         ]
 
     def progress(self, msg):

libraries/AP_HAL_ChibiOS/hwdef/LongBowF405WING/hwdef.dat

@@ -182,13 +182,11 @@ include ../include/minimize_fpv_osd.inc
 #undef AP_LANDINGGEAR_ENABLED
 #undef HAL_MOUNT_ENABLED
 #undef HAL_MOUNT_SERVO_ENABLED
-#undef QAUTOTUNE_ENABLED
 
 #define AP_CAMERA_MOUNT_ENABLED 1
 #define AP_LANDINGGEAR_ENABLED 1
 #define HAL_MOUNT_ENABLED 1
 #define HAL_MOUNT_SERVO_ENABLED 1
-#define QAUTOTUNE_ENABLED 1
 
 #define DEFAULT_NTF_LED_TYPES 257
 

libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat

@@ -174,12 +174,10 @@ include ../include/minimize_fpv_osd.inc
 undef AP_CAMERA_MOUNT_ENABLED
 undef HAL_MOUNT_ENABLED
 undef HAL_MOUNT_SERVO_ENABLED
-undef QAUTOTUNE_ENABLED
 
 define AP_CAMERA_MOUNT_ENABLED 1
 define HAL_MOUNT_ENABLED 1
 define AP_MOUNT_BACKEND_DEFAULT_ENABLED 0
 define HAL_MOUNT_SERVO_ENABLED 1
-define QAUTOTUNE_ENABLED 1
 
 define DEFAULT_NTF_LED_TYPES 257

libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc

@@ -102,7 +102,6 @@ define AP_LANDINGGEAR_ENABLED APM_BUILD_COPTER_OR_HELI
 # Plane-specific defines; these defines are only used in the Plane
 #  directory, but are seen across the entire codebase:
 define AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED 0
-define QAUTOTUNE_ENABLED 0
 
 # Copter-specific defines; these defines are only used in the Copter
 #  directory, but are seen across the entire codebase: