diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 1e10df8c42784..7d27791de5571 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 5f746640e16e2..04a5ba3a59525 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1038,6 +1038,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 24645b3b88471..e7cc5326f4280 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -362,6 +362,7 @@ class Parameters { k_param_takeoff_throttle_idle, k_param_pullup = 270, + k_param_quicktune, }; AP_Int16 format_version; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index ec6de84fe709c..d2d35a7b14e0a 100644 --- a/ArduPlane/Plane.h +++ b/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); diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index ac05d1174af0a..2dc6f1edaab01 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 858034478c9bf..e9b4804aac545 100644 --- a/ArduPlane/RC_Channel.h +++ b/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 diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 8260924afca89..213e547c6a668 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 @@ 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 @@ -325,6 +336,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; @@ -662,6 +676,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 @@ -688,6 +705,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 diff --git a/ArduPlane/qautotune.h b/ArduPlane/qautotune.h index 938c8e47e4332..684fe5e54783d 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 78fad7650e377..930fec0b53763 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -27,6 +27,7 @@ def build(bld): 'AP_Follow', 'AC_PrecLand', 'AP_IRLock', + 'AP_Quicktune', ], ) diff --git a/Tools/autotest/default_params/quadplane.parm b/Tools/autotest/default_params/quadplane.parm index 62c5225d052d5..9da3368889f2f 100644 --- a/Tools/autotest/default_params/quadplane.parm +++ b/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 diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 7296e349451a4..721752a5c337d 100644 --- a/Tools/autotest/quadplane.py +++ b/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 diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index f31edfaf87ff3..1c259adeae211 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -224,6 +224,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 diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index de6eec6589523..c4a554e805972 100755 --- a/Tools/scripts/extract_features.py +++ b/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): diff --git a/libraries/AP_HAL_ChibiOS/hwdef/LongBowF405WING/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/LongBowF405WING/hwdef.dat index e0797d7a34bc2..01db0f16784da 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/LongBowF405WING/hwdef.dat +++ b/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 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat index 6093b76e0aac0..5bc0e9cbf912d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat +++ b/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 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc index 005fb89895002..3c6e149ee06ec 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc +++ b/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: diff --git a/libraries/AP_Quicktune/AP_Quicktune.cpp b/libraries/AP_Quicktune/AP_Quicktune.cpp new file mode 100644 index 0000000000000..c8c5e5a5e573c --- /dev/null +++ b/libraries/AP_Quicktune/AP_Quicktune.cpp @@ -0,0 +1,689 @@ +/* + C++ implementation of quicktune based on original lua script + */ + +// quicktune is not performance sensitive, save flash +#pragma GCC optimize("Os") + +#include "AP_Quicktune.h" + +#if AP_QUICKTUNE_ENABLED + +#define UPDATE_RATE_HZ 40 +#define STAGE_DELAY 4000 +#define PILOT_INPUT_DELAY 4000 +#define YAW_FLTE_MAX 2.0 +#define FLTD_MUL 0.5 +#define FLTT_MUL 0.5 +#define DEFAULT_SMAX 50.0 +#define OPTIONS_TWO_POSITION (1<<0) + +#include +#include +#include +#include +#include +#include + +const AP_Param::GroupInfo AP_Quicktune::var_info[] = { + // @Param: ENABLE + // @DisplayName: Quicktune enable + // @Description: Enable quicktune system + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO_FLAGS("ENABLE", 1, AP_Quicktune, enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: AXES + // @DisplayName: Quicktune axes + // @Description: Axes to tune + // @Bitmask: 0:Roll,1:Pitch,2:Yaw + // @User: Standard + AP_GROUPINFO("AXES", 2, AP_Quicktune, axes_enabled, 7), + + // @Param: DOUBLE_TIME + // @DisplayName: Quicktune doubling time + // @Description: Time to double a tuning parameter. Raise this for a slower tune. + // @Range: 5 20 + // @Units: s + // @User: Standard + AP_GROUPINFO("DOUBLE_TIME", 3, AP_Quicktune, double_time, 10), + + // @Param: GAIN_MARGIN + // @DisplayName: Quicktune gain margin + // @Description: Reduction in gain after oscillation detected. Raise this number to get a more conservative tune + // @Range: 20 80 + // @Units: % + // @User: Standard + AP_GROUPINFO("GAIN_MARGIN", 4, AP_Quicktune, gain_margin, 60), + + // @Param: OSC_SMAX + // @DisplayName: Quicktune oscillation rate threshold + // @Description: Threshold for oscillation detection. A lower value will lead to a more conservative tune. + // @Range: 1 10 + // @User: Standard + AP_GROUPINFO("OSC_SMAX", 5, AP_Quicktune, osc_smax, 4), + + // @Param: YAW_P_MAX + // @DisplayName: Quicktune Yaw P max + // @Description: Maximum value for yaw P gain + // @Range: 0.1 3 + // @User: Standard + AP_GROUPINFO("YAW_P_MAX", 6, AP_Quicktune, yaw_p_max, 0.5), + + // @Param: YAW_D_MAX + // @DisplayName: Quicktune Yaw D max + // @Description: Maximum value for yaw D gain + // @Range: 0.001 1 + // @User: Standard + AP_GROUPINFO("YAW_D_MAX", 7, AP_Quicktune, yaw_d_max, 0.01), + + // @Param: RP_PI_RATIO + // @DisplayName: Quicktune roll/pitch PI ratio + // @Description: Ratio between P and I gains for roll and pitch. Raise this to get a lower I gain + // @Range: 1.0 2.0 + // @User: Standard + AP_GROUPINFO("RP_PI_RATIO", 8, AP_Quicktune, rp_pi_ratio, 1.0), + + // @Param: Y_PI_RATIO + // @DisplayName: Quicktune Yaw PI ratio + // @Description: Ratio between P and I gains for yaw. Raise this to get a lower I gain + // @Range: 1.0 20 + // @User: Standard + AP_GROUPINFO("Y_PI_RATIO", 9, AP_Quicktune, y_pi_ratio, 10), + + // @Param: AUTO_FILTER + // @DisplayName: Quicktune auto filter enable + // @Description: When enabled the PID filter settings are automatically set based on INS_GYRO_FILTER + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("AUTO_FILTER", 10, AP_Quicktune, auto_filter, 1), + + // @Param: AUTO_SAVE + // @DisplayName: Quicktune auto save + // @Description: Number of seconds after completion of tune to auto-save. This is useful when using a 2 position switch for quicktune. Zero (the default value) disables automatic saving, and you will need to have a 3 position switch to save or use GCS auxilliary functions. + // @Units: s + // @User: Standard + AP_GROUPINFO("AUTO_SAVE", 11, AP_Quicktune, auto_save, 0), + + // @Param: REDUCE_MAX + // @DisplayName: Quicktune maximum gain reduction + // @Description: This controls how much quicktune is allowed to lower gains from the original gains. If the vehicle already has a reasonable tune and is not oscillating then you can set this to zero to prevent gain reductions. The default of 20% is reasonable for most vehicles. Using a maximum gain reduction lowers the chance of an angle P oscillation happening if quicktune gets a false positive oscillation at a low gain, which can result in very low rate gains and a dangerous angle P oscillation. + // @Units: % + // @Range: 0 100 + // @User: Standard + AP_GROUPINFO("REDUCE_MAX", 12, AP_Quicktune, reduce_max, 20), + + // @Param: OPTIONS + // @DisplayName: Quicktune options + // @Description: Additional options. When the Two Position Switch option is enabled then a high switch position will start the tune, low will disable the tune. you should also set a QUIK_AUTO_SAVE time so that you will be able to save the tune. + // @Bitmask: 0:UseTwoPositionSwitch + // @User: Standard + AP_GROUPINFO("OPTIONS", 13, AP_Quicktune, options, 0), + + // @Param: ANGLE_MAX + // @DisplayName: maximum angle error for tune abort + // @Description: If while tuning the angle error goes over this limit then the tune will aborts to prevent a bad oscillation in the case of the tuning algorithm failing. If you get an error "Quicktune: attitude error ABORTING" and you think it is a false positive then you can either raise this parameter or you can try increasing the QWIK_DOUBLE_TIME to do the tune more slowly. + // @Units: deg + // @User: Standard + AP_GROUPINFO("ANGLE_MAX", 14, AP_Quicktune, angle_max, 10), + + AP_GROUPEND +}; + +// Call at loop rate +void AP_Quicktune::update(bool mode_supports_quicktune) +{ + if (enable < 1) { + if (need_restore) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "QuickTune disabled"); + abort_tune(); + } + return; + } + const uint32_t now = AP_HAL::millis(); + + if (!mode_supports_quicktune) { + /* + user has switched to a non-quicktune mode. If we have + pending parameter changes then revert + */ + if (need_restore) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "QuickTune aborted"); + } + abort_tune(); + return; + } + + if (need_restore) { + const float att_error = AC_AttitudeControl::get_singleton()->get_att_error_angle_deg(); + if (angle_max > 0 && att_error > angle_max) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Quicktune: attitude error %.1fdeg - ABORTING", att_error); + abort_tune(); + return; + } + } + + if (have_pilot_input()) { + last_pilot_input_ms = now; + } + + SwitchPos sw_pos_tune = SwitchPos::MID; + SwitchPos sw_pos_save = SwitchPos::HIGH; + if ((options & OPTIONS_TWO_POSITION) != 0) { + sw_pos_tune = SwitchPos::HIGH; + sw_pos_save = SwitchPos::NONE; + } + + const auto &vehicle = *AP::vehicle(); + + if (sw_pos == sw_pos_tune && + (!hal.util->get_soft_armed() || !vehicle.get_likely_flying())) { + if (now - last_warning_ms > 5000) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Quicktune: Must be flying to tune"); + last_warning_ms = now; + } + return; + } + if (sw_pos == SwitchPos::LOW || !hal.util->get_soft_armed() || !vehicle.get_likely_flying()) { + // Abort, revert parameters + if (need_restore) { + need_restore = false; + restore_all_params(); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Quicktune: Reverted"); + tune_done_time_ms = 0; + } + reset_axes_done(); + return; + } + if (sw_pos == sw_pos_save) { + // Save all params + if (need_restore) { + need_restore = false; + save_all_params(); + } + } + if (sw_pos != sw_pos_tune) { + return; + } + + if (now - last_stage_change_ms < STAGE_DELAY) { + // Update slew gain + if (slew_parm != Param::END) { + const float P = get_param_value(slew_parm); + const AxisName axis = get_axis(slew_parm); + adjust_gain(slew_parm, P+slew_delta); + slew_steps = slew_steps - 1; + Write_QWIK(get_slew_rate(axis), P, slew_parm); + if (slew_steps == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %.4f", get_param_name(slew_parm), P); + slew_parm = Param::END; + if (get_current_axis() == AxisName::DONE) { + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: DONE"); + tune_done_time_ms = now; + } + } + } + return; + } + + const AxisName axis = get_current_axis(); + + if (axis == AxisName::DONE) { + // Nothing left to do, check autosave time + if (tune_done_time_ms != 0 && auto_save > 0) { + if (now - tune_done_time_ms > (auto_save*1000)) { + need_restore = false; + save_all_params(); + tune_done_time_ms = 0; + } + } + return; + } + + if (!need_restore) { + need_restore = true; + // We are just starting tuning, get current values + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: Starting tune"); + // Get all params + for (uint8_t p = 0; p < uint8_t(Param::END); p++) { + param_saved[p] = get_param_value(Param(p)); + } + // Set up SMAX + const Param is[3] { Param::RLL_SMAX, Param::PIT_SMAX, Param::YAW_SMAX }; + for (const auto p : is) { + const float smax = get_param_value(p); + if (smax <= 0) { + adjust_gain(p, DEFAULT_SMAX); + } + } + } + + if (now - last_pilot_input_ms < PILOT_INPUT_DELAY) { + return; + } + + if (!BIT_IS_SET(filters_done, uint8_t(axis))) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Starting %s tune", get_axis_name(axis)); + setup_filters(axis); + } + + const Param pname = get_pname(axis, current_stage); + const float pval = get_param_value(pname); + const float limit = gain_limit(pname); + const bool limited = (limit > 0.0 && pval >= limit); + const float srate = get_slew_rate(axis); + const bool oscillating = srate > osc_smax; + + // Check if reached limit + if (limited || oscillating) { + float reduction = (100.0-gain_margin)*0.01; + if (!oscillating) { + reduction = 1.0; + } + float new_gain = pval * reduction; + if (limit > 0.0 && new_gain > limit) { + new_gain = limit; + } + float old_gain = param_saved[uint8_t(pname)]; + if (new_gain < old_gain && (pname == Param::PIT_D || pname == Param::RLL_D)) { + // We are lowering a D gain from the original gain. Also + // lower the P gain by the same amount so that we don't + // trigger P oscillation. We don't drop P by more than a + // factor of 2 + const float ratio = fmaxf(new_gain / old_gain, 0.5); + const uint8_t P_TO_D_OFS = uint8_t(Param::RLL_D) - uint8_t(Param::RLL_P); + const Param P_name = Param(uint8_t(pname)-P_TO_D_OFS); //from D to P + const float old_pval = get_param_value(P_name);; + const float new_pval = old_pval * ratio; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Adjusting %s %.3f -> %.3f", get_param_name(P_name), old_pval, new_pval); + adjust_gain_limited(P_name, new_pval); + } + // Set up slew gain + slew_parm = pname; + const float slew_target = limit_gain(pname, new_gain); + slew_steps = UPDATE_RATE_HZ/2; + slew_delta = (slew_target - get_param_value(pname)) / slew_steps; + + Write_QWIK(srate, pval, pname); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Quicktune: %s done", get_param_name(pname)); + advance_stage(axis); + last_stage_change_ms = now; + } else { + float new_gain = pval*get_gain_mul(); + // cope with the gain starting at zero (some users with have a zero D gain) + new_gain = MAX(new_gain, 0.0001); + adjust_gain_limited(pname, new_gain); + Write_QWIK(srate, pval, pname); + if (now - last_gain_report_ms > 3000U) { + last_gain_report_ms = now; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %.4f sr:%.2f", get_param_name(pname), new_gain, srate); + } + } +} + +/* + abort the tune if it has started + */ +void AP_Quicktune::abort_tune() +{ + if (need_restore) { + need_restore = false; + restore_all_params(); + } + tune_done_time_ms = 0; + reset_axes_done(); + sw_pos = SwitchPos::LOW; +} + +void AP_Quicktune::update_switch_pos(const RC_Channel::AuxSwitchPos ch_flag) +{ + sw_pos = SwitchPos(ch_flag); +} + +void AP_Quicktune::reset_axes_done() +{ + axes_done = 0; + filters_done = 0; + current_stage = Stage::D; +} + +void AP_Quicktune::setup_filters(AP_Quicktune::AxisName axis) +{ + if (auto_filter <= 0) { + BIT_SET(filters_done, uint8_t(axis)); + return; + } + AP_InertialSensor *imu = AP_InertialSensor::get_singleton(); + if (imu == nullptr) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + const float gyro_filter = imu->get_gyro_filter_hz(); + adjust_gain(get_pname(axis, Stage::FLTT), gyro_filter * FLTT_MUL); + adjust_gain(get_pname(axis, Stage::FLTD), gyro_filter * FLTD_MUL); + + if (axis == AxisName::YAW) { + const float FLTE = get_param_value(Param::YAW_FLTE); + if (FLTE < 0.0 || FLTE > YAW_FLTE_MAX) { + adjust_gain(Param::YAW_FLTE, YAW_FLTE_MAX); + } + } + BIT_SET(filters_done, uint8_t(axis)); +} + +// Check for pilot input to pause tune +bool AP_Quicktune::have_pilot_input() const +{ + auto &RC = rc(); + const float roll = RC.get_roll_channel().norm_input_dz(); + const float pitch = RC.get_pitch_channel().norm_input_dz(); + const float yaw = RC.get_yaw_channel().norm_input_dz(); + + if (fabsf(roll) > 0 || fabsf(pitch) > 0 || fabsf(yaw) > 0) { + return true; + } + return false; +} + +// Get the axis name we are working on, or DONE for all done +AP_Quicktune::AxisName AP_Quicktune::get_current_axis() const +{ + for (int8_t i = 0; i < int8_t(AxisName::DONE); i++) { + if (BIT_IS_SET(axes_enabled, i) && !BIT_IS_SET(axes_done, i)) { + return AxisName(i); + } + } + return AxisName::DONE; +} + +// get the AC_PID object for an axis +AC_PID *AP_Quicktune::get_axis_pid(AP_Quicktune::AxisName axis) const +{ + auto &attitude_control = *AC_AttitudeControl::get_singleton(); + switch(axis) { + case AxisName::RLL: + return &attitude_control.get_rate_roll_pid(); + case AxisName::PIT: + return &attitude_control.get_rate_pitch_pid(); + case AxisName::YAW: + return &attitude_control.get_rate_yaw_pid(); + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } + return nullptr; +} + +// get slew rate parameter for an axis +float AP_Quicktune::get_slew_rate(AP_Quicktune::AxisName axis) const +{ + auto *pid = get_axis_pid(axis); + if (pid == nullptr) { + return 0; + } + return pid->get_pid_info().slew_rate; +} + +// Move to next stage of tune +void AP_Quicktune::advance_stage(AP_Quicktune::AxisName axis) +{ + if (current_stage == Stage::D) { + current_stage = Stage::P; + } else { + BIT_SET(axes_done, uint8_t(axis)); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: %s done", get_axis_name(axis)); + current_stage = Stage::D; + } +} + +void AP_Quicktune::adjust_gain(AP_Quicktune::Param param, float value) +{ + need_restore = true; + BIT_SET(param_changed, uint8_t(param)); + set_param_value(param, value); + + if (get_stage(param) == Stage::P) { + // Also change I gain + const uint8_t P_TO_I_OFS = uint8_t(Param::RLL_I) - uint8_t(Param::RLL_P); + const uint8_t P_TO_FF_OFS = uint8_t(Param::RLL_FF) - uint8_t(Param::RLL_P); + const Param iname = Param(uint8_t(param)+P_TO_I_OFS); + const Param ffname = Param(uint8_t(param)+P_TO_FF_OFS); + float FF = get_param_value(ffname); + if (FF > 0) { + // If we have any FF on an axis then we don't couple I to P, + // usually we want I = FF for a one second time constant for trim + return; + } + BIT_SET(param_changed, uint8_t(iname)); + + // Work out ratio of P to I that we want + float pi_ratio = rp_pi_ratio; + if (get_axis(param) == AxisName::YAW) { + pi_ratio = y_pi_ratio; + } + if (pi_ratio >= 1) { + set_param_value(iname, value/pi_ratio); + } + } + +} + +void AP_Quicktune::adjust_gain_limited(AP_Quicktune::Param param, float value) +{ + adjust_gain(param, limit_gain(param, value)); +} + +float AP_Quicktune::limit_gain(AP_Quicktune::Param param, float value) +{ + const float saved_value = param_saved[uint8_t(param)]; + if (reduce_max >= 0 && reduce_max < 100 && saved_value > 0) { + // Check if we exceeded gain reduction + const float reduction_pct = 100.0 * (saved_value - value) / saved_value; + if (reduction_pct > reduce_max) { + const float new_value = saved_value * (100 - reduce_max) * 0.01; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Limiting %s %.3f -> %.3f", get_param_name(param), value, new_value); + value = new_value; + } + } + return value; +} + +const char* AP_Quicktune::get_param_name(AP_Quicktune::Param param) const +{ + switch (param) + { + case Param::RLL_P: + return "Roll P"; + case Param::RLL_I: + return "Roll I"; + case Param::RLL_D: + return "Roll D"; + case Param::PIT_P: + return "Pitch P"; + case Param::PIT_I: + return "Pitch I"; + case Param::PIT_D: + return "Pitch D"; + case Param::YAW_P: + return "Yaw P"; + case Param::YAW_I: + return "Yaw I"; + case Param::YAW_D: + return "Yaw D"; + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return "UNK"; + } +} + +float AP_Quicktune::get_gain_mul() const +{ + return expf(logf(2.0)/(UPDATE_RATE_HZ*MAX(1,double_time))); +} + +void AP_Quicktune::restore_all_params() +{ + for (uint8_t p = 0; p < uint8_t(Param::END); p++) { + const auto param = Param(p); + if (BIT_IS_SET(param_changed, p)) { + set_param_value(param, param_saved[p]); + BIT_CLEAR(param_changed, p); + } + } +} + +void AP_Quicktune::save_all_params() +{ + for (uint8_t p = 0; p < uint8_t(Param::END); p++) { + const auto param = Param(p); + set_and_save_param_value(param, get_param_value(param)); + param_saved[p] = get_param_value(param); + BIT_CLEAR(param_changed, p); + } + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: Saved"); +} + +AP_Quicktune::Param AP_Quicktune::get_pname(AP_Quicktune::AxisName axis, AP_Quicktune::Stage stage) const +{ + const uint8_t axis_offset = uint8_t(axis) * param_per_axis; + switch (stage) + { + case Stage::P: + return Param(uint8_t(Param::RLL_P) + axis_offset); + case Stage::D: + return Param(uint8_t(Param::RLL_D) + axis_offset); + case Stage::FLTT: + return Param(uint8_t(Param::RLL_FLTT) + axis_offset); + case Stage::FLTD: + return Param(uint8_t(Param::RLL_FLTD) + axis_offset); + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return Param::END; + } +} + +AP_Quicktune::Stage AP_Quicktune::get_stage(AP_Quicktune::Param param) const +{ + if (param == Param::RLL_P || param == Param::PIT_P || param == Param::YAW_P) { + return Stage::P; + } else { + return Stage::END; //Means "anything but P gain" + } +} + +AP_Float *AP_Quicktune::get_param_pointer(AP_Quicktune::Param param) const +{ + const AxisName axis = get_axis(param); + auto *pid = get_axis_pid(axis); + if (pid == nullptr) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return nullptr; + } + const Param roll_param = Param(uint8_t(param) % param_per_axis); + switch (roll_param) + { + case Param::RLL_P: + return &pid->kP(); + case Param::RLL_I: + return &pid->kI(); + case Param::RLL_D: + return &pid->kD(); + case Param::RLL_SMAX: + return &pid->slew_limit(); + case Param::RLL_FLTT: + return &pid->filt_T_hz(); + case Param::RLL_FLTD: + return &pid->filt_D_hz(); + case Param::RLL_FLTE: + return &pid->filt_E_hz(); + case Param::RLL_FF: + return &pid->ff(); + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return nullptr; + } +} + +float AP_Quicktune::get_param_value(AP_Quicktune::Param param) const +{ + AP_Float *ptr = get_param_pointer(param); + if (ptr != nullptr) { + return ptr->get(); + } + return 0.0; +} + +void AP_Quicktune::set_param_value(AP_Quicktune::Param param, float value) +{ + AP_Float *ptr = get_param_pointer(param); + if (ptr != nullptr) { + ptr->set(value); + } +} + +void AP_Quicktune::set_and_save_param_value(AP_Quicktune::Param param, float value) +{ + AP_Float *ptr = get_param_pointer(param); + if (ptr != nullptr) { + ptr->set_and_save(value); + } +} + +AP_Quicktune::AxisName AP_Quicktune::get_axis(AP_Quicktune::Param param) const +{ + if (param >= Param::END) { + return AxisName::END; + } + return AxisName(uint8_t(param) / param_per_axis); +} + +const char* AP_Quicktune::get_axis_name(AP_Quicktune::AxisName axis) const +{ + switch (axis) + { + case AxisName::RLL: + return "Roll"; + case AxisName::PIT: + return "Pitch"; + case AxisName::YAW: + return "Yaw"; + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return "UNK"; + } +} + +float AP_Quicktune::gain_limit(AP_Quicktune::Param param) const +{ + if (get_axis(param) == AxisName::YAW) { + if (param == Param::YAW_P) { + return yaw_p_max; + } + if (param == Param::YAW_D) { + return yaw_d_max; + } + } + return 0.0; +} + + +// @LoggerMessage: QWIK +// @Description: Quicktune +// @Field: TimeUS: Time since system startup +// @Field: ParamNo: number of parameter being tuned +// @Field: SRate: slew rate +// @Field: Gain: test gain for current axis and PID element +// @Field: Param: name of parameter being being tuned +void AP_Quicktune::Write_QWIK(float srate, float gain, AP_Quicktune::Param param) +{ +#if HAL_LOGGING_ENABLED + AP::logger().WriteStreaming("QWIK","TimeUS,ParamNo,SRate,Gain,Param", "s#---", "-----", "QBffN", + AP_HAL::micros64(), + unsigned(param), + srate, + gain, + get_param_name(param)); +#endif +} + +#endif //AP_QUICKTUNE_ENABLED diff --git a/libraries/AP_Quicktune/AP_Quicktune.h b/libraries/AP_Quicktune/AP_Quicktune.h new file mode 100644 index 0000000000000..5c495a9e010ac --- /dev/null +++ b/libraries/AP_Quicktune/AP_Quicktune.h @@ -0,0 +1,167 @@ +#pragma once + +#include + +#ifndef AP_QUICKTUNE_ENABLED +#define AP_QUICKTUNE_ENABLED 1 // NOTE: may be disabled by vehicle header +#endif + +#if AP_QUICKTUNE_ENABLED + +#include +#include +#include + +class AP_Quicktune { +public: + AP_Quicktune() + { + AP_Param::setup_object_defaults(this, var_info); + } + + // Empty destructor to suppress compiler warning + virtual ~AP_Quicktune() {} + + /* Do not allow copies */ + CLASS_NO_COPY(AP_Quicktune); + + // Parameter block + static const struct AP_Param::GroupInfo var_info[]; + + void update(bool mode_supports_quicktune); + void update_switch_pos(const RC_Channel::AuxSwitchPos ch_flag); + +private: + + // Parameters + AP_Int8 enable; + AP_Int8 axes_enabled; + AP_Float double_time; + AP_Float gain_margin; + AP_Float osc_smax; + AP_Float yaw_p_max; + AP_Float yaw_d_max; + AP_Float rp_pi_ratio; + AP_Float y_pi_ratio; + AP_Int8 auto_filter; + AP_Float auto_save; + AP_Float reduce_max; + AP_Int16 options; + AP_Int8 angle_max; + + // Low, Mid and High must be in the same positions as they are in RC_Channel::AuxSwitchPos + enum class SwitchPos : uint8_t { + LOW, + MID, + HIGH, + NONE, + }; + + + enum class AxisName : uint8_t { + RLL = 0, + PIT, + YAW, + DONE, + END, + }; + + /* + note! we rely on the enum being in the same order between axes + */ + enum class Param : uint8_t { + RLL_P = 0, + RLL_I, + RLL_D, + RLL_SMAX, + RLL_FLTT, + RLL_FLTD, + RLL_FLTE, + RLL_FF, + + PIT_P, + PIT_I, + PIT_D, + PIT_SMAX, + PIT_FLTT, + PIT_FLTD, + PIT_FLTE, + PIT_FF, + + YAW_P, + YAW_I, + YAW_D, + YAW_SMAX, + YAW_FLTT, + YAW_FLTD, + YAW_FLTE, + YAW_FF, + END, + }; + + static const uint8_t param_per_axis = uint8_t(Param::PIT_P) - uint8_t(Param::RLL_P); + static_assert(uint8_t(Param::END) == 3*param_per_axis, "AP_Quicktune Param error"); + + // Also the gains + enum class Stage : uint8_t { + D, + P, + DONE, + I, + FF, + SMAX, + FLTT, + FLTD, + FLTE, + END, + }; + + // Time keeping + uint32_t last_stage_change_ms; + uint32_t last_gain_report_ms; + uint32_t last_pilot_input_ms; + uint32_t last_warning_ms; + uint32_t tune_done_time_ms; + + // Bitmasks + uint32_t axes_done; + uint32_t filters_done; + uint32_t param_changed; //Bitmask of changed parameters + + Stage current_stage = Stage::D; + Param slew_parm = Param::END; + uint8_t slew_steps; + float slew_delta; + SwitchPos sw_pos; //Switch pos to be set by aux func + bool need_restore; + float param_saved[uint8_t(Param::END)]; //Saved values of the parameters + + void reset_axes_done(); + void setup_filters(AxisName axis); + bool have_pilot_input() const; + AxisName get_current_axis() const; + float get_slew_rate(AxisName axis) const; + void advance_stage(AxisName axis); + void adjust_gain(Param param, float value); + void adjust_gain_limited(Param param, float value); + float get_gain_mul() const; + void restore_all_params(); + void save_all_params(); + Param get_pname(AxisName axis, Stage stage) const; + AP_Float *get_param_pointer(Param param) const; + float get_param_value(Param param) const; + void set_param_value(Param param, float value); + void set_and_save_param_value(Param param, float value); + float gain_limit(Param param) const; + AxisName get_axis(Param param) const; + float limit_gain(Param param, float value); + const char* get_param_name(Param param) const; + Stage get_stage(Param param) const; + const char* get_axis_name(AxisName axis) const; + AC_PID *get_axis_pid(AxisName axis) const; + void Write_QWIK(float SRate, float Gain, Param param); + + void abort_tune(void); +}; + +#endif // AP_QUICKTUNE_ENABLED diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 2868030a828be..7194fa092b80e 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -260,6 +260,7 @@ class RC_Channel { FLIGHTMODE_PAUSE = 178, // e.g. pause movement towards waypoint ICE_START_STOP = 179, // AP_ICEngine start stop AUTOTUNE_TEST_GAINS = 180, // auto tune tuning switch to test or revert gains + QUICKTUNE = 181, //quicktune 3 position switch // inputs from 200 will eventually used to replace RCMAP