From 808c92088f97bf86f4dc992726bbda7d42cbce5f Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Sat, 27 Jul 2024 15:52:11 +1000 Subject: [PATCH 01/14] Quicktune: Added Quicktune Thank you Tridge for the help. --- libraries/AP_Quicktune/AP_Quicktune.cpp | 728 ++++++++++++++++++++++++ libraries/AP_Quicktune/AP_Quicktune.h | 158 +++++ 2 files changed, 886 insertions(+) create mode 100644 libraries/AP_Quicktune/AP_Quicktune.cpp create mode 100644 libraries/AP_Quicktune/AP_Quicktune.h diff --git a/libraries/AP_Quicktune/AP_Quicktune.cpp b/libraries/AP_Quicktune/AP_Quicktune.cpp new file mode 100644 index 0000000000000..874d6b02e6bc4 --- /dev/null +++ b/libraries/AP_Quicktune/AP_Quicktune.cpp @@ -0,0 +1,728 @@ +#include "AP_Quicktune.h" + +#if AP_QUICKTUNE_ENABLED + +#define UPDATE_RATE_HZ 40 +#define UPDATE_PERIOD_MS (1000U/UPDATE_RATE_HZ) +#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) + +/* + if while tuning the attitude error goes over 25 degrees then abort + the tune + */ +#define MAX_ATTITUDE_ERROR 25.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, 5), + + // @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: 0.5 1.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: 0.5 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 + // @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), + + 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_EMERGENCY, "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_EMERGENCY, "QuickTune aborted"); + } + abort_tune(); + return; + } + + if (need_restore) { + const float att_error = AC_AttitudeControl::get_singleton()->get_att_error_angle_deg(); + if (att_error > MAX_ATTITUDE_ERROR) { + GCS_SEND_TEXT(MAV_SEVERITY_EMERGENCY, "Tuning: attitude error %.1fdeg - ABORTING", att_error); + abort_tune(); + return; + } + } + + if (have_pilot_input()) { + last_pilot_input = 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()) && now > last_warning + 5000) { + GCS_SEND_TEXT(MAV_SEVERITY_EMERGENCY, "Tuning: Must be flying to tune"); + last_warning = 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_EMERGENCY, "Tuning: Reverted"); + tune_done_time = 0; + } + reset_axes_done(); + return; + } + if (sw_pos == sw_pos_save) { + // Save all params + if (need_restore) { + need_restore = false; + save_all_params(); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Tuning: Saved"); + } + } + if (sw_pos != sw_pos_tune) { + return; + } + + if (now - last_stage_change < STAGE_DELAY) { + // Update slew gain + if (slew_parm != Param::END) { + float P = get_param_value(slew_parm); + AxisName axis = get_axis(slew_parm); + // local ax_stage = string.sub(slew_parm, -1) + adjust_gain(slew_parm, P+slew_delta); + slew_steps = slew_steps - 1; + Write_QUIK(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, "Tuning: DONE"); + tune_done_time = now; + } + } + } + return; + } + + const AxisName axis = get_current_axis(); + + if (axis == AxisName::DONE) { + // Nothing left to do, check autosave time + if (tune_done_time != 0 && auto_save > 0) { + if (now - tune_done_time > (auto_save*1000)) { + need_restore = false; + save_all_params(); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Tuning: Saved"); + tune_done_time = 0; + } + } + return; + } + + if (!need_restore) { + need_restore = true; + // We are just starting tuning, get current values + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Tuning: Starting tune"); + // Get all params + for (int8_t pname = 0; pname < uint8_t(Param::END); pname++) { + param_saved[pname] = get_param_value(Param(pname)); + } + // Set up SMAX + Param is[3]; + is[0] = Param::RLL_SMAX; + is[1] = Param::PIT_SMAX; + is[2] = Param::YAW_SMAX; + for (uint8_t i = 0; i < 3; i++) { + float smax = get_param_value(is[i]); + if (smax <= 0) { + adjust_gain(is[i], DEFAULT_SMAX); + } + } + } + + if (now - last_pilot_input < 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); + } + + Param pname = get_pname(axis, current_stage); + float pval = get_param_value(pname); + float limit = gain_limit(pname); + bool limited = (limit > 0.0 && pval >= limit); + float srate = get_slew_rate(axis); + 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 + float ratio = fmaxf(new_gain / old_gain, 0.5); + Param P_name = Param(uint8_t(pname)-2); //from D to P + float old_pval = get_param_value(P_name);; + 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; + slew_target = limit_gain(pname, new_gain); + slew_steps = UPDATE_RATE_HZ/2; + slew_delta = (slew_target - get_param_value(pname)) / slew_steps; + + Write_QUIK(srate, pval, pname); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Tuning: %s done", get_param_name(pname)); + advance_stage(axis); + last_stage_change = now; + } else { + float new_gain = pval*get_gain_mul(); + if (new_gain <= 0.0001) { + new_gain = 0.001; + } + adjust_gain_limited(pname, new_gain); + Write_QUIK(srate, pval, pname); + if (now - last_gain_report > 3000) { + last_gain_report = 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 = 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)); + } + AP_InertialSensor *imu = AP_InertialSensor::get_singleton(); + if (imu == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_EMERGENCY, "Quicktune: can't find IMU."); + return; + } + 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 * FLTT_MUL); + + if (axis == AxisName::YAW) { + 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 &rcmap = *AP::rcmap(); + float roll = rc().rc_channel(rcmap.roll()-1)->norm_input_dz(); + float pitch = rc().rc_channel(rcmap.pitch()-1)->norm_input_dz(); + float yaw = rc().rc_channel(rcmap.yaw()-1)->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() +{ + for (int8_t i = 0; i < int8_t(AxisName::DONE); i++) { + if (BIT_IS_SET(axes_enabled, i) == true && BIT_IS_SET(axes_done, i) == false) { + return AxisName(i); + } + } + return AxisName::DONE; +} + +float AP_Quicktune::get_slew_rate(AP_Quicktune::AxisName axis) +{ + auto &attitude_control = *AC_AttitudeControl::get_singleton(); + switch(axis) { + case AxisName::RLL: + return attitude_control.get_rate_roll_pid().get_pid_info().slew_rate; + case AxisName::PIT: + return attitude_control.get_rate_pitch_pid().get_pid_info().slew_rate; + case AxisName::YAW: + return attitude_control.get_rate_yaw_pid().get_pid_info().slew_rate; + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return 0.0; + } +} + +// 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, "Tuning: %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 + Param iname = Param(uint8_t(param)+1); + Param ffname = Param(uint8_t(param)+7); + 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) +{ + float saved_value = param_saved[uint8_t(param)]; + if (reduce_max >= 0 && reduce_max < 100 && saved_value > 0) { + // Check if we exceeded gain reduction + float reduction_pct = 100.0 * (saved_value - value) / saved_value; + if (reduction_pct > reduce_max) { + 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) +{ + 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() +{ + return expf(logf(2.0)/(UPDATE_RATE_HZ*double_time)); +} + +void AP_Quicktune::restore_all_params() +{ + for (int8_t pname = 0; pname < uint8_t(Param::END); pname++) { + if (BIT_IS_SET(param_changed, pname)) { + set_param_value(Param(pname), param_saved[pname]); + BIT_CLEAR(param_changed, pname); + } + } +} + +void AP_Quicktune::save_all_params() +{ + // for pname in pairs(params) do + for (int8_t pname = 0; pname < uint8_t(Param::END); pname++) { + if (BIT_IS_SET(param_changed, pname)) { + set_and_save_param_value(Param(pname), get_param_value(Param(pname))); + param_saved[pname] = get_param_value(Param(pname)); + BIT_CLEAR(param_changed, pname); + } + } +} + +AP_Quicktune::Param AP_Quicktune::get_pname(AP_Quicktune::AxisName axis, AP_Quicktune::Stage stage) +{ + switch (axis) + { + case AxisName::RLL: + switch (stage) + { + case Stage::P: + return Param::RLL_P; + case Stage::D: + return Param::RLL_D; + case Stage::FLTT: + return Param::RLL_FLTT; + case Stage::FLTD: + return Param::RLL_FLTD; + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return Param::END; + } + case AxisName::PIT: + switch (stage) + { + case Stage::P: + return Param::PIT_P; + case Stage::D: + return Param::PIT_D; + case Stage::FLTT: + return Param::PIT_FLTT; + case Stage::FLTD: + return Param::PIT_FLTD; + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return Param::END; + } + case AxisName::YAW: + switch (stage) + { + case Stage::P: + return Param::YAW_P; + case Stage::D: + return Param::YAW_D; + case Stage::FLTT: + return Param::YAW_FLTT; + case Stage::FLTD: + return Param::YAW_FLTD; + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return Param::END; + } + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return Param::END; + } +} + +AP_Quicktune::Stage AP_Quicktune::get_stage(AP_Quicktune::Param param) +{ + 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) +{ + auto &attitude_control = *AC_AttitudeControl::get_singleton(); + switch (param) + { + case Param::RLL_P: + return &attitude_control.get_rate_roll_pid().kP(); + case Param::RLL_I: + return &attitude_control.get_rate_roll_pid().kI(); + case Param::RLL_D: + return &attitude_control.get_rate_roll_pid().kD(); + case Param::RLL_SMAX: + return &attitude_control.get_rate_roll_pid().slew_limit(); + case Param::RLL_FLTT: + return &attitude_control.get_rate_roll_pid().filt_T_hz(); + case Param::RLL_FLTD: + return &attitude_control.get_rate_roll_pid().filt_D_hz(); + case Param::RLL_FLTE: + return &attitude_control.get_rate_roll_pid().filt_E_hz(); + case Param::RLL_FF: + return &attitude_control.get_rate_roll_pid().ff(); + case Param::PIT_P: + return &attitude_control.get_rate_pitch_pid().kP(); + case Param::PIT_I: + return &attitude_control.get_rate_pitch_pid().kI(); + case Param::PIT_D: + return &attitude_control.get_rate_pitch_pid().kD(); + case Param::PIT_SMAX: + return &attitude_control.get_rate_pitch_pid().slew_limit(); + case Param::PIT_FLTT: + return &attitude_control.get_rate_pitch_pid().filt_T_hz(); + case Param::PIT_FLTD: + return &attitude_control.get_rate_pitch_pid().filt_D_hz(); + case Param::PIT_FLTE: + return &attitude_control.get_rate_pitch_pid().filt_E_hz(); + case Param::PIT_FF: + return &attitude_control.get_rate_pitch_pid().ff(); + case Param::YAW_P: + return &attitude_control.get_rate_yaw_pid().kP(); + case Param::YAW_I: + return &attitude_control.get_rate_yaw_pid().kI(); + case Param::YAW_D: + return &attitude_control.get_rate_yaw_pid().kD(); + case Param::YAW_SMAX: + return &attitude_control.get_rate_yaw_pid().slew_limit(); + case Param::YAW_FLTT: + return &attitude_control.get_rate_yaw_pid().filt_T_hz(); + case Param::YAW_FLTD: + return &attitude_control.get_rate_yaw_pid().filt_D_hz(); + case Param::YAW_FLTE: + return &attitude_control.get_rate_yaw_pid().filt_E_hz(); + case Param::YAW_FF: + return &attitude_control.get_rate_roll_pid().ff(); + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return nullptr; + } +} + +float AP_Quicktune::get_param_value(AP_Quicktune::Param param) +{ + AP_Float *ptr = get_param_pointer(param); + if (ptr != nullptr) { + return ptr->get(); + } + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + 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); + return; + } + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; +} + +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); + return; + } + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; +} + +AP_Quicktune::AxisName AP_Quicktune::get_axis(AP_Quicktune::Param param) +{ + if (param < Param::PIT_P) { + return AxisName::RLL; + } else if (param < Param::YAW_P) { + return AxisName::PIT; + } else if (param < Param::END) { + return AxisName::YAW; + } else { + return AxisName::END; + } +} + +const char* AP_Quicktune::get_axis_name(AP_Quicktune::AxisName axis) +{ + 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) +{ + 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; +} + +void AP_Quicktune::Write_QUIK(float srate, float gain, AP_Quicktune::Param param) +{ +#if HAL_LOGGING_ENABLED + AP::logger().WriteStreaming("QUIK","TimeUS,SRate,Gain,Param,ParamNo", "QffNI", AP_HAL::micros64(), srate, gain, get_param_name(param), int(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..2e3a0edef921f --- /dev/null +++ b/libraries/AP_Quicktune/AP_Quicktune.h @@ -0,0 +1,158 @@ +#pragma once + +#include + +#ifndef AP_QUICKTUNE_ENABLED + #define AP_QUICKTUNE_ENABLED 1 +#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; + + // 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, + PIT, + YAW, + DONE, + END, + }; + + enum class Param : uint8_t { + RLL_P, + 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, + }; + + // 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; + uint32_t last_gain_report; + uint32_t last_pilot_input; + uint32_t last_warning; + uint32_t tune_done_time; + + // 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; + float slew_target; + 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(); + AxisName get_current_axis(); + float get_slew_rate(AxisName axis); + void advance_stage(AxisName axis); + void adjust_gain(Param param, float value); + void adjust_gain_limited(Param param, float value); + float get_gain_mul(); + void restore_all_params(); + void save_all_params(); + Param get_pname(AxisName axis, Stage stage); + AP_Float *get_param_pointer(Param param); + float get_param_value(Param param); + void set_param_value(Param param, float value); + void set_and_save_param_value(Param param, float value); + float gain_limit(Param param); + AxisName get_axis(Param param); + float limit_gain(Param param, float value); + const char* get_param_name(Param param); + Stage get_stage(Param param); + const char* get_axis_name(AxisName axis); + void Write_QUIK(float SRate, float Gain, Param param); + + void abort_tune(void); +}; + +#endif // AP_QUICKTUNE_ENABLED From a3ad815ffc647246e6c29d67b5b97bc02b940477 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Sat, 27 Jul 2024 15:52:11 +1000 Subject: [PATCH 02/14] RC_Channel: Added Quicktune Thank you Tridge for the help. --- libraries/RC_Channel/RC_Channel.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 34dfb90e92638..17a883d15ec3f 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -254,6 +254,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 From 24ed8c043e9c09f4ea3b62b3be397c101c326d2b Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Sat, 27 Jul 2024 15:52:11 +1000 Subject: [PATCH 03/14] ArduCopter: Added Quicktune Thank you Tridge for the help. --- ArduCopter/Copter.cpp | 15 +++++++++++++++ ArduCopter/Copter.h | 14 ++++++++++++++ ArduCopter/Parameters.cpp | 6 ++++++ ArduCopter/Parameters.h | 1 + ArduCopter/RC_Channel.cpp | 7 +++++++ ArduCopter/mode.h | 13 +++++++++++++ ArduCopter/wscript | 3 ++- 7 files changed, 58 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 56f73753a040a..040e696e05ac6 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, @@ -807,6 +810,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 11ea29a1f3b59..c79d549409b59 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -72,6 +72,7 @@ #include #include #include +#include // Configuration #include "defines.h" @@ -83,6 +84,12 @@ #define MOTOR_CLASS AP_MotorsMulticopter #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 + #if MODE_AUTOROTATE_ENABLED == ENABLED #include // Autorotation controllers #endif @@ -487,6 +494,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. @@ -715,6 +726,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 f0e3d6f72535b..2933baee7bb58 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 == ENABLED + // @Group: QUIK_ + // @Path: ../libraries/AP_Quicktune/AP_Quicktune.cpp + GOBJECT(quicktune, "QUIK_", 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 087c6f9e3dad5..4ee3985ededff 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 0ae33562a80df..ddf8a8362e1bc 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -110,6 +110,7 @@ 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: + case AUX_FUNC::QUICKTUNE: break; case AUX_FUNC::ACRO_TRAINER: case AUX_FUNC::ATTCON_ACCEL_LIM: @@ -657,6 +658,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 031a9a0d26a6f..aec29ec9a6873 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -194,6 +194,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 @@ -483,6 +488,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: }; @@ -1277,6 +1286,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', ], ) From 9363c2f2f3f304f3fca4028e7e819dc24463bad6 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Sat, 27 Jul 2024 15:52:11 +1000 Subject: [PATCH 04/14] ArduPlane: Added Quicktune Thank you Tridge for the help. --- ArduPlane/ArduPlane.cpp | 15 +++++++++++++++ ArduPlane/Parameters.cpp | 6 ++++++ ArduPlane/Parameters.h | 1 + ArduPlane/Plane.h | 9 +++++++++ ArduPlane/RC_Channel.cpp | 7 +++++++ ArduPlane/RC_Channel.h | 1 - ArduPlane/mode.h | 16 ++++++++++++++++ ArduPlane/wscript | 1 + 8 files changed, 55 insertions(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index e523b3ea04b80..ba3564811c083 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, @@ -989,4 +992,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 2c4d9cfed6c56..e0d2bbb1709ad 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1023,6 +1023,12 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp PARAM_VEHICLE_INFO, +#if AP_QUICKTUNE_ENABLED == ENABLED + // @Group: QUIK_ + // @Path: ../libraries/AP_Quicktune/AP_Quicktune.cpp + GOBJECT(quicktune, "QUIK_", AP_Quicktune), +#endif + AP_VAREND }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 38801d99631cc..9589cfcab212c 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -359,6 +359,7 @@ class Parameters { k_param_autotune_options, k_param_takeoff_throttle_min, k_param_takeoff_options, + k_param_quicktune, }; AP_Int16 format_version; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c86b9b92cbbbe..4aa0e92d504d9 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -86,6 +86,7 @@ #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_ExternalControl_Plane.h" #endif +#include #include #if AC_PRECLAND_ENABLED @@ -319,6 +320,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; @@ -856,6 +861,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 d674c55806f07..0d335c179d151 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -175,6 +175,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option, #if QAUTOTUNE_ENABLED case AUX_FUNC::AUTOTUNE_TEST_GAINS: #endif + case AUX_FUNC::QUICKTUNE: break; case AUX_FUNC::SOARING: @@ -456,6 +457,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 955a66bbc8b06..7b8d8d0f21d97 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -9,6 +9,7 @@ #include "quadplane.h" #include #include +#include class AC_PosControl; class AC_AttitudeControl_Multi; @@ -141,6 +142,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 @@ -312,6 +318,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; @@ -649,6 +658,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 @@ -675,6 +687,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/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', ], ) From 4375bb152396257ac724c52bb5e81cedeef6ee89 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Wed, 31 Jul 2024 16:26:02 +1000 Subject: [PATCH 05/14] Tools: Added Quicktune Thank you Tridge for the help. --- Tools/autotest/quadplane.py | 39 +++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 0131a21265a09..0ce76f1fa26fb 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1346,6 +1346,44 @@ def VTOLQuicktune(self): self.wait_disarmed(timeout=120) + + def VTOLQuicktune_CPP(self): + '''VTOL Quicktune in C++''' + self.set_parameters({ + "RC7_OPTION": 179, + "QUIK_ENABLE" : 1, + "QUIK_DOUBLE_TIME" : 5, # run faster for autotest + }) + + self.context_push() + self.context_collect('STATUSTEXT') + + self.wait_ready_to_arm() + self.change_mode("QLOITER") + self.arm_vehicle() + self.takeoff(20, 'QLOITER') + + # use rc switch to start tune + self.set_rc(7, 1500) + + self.wait_text("Tuning: starting tune", check_context=True) + for axis in ['Roll', 'Pitch', 'Yaw']: + self.wait_text("Starting %s tune" % axis, check_context=True) + self.wait_text("Tuning: %s D done" % axis, check_context=True, timeout=120) + self.wait_text("Tuning: %s P done" % axis, check_context=True, timeout=120) + self.wait_text("Tuning: %s done" % axis, check_context=True, timeout=120) + self.wait_text("Tuning: Yaw done", check_context=True, timeout=120) + + # to test aux function method, use aux fn for save + self.run_auxfunc(179, 2) + self.wait_text("Tuning: saved", check_context=True) + self.change_mode("QLAND") + + self.wait_disarmed(timeout=120) + self.set_parameter("QUIK_ENABLE", 0) + self.context_pop() + self.reboot_sitl() + def PrecisionLanding(self): '''VTOL precision landing''' @@ -1800,6 +1838,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 From 43a7e0b7a09632877ddaec389bded1bb10b6b1e0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Jul 2024 07:28:13 +1000 Subject: [PATCH 06/14] autotest: fixed whitespace issues was causing CI failuress with flake8 test --- Tools/autotest/quadplane.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 0ce76f1fa26fb..3dad889d6bc20 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1346,7 +1346,6 @@ def VTOLQuicktune(self): self.wait_disarmed(timeout=120) - def VTOLQuicktune_CPP(self): '''VTOL Quicktune in C++''' self.set_parameters({ @@ -1383,7 +1382,7 @@ def VTOLQuicktune_CPP(self): self.set_parameter("QUIK_ENABLE", 0) self.context_pop() self.reboot_sitl() - + def PrecisionLanding(self): '''VTOL precision landing''' From 3840a4855bd02a63c37389eef4dd50797fdcc6e7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Jul 2024 07:39:07 +1000 Subject: [PATCH 07/14] AP_QuickTune: added logger documentation fixes CI test --- libraries/AP_Quicktune/AP_Quicktune.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Quicktune/AP_Quicktune.cpp b/libraries/AP_Quicktune/AP_Quicktune.cpp index 874d6b02e6bc4..78a9096d39b7f 100644 --- a/libraries/AP_Quicktune/AP_Quicktune.cpp +++ b/libraries/AP_Quicktune/AP_Quicktune.cpp @@ -718,10 +718,23 @@ float AP_Quicktune::gain_limit(AP_Quicktune::Param param) return 0.0; } + +// @LoggerMessage: QUIK +// @Description: Quicktune +// @Field: TimeUS: Time since system startup +// @Field: SRate: slew rate +// @Field: Gain: test gain for current axis and PID element +// @Field: Param: name of parameter being being tuned +// @Field: ParamNo: number of parameter being tuned void AP_Quicktune::Write_QUIK(float srate, float gain, AP_Quicktune::Param param) { #if HAL_LOGGING_ENABLED - AP::logger().WriteStreaming("QUIK","TimeUS,SRate,Gain,Param,ParamNo", "QffNI", AP_HAL::micros64(), srate, gain, get_param_name(param), int(param)); + AP::logger().WriteStreaming("QUIK","TimeUS,SRate,Gain,Param,ParamNo", "QffNI", + AP_HAL::micros64(), + srate, + gain, + get_param_name(param), + unsigned(param)); #endif } From 34f5933386f7eff48663fe537e4333387d9c7f62 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Wed, 31 Jul 2024 16:42:49 +1000 Subject: [PATCH 08/14] Tools: Add untuned freestyle frame for testing --- Tools/autotest/models/freestyle-untuned.param | 80 +++++++++++++++++++ Tools/autotest/pysim/vehicleinfo.py | 9 +++ 2 files changed, 89 insertions(+) create mode 100644 Tools/autotest/models/freestyle-untuned.param diff --git a/Tools/autotest/models/freestyle-untuned.param b/Tools/autotest/models/freestyle-untuned.param new file mode 100644 index 0000000000000..507202eff2452 --- /dev/null +++ b/Tools/autotest/models/freestyle-untuned.param @@ -0,0 +1,80 @@ +ACRO_OPTIONS 1.000000 +ACRO_RP_EXPO 0.400000 +ACRO_RP_RATE 533.000000 +ACRO_THR_MID 0.300000 +ACRO_TRAINER 0.000000 +ACRO_Y_EXPO 0.400000 +ACRO_Y_RATE 533.000000 +ANGLE_MAX 7000.000000 +ARMING_RUDDER 0.000000 +ATC_ACCEL_P_MAX 160000.000000 +ATC_ACCEL_R_MAX 160000.000000 +ATC_ACCEL_Y_MAX 120000.000000 +ATC_INPUT_TC 0.040000 +ATC_SLEW_YAW 24000.000000 +ATC_THR_MIX_MAN 4.000000 +AUTO_OPTIONS 3.000000 +BATT_CAPACITY 1300.000000 +BATT_OPTIONS 64.000000 +CIRCLE_RATE 200.000000 +DISARM_DELAY 0.000000 +EK3_GLITCH_RAD 0.000000 +FLTMODE_CH 0.000000 +FRAME_CLASS 1.000000 +GCS_PID_MASK 7.000000 +INS_ACCEL_FILTER 15.000000 +INS_FAST_SAMPLE 3.000000 +INS_GYRO_FILTER 120.000000 +INS_HNTC2_ATT 60.000000 +INS_HNTC2_BW 10.000000 +INS_HNTC2_ENABLE 1.000000 +INS_HNTC2_FREQ 90.000000 +INS_HNTC2_HMNCS 15.000000 +INS_HNTC2_MODE 3.000000 +INS_HNTC2_OPTS 22.000000 +INS_HNTC2_REF 1.000000 +LAND_ALT_LOW 700.000000 +LOG_BITMASK 196607.000000 +LOG_DARM_RATEMAX 1.000000 +LOIT_SPEED 4000.000000 +LOIT_ACC_MAX 1000.000000 +LOIT_BRK_ACCEL 500.000000 +LOIT_BRK_JERK 1000.000000 +LOIT_BRK_DELAY 0.250000 +MOT_BAT_VOLT_MAX 25.200000 +MOT_BAT_VOLT_MIN 19.800000 +MOT_SPIN_ARM 0.010000 +MOT_SPIN_MIN 0.010000 +MOT_SPOOL_TIME 0.250000 +MOT_THST_EXPO 0.700000 +MOT_THST_HOVER 0.125000 +PILOT_SPEED_DN 500.000000 +PILOT_SPEED_UP 500.000000 +PILOT_Y_RATE 250.000000 +PSC_ACCZ_FLTD 40.000000 +PSC_ACCZ_FLTT 40.000000 +PSC_ACCZ_P 0.200000 +PSC_JERK_XY 10.000000 +PSC_VELZ_P 3.000000 +RC7_OPTION 0.000000 +RTL_ALT 700.000000 +RTL_LOIT_TIME 2000.000000 +RTL_SPEED 900.000000 +WPNAV_ACCEL 1100.000000 +WPNAV_ACCEL_Z 300.000000 +WPNAV_JERK 6.000000 +WPNAV_SPEED 3000.000000 +WPNAV_SPEED_DN 500.000000 +WPNAV_SPEED_UP 500.000000 + +ATC_RAT_PIT_P 0.010000 +ATC_RAT_PIT_I 0.010000 +ATC_RAT_PIT_D 0.000300 + +ATC_RAT_RLL_P 0.012000 +ATC_RAT_RLL_I 0.012000 +ATC_RAT_RLL_D 0.000200 + +ATC_RAT_YAW_P 0.1500000 +ATC_RAT_YAW_I 0.0150000 +ATC_RAT_YAW_D 0.0020000 diff --git a/Tools/autotest/pysim/vehicleinfo.py b/Tools/autotest/pysim/vehicleinfo.py index ca07c51b8f19d..ec019ff4c718f 100644 --- a/Tools/autotest/pysim/vehicleinfo.py +++ b/Tools/autotest/pysim/vehicleinfo.py @@ -208,6 +208,15 @@ def __init__(self): "models/freestyle.param", ], }, + "freestyle-untuned": { + "model": "X:@ROMFS/models/freestyle.json", + "waf_target": "bin/arducopter", + "default_params_filename": [ + "default_params/copter.parm", + "default_params/copter-X.parm", + "models/freestyle-untuned.param", + ], + }, }, }, "Helicopter": { From 1e5c4532c993b0e95f7416df8b9bb08ffce9713a Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Sat, 3 Aug 2024 21:27:52 +1000 Subject: [PATCH 09/14] AP_Quicktune: Change get_param_pointer() to use two switch statements instead (note, this does not save flash, it actually costs 20 bytes more flash). --- libraries/AP_Quicktune/AP_Quicktune.cpp | 103 ++++++++++++------------ 1 file changed, 53 insertions(+), 50 deletions(-) diff --git a/libraries/AP_Quicktune/AP_Quicktune.cpp b/libraries/AP_Quicktune/AP_Quicktune.cpp index 78a9096d39b7f..7d0aa4097adac 100644 --- a/libraries/AP_Quicktune/AP_Quicktune.cpp +++ b/libraries/AP_Quicktune/AP_Quicktune.cpp @@ -580,64 +580,67 @@ AP_Quicktune::Stage AP_Quicktune::get_stage(AP_Quicktune::Param param) { if (param == Param::RLL_P || param == Param::PIT_P || param == Param::YAW_P) { return Stage::P; + } else if (param == Param::RLL_I || param == Param::PIT_I || param == Param::YAW_I) { + return Stage::I; + } else if (param == Param::RLL_D || param == Param::PIT_D || param == Param::YAW_D) { + return Stage::D; + } else if (param == Param::RLL_SMAX || param == Param::PIT_SMAX || param == Param::YAW_SMAX) { + return Stage::SMAX; + } else if (param == Param::RLL_FLTT || param == Param::PIT_FLTT || param == Param::YAW_FLTT) { + return Stage::FLTT; + } else if (param == Param::RLL_FLTD || param == Param::PIT_FLTD || param == Param::YAW_FLTD) { + return Stage::FLTD; + } else if (param == Param::RLL_FLTE || param == Param::PIT_FLTE || param == Param::YAW_FLTE) { + return Stage::FLTE; + } else if (param == Param::RLL_FF || param == Param::PIT_FF || param == Param::YAW_FF) { + return Stage::FF; } else { - return Stage::END; //Means "anything but P gain" + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return Stage::END; } } AP_Float *AP_Quicktune::get_param_pointer(AP_Quicktune::Param param) { auto &attitude_control = *AC_AttitudeControl::get_singleton(); - switch (param) + AC_PID* pid_ptr; + + AxisName axis = get_axis(param); + switch (axis) { - case Param::RLL_P: - return &attitude_control.get_rate_roll_pid().kP(); - case Param::RLL_I: - return &attitude_control.get_rate_roll_pid().kI(); - case Param::RLL_D: - return &attitude_control.get_rate_roll_pid().kD(); - case Param::RLL_SMAX: - return &attitude_control.get_rate_roll_pid().slew_limit(); - case Param::RLL_FLTT: - return &attitude_control.get_rate_roll_pid().filt_T_hz(); - case Param::RLL_FLTD: - return &attitude_control.get_rate_roll_pid().filt_D_hz(); - case Param::RLL_FLTE: - return &attitude_control.get_rate_roll_pid().filt_E_hz(); - case Param::RLL_FF: - return &attitude_control.get_rate_roll_pid().ff(); - case Param::PIT_P: - return &attitude_control.get_rate_pitch_pid().kP(); - case Param::PIT_I: - return &attitude_control.get_rate_pitch_pid().kI(); - case Param::PIT_D: - return &attitude_control.get_rate_pitch_pid().kD(); - case Param::PIT_SMAX: - return &attitude_control.get_rate_pitch_pid().slew_limit(); - case Param::PIT_FLTT: - return &attitude_control.get_rate_pitch_pid().filt_T_hz(); - case Param::PIT_FLTD: - return &attitude_control.get_rate_pitch_pid().filt_D_hz(); - case Param::PIT_FLTE: - return &attitude_control.get_rate_pitch_pid().filt_E_hz(); - case Param::PIT_FF: - return &attitude_control.get_rate_pitch_pid().ff(); - case Param::YAW_P: - return &attitude_control.get_rate_yaw_pid().kP(); - case Param::YAW_I: - return &attitude_control.get_rate_yaw_pid().kI(); - case Param::YAW_D: - return &attitude_control.get_rate_yaw_pid().kD(); - case Param::YAW_SMAX: - return &attitude_control.get_rate_yaw_pid().slew_limit(); - case Param::YAW_FLTT: - return &attitude_control.get_rate_yaw_pid().filt_T_hz(); - case Param::YAW_FLTD: - return &attitude_control.get_rate_yaw_pid().filt_D_hz(); - case Param::YAW_FLTE: - return &attitude_control.get_rate_yaw_pid().filt_E_hz(); - case Param::YAW_FF: - return &attitude_control.get_rate_roll_pid().ff(); + case AxisName::RLL: + pid_ptr = &attitude_control.get_rate_roll_pid(); + break; + case AxisName::PIT: + pid_ptr = &attitude_control.get_rate_pitch_pid(); + break; + case AxisName::YAW: + pid_ptr = &attitude_control.get_rate_yaw_pid(); + break; + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return nullptr; + } + + Stage stage = get_stage(param); + switch (stage) + { + case Stage::P: + return &pid_ptr->kP(); + case Stage::I: + return &pid_ptr->kI(); + case Stage::D: + return &pid_ptr->kD(); + case Stage::SMAX: + return &pid_ptr->slew_limit(); + case Stage::FLTT: + return &pid_ptr->filt_T_hz(); + case Stage::FLTD: + return &pid_ptr->filt_D_hz(); + case Stage::FLTE: + return &pid_ptr->filt_E_hz(); + case Stage::FF: + return &pid_ptr->ff(); default: INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); return nullptr; From f694118d85ef01bac3f467d9b578d41bb68820d4 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Sat, 3 Aug 2024 22:28:48 +1000 Subject: [PATCH 10/14] AP_Quicktune: Move have_pilot_input() to AP_Vehicle. --- libraries/AP_Quicktune/AP_Quicktune.cpp | 20 +++----------------- libraries/AP_Quicktune/AP_Quicktune.h | 1 - 2 files changed, 3 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Quicktune/AP_Quicktune.cpp b/libraries/AP_Quicktune/AP_Quicktune.cpp index 7d0aa4097adac..3dc88a7db34ef 100644 --- a/libraries/AP_Quicktune/AP_Quicktune.cpp +++ b/libraries/AP_Quicktune/AP_Quicktune.cpp @@ -156,7 +156,9 @@ void AP_Quicktune::update(bool mode_supports_quicktune) } } - if (have_pilot_input()) { + const auto &vehicle = *AP::vehicle(); + + if (vehicle.have_pilot_input()) { last_pilot_input = now; } @@ -167,8 +169,6 @@ void AP_Quicktune::update(bool mode_supports_quicktune) 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()) && now > last_warning + 5000) { GCS_SEND_TEXT(MAV_SEVERITY_EMERGENCY, "Tuning: Must be flying to tune"); last_warning = now; @@ -363,20 +363,6 @@ void AP_Quicktune::setup_filters(AP_Quicktune::AxisName axis) BIT_SET(filters_done, uint8_t(axis)); } -// Check for pilot input to pause tune -bool AP_Quicktune::have_pilot_input() -{ - const auto &rcmap = *AP::rcmap(); - float roll = rc().rc_channel(rcmap.roll()-1)->norm_input_dz(); - float pitch = rc().rc_channel(rcmap.pitch()-1)->norm_input_dz(); - float yaw = rc().rc_channel(rcmap.yaw()-1)->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() { diff --git a/libraries/AP_Quicktune/AP_Quicktune.h b/libraries/AP_Quicktune/AP_Quicktune.h index 2e3a0edef921f..25b65f463f29e 100644 --- a/libraries/AP_Quicktune/AP_Quicktune.h +++ b/libraries/AP_Quicktune/AP_Quicktune.h @@ -130,7 +130,6 @@ class AP_Quicktune { void reset_axes_done(); void setup_filters(AxisName axis); - bool have_pilot_input(); AxisName get_current_axis(); float get_slew_rate(AxisName axis); void advance_stage(AxisName axis); From 19a8eb85c2c94000a3b9aed7f4a023dec9f15c61 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Sat, 3 Aug 2024 22:29:05 +1000 Subject: [PATCH 11/14] AP_Vehicle: Add have_pilot_input() --- libraries/AP_Vehicle/AP_Vehicle.cpp | 15 +++++++++++++++ libraries/AP_Vehicle/AP_Vehicle.h | 3 +++ 2 files changed, 18 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 562ac606075a4..f73a4a634ed95 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -1000,6 +1000,21 @@ void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const #endif } +// Check for pilot input +bool AP_Vehicle::have_pilot_input() const +{ + const auto &rcmap = *AP::rcmap(); + float roll = rc().rc_channel(rcmap.roll()-1)->norm_input_dz(); + float pitch = rc().rc_channel(rcmap.pitch()-1)->norm_input_dz(); + float yaw = rc().rc_channel(rcmap.yaw()-1)->norm_input_dz(); + + if (fabsf(roll) > 0 || fabsf(pitch) > 0 || fabsf(yaw) > 0) { + return true; + } + return false; +} + + #if HAL_INS_ACCELCAL_ENABLED #ifndef HAL_CAL_ALWAYS_REBOOT diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 5256dd82e281f..3b5dee88dc8b7 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -300,6 +300,9 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { virtual bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED { return false; } #endif + // Check for pilot input + bool have_pilot_input() const; + protected: virtual void init_ardupilot() = 0; From e3460568bb13050696f7086bb498b8bb0481b309 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Tue, 6 Aug 2024 12:34:55 +1000 Subject: [PATCH 12/14] AP_Quicktune: Fixes --- libraries/AP_Quicktune/AP_Quicktune.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/libraries/AP_Quicktune/AP_Quicktune.cpp b/libraries/AP_Quicktune/AP_Quicktune.cpp index 3dc88a7db34ef..2bea5814872b2 100644 --- a/libraries/AP_Quicktune/AP_Quicktune.cpp +++ b/libraries/AP_Quicktune/AP_Quicktune.cpp @@ -202,7 +202,6 @@ void AP_Quicktune::update(bool mode_supports_quicktune) if (slew_parm != Param::END) { float P = get_param_value(slew_parm); AxisName axis = get_axis(slew_parm); - // local ax_stage = string.sub(slew_parm, -1) adjust_gain(slew_parm, P+slew_delta); slew_steps = slew_steps - 1; Write_QUIK(get_slew_rate(axis), P, slew_parm); @@ -344,6 +343,7 @@ 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) { @@ -497,7 +497,6 @@ void AP_Quicktune::restore_all_params() void AP_Quicktune::save_all_params() { - // for pname in pairs(params) do for (int8_t pname = 0; pname < uint8_t(Param::END); pname++) { if (BIT_IS_SET(param_changed, pname)) { set_and_save_param_value(Param(pname), get_param_value(Param(pname))); @@ -651,7 +650,6 @@ void AP_Quicktune::set_param_value(AP_Quicktune::Param param, float value) return; } INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - return; } void AP_Quicktune::set_and_save_param_value(AP_Quicktune::Param param, float value) @@ -662,7 +660,6 @@ void AP_Quicktune::set_and_save_param_value(AP_Quicktune::Param param, float val return; } INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - return; } AP_Quicktune::AxisName AP_Quicktune::get_axis(AP_Quicktune::Param param) From a839230eea268ad306434e5ee4d7c5a6a2b5d7c7 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Sat, 17 Aug 2024 12:21:45 +1000 Subject: [PATCH 13/14] AP_QUicktune: remove extraneous internal errors --- libraries/AP_Quicktune/AP_Quicktune.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/AP_Quicktune/AP_Quicktune.cpp b/libraries/AP_Quicktune/AP_Quicktune.cpp index 2bea5814872b2..8b5a3fce7e6a4 100644 --- a/libraries/AP_Quicktune/AP_Quicktune.cpp +++ b/libraries/AP_Quicktune/AP_Quicktune.cpp @@ -638,7 +638,6 @@ float AP_Quicktune::get_param_value(AP_Quicktune::Param param) if (ptr != nullptr) { return ptr->get(); } - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); return 0.0; } @@ -649,7 +648,6 @@ void AP_Quicktune::set_param_value(AP_Quicktune::Param param, float value) ptr->set(value); return; } - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } void AP_Quicktune::set_and_save_param_value(AP_Quicktune::Param param, float value) @@ -659,7 +657,6 @@ void AP_Quicktune::set_and_save_param_value(AP_Quicktune::Param param, float val ptr->set_and_save(value); return; } - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } AP_Quicktune::AxisName AP_Quicktune::get_axis(AP_Quicktune::Param param) From b1125205870eea8e475c1867312f1b0c63702712 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Sat, 17 Aug 2024 12:24:25 +1000 Subject: [PATCH 14/14] AP_Quicktune: Remove the two if statements --- libraries/AP_Quicktune/AP_Quicktune.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Quicktune/AP_Quicktune.cpp b/libraries/AP_Quicktune/AP_Quicktune.cpp index 8b5a3fce7e6a4..c4d68315bd312 100644 --- a/libraries/AP_Quicktune/AP_Quicktune.cpp +++ b/libraries/AP_Quicktune/AP_Quicktune.cpp @@ -185,13 +185,10 @@ void AP_Quicktune::update(bool mode_supports_quicktune) reset_axes_done(); return; } - if (sw_pos == sw_pos_save) { - // Save all params - if (need_restore) { - need_restore = false; - save_all_params(); - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Tuning: Saved"); - } + if (sw_pos == sw_pos_save && need_restore) { + need_restore = false; + save_all_params(); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Tuning: Saved"); } if (sw_pos != sw_pos_tune) { return;