diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 6247cd63ac574..767dc7cc463d6 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -259,6 +259,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if HAL_BUTTON_ENABLED SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168), #endif +#if AP_QUICKTUNE_ENABLED + SCHED_TASK(update_quicktune, 40, 100, 171), +#endif }; void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, @@ -831,6 +834,18 @@ void Copter::update_altitude() #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 Copter::update_quicktune(void) +{ + quicktune.update(flightmode->supports_quicktune()); +} +#endif + // vehicle specific waypoint info helpers bool Copter::get_wp_distance_m(float &distance) const { diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 97d480ba2d8fa..ba6003c008f96 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -73,6 +73,15 @@ #include #include +/* + default quicktune off in copter except for SITL + */ +#ifndef AP_QUICKTUNE_ENABLED +#define AP_QUICKTUNE_ENABLED CONFIG_HAL_BOARD == HAL_BOARD_SITL +#endif + +#include + // Configuration #include "defines.h" #include "config.h" @@ -87,6 +96,12 @@ #include // Autorotation controllers #endif +#if FRAME_CONFIG == HELI_FRAME +// force quicktune off in heli until it has had testing +#undef AP_QUICKTUNE_ENABLED +#define AP_QUICKTUNE_ENABLED 0 +#endif + #include "RC_Channel.h" // RC Channel Library #include "GCS_Mavlink.h" @@ -486,6 +501,10 @@ class Copter : public AP_Vehicle { AC_Circle *circle_nav; #endif +#if AP_QUICKTUNE_ENABLED + AP_Quicktune quicktune; +#endif + // System Timers // -------------- // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. @@ -716,6 +735,9 @@ class Copter : public AP_Vehicle { bool get_wp_bearing_deg(float &bearing) const override; bool get_wp_crosstrack_error_m(float &xtrack_error) const override; bool get_rate_ef_targets(Vector3f& rate_ef_targets) const override; +#if AP_QUICKTUNE_ENABLED + void update_quicktune(void); +#endif // Attitude.cpp void update_throttle_hover(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index ef7f785bea07a..088dd5dd4ca99 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -493,6 +493,12 @@ const AP_Param::Info Copter::var_info[] = { GOBJECTPTR(circle_nav, "CIRCLE_", AC_Circle), #endif +#if AP_QUICKTUNE_ENABLED + // @Group: QWIK_ + // @Path: ../libraries/AP_Quicktune/AP_Quicktune.cpp + GOBJECT(quicktune, "QWIK_", AP_Quicktune), +#endif + // @Group: ATC_ // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp GOBJECTVARPTR(attitude_control, "ATC_", &copter.attitude_control_var_info), diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 7ba292bc5fed5..8eba4c78936df 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -202,6 +202,7 @@ class Parameters { k_param_circle_nav, k_param_loiter_nav, // 105 k_param_custom_control, + k_param_quicktune, // 110: Telemetry control // diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index fdb175f80c5b8..40bba896f3c59 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -114,7 +114,10 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi case AUX_FUNC::ARMDISARM_AIRMODE: case AUX_FUNC::TURBINE_START: case AUX_FUNC::FLIGHTMODE_PAUSE: +#if AP_QUICKTUNE_ENABLED + case AUX_FUNC::QUICKTUNE: break; +#endif case AUX_FUNC::ACRO_TRAINER: case AUX_FUNC::ATTCON_ACCEL_LIM: case AUX_FUNC::ATTCON_FEEDFWD: @@ -667,6 +670,12 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc // do nothing, used in tuning.cpp for transmitter based tuning break; +#if AP_QUICKTUNE_ENABLED + case AUX_FUNC::QUICKTUNE: + copter.quicktune.update_switch_pos(ch_flag); + break; +#endif + default: return RC_Channel::do_aux_function(ch_option, ch_flag); } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 5579b51fb786e..7b0d82c71a939 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -200,6 +200,11 @@ class Mode { virtual bool allows_weathervaning() const { return false; } #endif +#if AP_QUICKTUNE_ENABLED + // does this mode support VTOL quicktune? + virtual bool supports_quicktune() const { return false; } +#endif + protected: // helper functions @@ -491,6 +496,10 @@ class ModeAltHold : public Mode { const char *name() const override { return "ALT_HOLD"; } const char *name4() const override { return "ALTH"; } +#if AP_QUICKTUNE_ENABLED + bool supports_quicktune() const override { return true; } +#endif + private: }; @@ -1311,6 +1320,10 @@ class ModeLoiter : public Mode { void precision_loiter_xy(); #endif +#if AP_QUICKTUNE_ENABLED + bool supports_quicktune() const override { return true; } +#endif + private: #if AC_PRECLAND_ENABLED diff --git a/ArduCopter/wscript b/ArduCopter/wscript index 08bec013e628a..a70eb3953cbdf 100644 --- a/ArduCopter/wscript +++ b/ArduCopter/wscript @@ -27,7 +27,8 @@ def build(bld): 'AP_Devo_Telem', 'AC_AutoTune', 'AP_KDECAN', - 'AP_SurfaceDistance' + 'AP_SurfaceDistance', + 'AP_Quicktune', ], )