Skip to content

Commit

Permalink
AP_Quicktune: review changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@tridge
tridge committed Oct 19, 2024
1 parent 4d857cb commit 6e76e07
Show file tree
Hide file tree
Showing 2 changed files with 36 additions and 36 deletions.
61 changes: 31 additions & 30 deletions libraries/AP_Quicktune/AP_Quicktune.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -177,18 +177,20 @@ 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()) {
// Abort, revert parameters
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;
Expand All @@ -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;
Expand All @@ -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;
}
}
}
Expand All @@ -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;
Expand All @@ -244,22 +244,22 @@ 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));
}
// 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;
}

Expand Down Expand Up @@ -301,23 +301,23 @@ 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) {
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;
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);
}
}
Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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)
Expand Down
11 changes: 5 additions & 6 deletions libraries/AP_Quicktune/AP_Quicktune.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand Down

0 comments on commit 6e76e07

Please sign in to comment.