diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 0181f392d8..db645762af 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index d643d8b013..6bdf1e8753 100644 --- a/ArduPlane/Parameters.cpp +++ b/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 }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 377eb9ded8..aa15f14e4c 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -361,6 +361,7 @@ public: k_param_takeoff_options, k_param_pullup = 270, + k_param_quicktune, }; AP_Int16 format_version; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 64b5ba6d95..c7c5f6a17d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -330,6 +330,10 @@ private: 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 @@ private: 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); diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index ac05d1174a..2dc6f1edaa 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/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); } diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h index 858034478c..e9b4804aac 100644 --- a/ArduPlane/RC_Channel.h +++ b/ArduPlane/RC_Channel.h @@ -30,7 +30,6 @@ private: void do_aux_function_soaring_3pos(AuxSwitchPos ch_flag); void do_aux_function_flare(AuxSwitchPos ch_flag); - }; class RC_Channels_Plane : public RC_Channels diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 230b372499..d4436d49eb 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -11,6 +11,12 @@ #include #include "pullup.h" +#ifndef AP_QUICKTUNE_ENABLED +#define AP_QUICKTUNE_ENABLED HAL_QUADPLANE_ENABLED +#endif + +#include + class AC_PosControl; class AC_AttitudeControl_Multi; class AC_Loiter; @@ -142,6 +148,11 @@ public: // 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 @@ protected: 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 @@ public: protected: bool _enter() override; +#if AP_QUICKTUNE_ENABLED + bool supports_quicktune() const override { return true; } +#endif }; class ModeQLoiter : public Mode @@ -685,6 +702,10 @@ protected: 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 diff --git a/ArduPlane/qautotune.h b/ArduPlane/qautotune.h index 938c8e47e4..684fe5e547 100644 --- a/ArduPlane/qautotune.h +++ b/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 diff --git a/ArduPlane/wscript b/ArduPlane/wscript index 78fad7650e..930fec0b53 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -27,6 +27,7 @@ def build(bld): 'AP_Follow', 'AC_PrecLand', 'AP_IRLock', + 'AP_Quicktune', ], )