From 6e76e0735d93c8fc7e3806abaffc3ec67e858706 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Oct 2024 17:34:30 +0900 Subject: [PATCH] AP_Quicktune: review changes --- libraries/AP_Quicktune/AP_Quicktune.cpp | 61 +++++++++++++------------ libraries/AP_Quicktune/AP_Quicktune.h | 11 ++--- 2 files changed, 36 insertions(+), 36 deletions(-) diff --git a/libraries/AP_Quicktune/AP_Quicktune.cpp b/libraries/AP_Quicktune/AP_Quicktune.cpp index c1c4c32641f49b..05ad7b9c33049f 100644 --- a/libraries/AP_Quicktune/AP_Quicktune.cpp +++ b/libraries/AP_Quicktune/AP_Quicktune.cpp @@ -136,7 +136,7 @@ void AP_Quicktune::update(bool mode_supports_quicktune) { if (enable < 1) { if (need_restore) { - GCS_SEND_TEXT(MAV_SEVERITY_EMERGENCY, "QuickTune disabled"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "QuickTune disabled"); abort_tune(); } return; @@ -149,7 +149,7 @@ void AP_Quicktune::update(bool mode_supports_quicktune) pending parameter changes then revert */ if (need_restore) { - GCS_SEND_TEXT(MAV_SEVERITY_EMERGENCY, "QuickTune aborted"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "QuickTune aborted"); } abort_tune(); return; @@ -158,14 +158,14 @@ void AP_Quicktune::update(bool mode_supports_quicktune) 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_EMERGENCY, "Tuning: attitude error %.1fdeg - ABORTING", att_error); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Quicktune: attitude error %.1fdeg - ABORTING", att_error); abort_tune(); return; } } if (have_pilot_input()) { - last_pilot_input = now; + last_pilot_input_ms = now; } SwitchPos sw_pos_tune = SwitchPos::MID; @@ -177,9 +177,11 @@ void AP_Quicktune::update(bool mode_supports_quicktune) 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; + if (sw_pos == sw_pos_tune && + (!hal.util->get_soft_armed() || !vehicle.get_likely_flying()) && + 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()) { @@ -187,8 +189,8 @@ void AP_Quicktune::update(bool mode_supports_quicktune) if (need_restore) { need_restore = false; restore_all_params(); - GCS_SEND_TEXT(MAV_SEVERITY_EMERGENCY, "Tuning: Reverted"); - tune_done_time = 0; + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Quicktune: Reverted"); + tune_done_time_ms = 0; } reset_axes_done(); return; @@ -198,18 +200,17 @@ void AP_Quicktune::update(bool mode_supports_quicktune) 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) { + if (now - last_stage_change_ms < STAGE_DELAY) { // Update slew gain if (slew_parm != Param::END) { - float P = get_param_value(slew_parm); - AxisName axis = get_axis(slew_parm); + const float P = get_param_value(slew_parm); + const 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; @@ -218,8 +219,8 @@ void AP_Quicktune::update(bool mode_supports_quicktune) 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; + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: DONE"); + tune_done_time_ms = now; } } } @@ -230,12 +231,11 @@ void AP_Quicktune::update(bool mode_supports_quicktune) 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)) { + if (tune_done_time_ms != 0 && auto_save > 0) { + if (now - tune_done_time_ms > (auto_save*1000)) { need_restore = false; save_all_params(); - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Tuning: Saved"); - tune_done_time = 0; + tune_done_time_ms = 0; } } return; @@ -244,7 +244,7 @@ void AP_Quicktune::update(bool mode_supports_quicktune) if (!need_restore) { need_restore = true; // We are just starting tuning, get current values - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Tuning: Starting tune"); + 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)); @@ -252,14 +252,14 @@ void AP_Quicktune::update(bool mode_supports_quicktune) // Set up SMAX const Param is[3] { Param::RLL_SMAX, Param::PIT_SMAX, Param::YAW_SMAX }; for (const auto p : is) { - float smax = get_param_value(p); + const float smax = get_param_value(p); if (smax <= 0) { adjust_gain(p, DEFAULT_SMAX); } } } - if (now - last_pilot_input < PILOT_INPUT_DELAY) { + if (now - last_pilot_input_ms < PILOT_INPUT_DELAY) { return; } @@ -301,14 +301,14 @@ void AP_Quicktune::update(bool mode_supports_quicktune) } // Set up slew gain slew_parm = pname; - slew_target = limit_gain(pname, new_gain); + 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_QUIK(srate, pval, pname); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Tuning: %s done", get_param_name(pname)); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Quicktune: %s done", get_param_name(pname)); advance_stage(axis); - last_stage_change = now; + last_stage_change_ms = now; } else { float new_gain = pval*get_gain_mul(); if (new_gain <= 0.0001) { @@ -316,8 +316,8 @@ void AP_Quicktune::update(bool mode_supports_quicktune) } adjust_gain_limited(pname, new_gain); Write_QUIK(srate, pval, pname); - if (now - last_gain_report > 3000) { - last_gain_report = now; + 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); } } @@ -332,7 +332,7 @@ void AP_Quicktune::abort_tune() need_restore = false; restore_all_params(); } - tune_done_time = 0; + tune_done_time_ms = 0; reset_axes_done(); sw_pos = SwitchPos::LOW; } @@ -432,7 +432,7 @@ void AP_Quicktune::advance_stage(AP_Quicktune::AxisName axis) 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)); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: %s done", get_axis_name(axis)); current_stage = Stage::D; } } @@ -481,7 +481,7 @@ float AP_Quicktune::limit_gain(AP_Quicktune::Param param, float value) // Check if we exceeded gain reduction const 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; + 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; } @@ -541,6 +541,7 @@ void AP_Quicktune::save_all_params() 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) diff --git a/libraries/AP_Quicktune/AP_Quicktune.h b/libraries/AP_Quicktune/AP_Quicktune.h index 9359220c526f42..b51d181d74c0b7 100644 --- a/libraries/AP_Quicktune/AP_Quicktune.h +++ b/libraries/AP_Quicktune/AP_Quicktune.h @@ -117,11 +117,11 @@ class AP_Quicktune { }; // 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; + 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; @@ -130,7 +130,6 @@ class AP_Quicktune { 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