Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: use GCS_SEND_TEXT rather than gcs().send_text 

Co-authored-by: muramura <ma2maru@gmail.com>
This commit is contained in:
Peter Barker 2024-08-07 13:17:19 +10:00 committed by Andrew Tridgell
parent b6717e541a
commit bf09835509
3 changed files with 47 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -124,24 +124,24 @@ bool AC_AutoTune::init_position_controller(void)
void AC_AutoTune::send_step_string()
{
if (pilot_override) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Paused: Pilot Override Active");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Paused: Pilot Override Active");
return;
}
switch (step) {
case WAITING_FOR_LEVEL:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Leveling");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Leveling");
return;
case UPDATE_GAINS:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Updating Gains");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Updating Gains");
return;
case ABORT:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Aborting Test");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Aborting Test");
return;
case TESTING:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Testing");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Testing");
return;
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: unknown step");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: unknown step");
}
const char *AC_AutoTune::type_string() const
@ -242,7 +242,7 @@ void AC_AutoTune::run()
}
if (pilot_override) {
if (now - last_pilot_override_warning > 1000) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: pilot overrides active");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: pilot overrides active");
last_pilot_override_warning = now;
}
}
@ -276,7 +276,7 @@ bool AC_AutoTune::currently_level()
// abort AutoTune if we pass 2 * AUTOTUNE_LEVEL_TIMEOUT_MS
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - level_start_time_ms > 3 * AUTOTUNE_LEVEL_TIMEOUT_MS) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: Failed to level, please tune manually");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AutoTune: Failed to level, please tune manually");
mode = FAILED;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
}
@ -612,22 +612,22 @@ void AC_AutoTune::update_gcs(uint8_t message_id) const
{
switch (message_id) {
case AUTOTUNE_MESSAGE_STARTED:
gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Started");
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"AutoTune: Started");
break;
case AUTOTUNE_MESSAGE_STOPPED:
gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Stopped");
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"AutoTune: Stopped");
break;
case AUTOTUNE_MESSAGE_SUCCESS:
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Success");
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: Success");
break;
case AUTOTUNE_MESSAGE_FAILED:
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Failed");
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: Failed");
break;
case AUTOTUNE_MESSAGE_TESTING:
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Pilot Testing");
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: Pilot Testing");
break;
case AUTOTUNE_MESSAGE_SAVED_GAINS:
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Saved gains for %s%s%s%s",
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: Saved gains for %s%s%s%s",
(axes_completed&AUTOTUNE_AXIS_BITMASK_ROLL)?"Roll ":"",
(axes_completed&AUTOTUNE_AXIS_BITMASK_PITCH)?"Pitch ":"",
(axes_completed&AUTOTUNE_AXIS_BITMASK_YAW)?"Yaw(E)":"",

44
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -282,12 +282,12 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign)
attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw_cd, true);
if ((tune_type == RP_UP || tune_type == RD_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Gain Determination Failed");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Max Gain Determination Failed");
mode = FAILED;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
update_gcs(AUTOTUNE_MESSAGE_FAILED);
} else if ((tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP || tune_type == SP_UP) && exceeded_freq_range(start_freq)){
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Exceeded frequency range");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Exceeded frequency range");
mode = FAILED;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
update_gcs(AUTOTUNE_MESSAGE_FAILED);
@ -310,7 +310,7 @@ void AC_AutoTune_Heli::do_gcs_announcements()
return;
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: %s %s", axis_string(), type_string());
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: %s %s", axis_string(), type_string());
send_step_string();
switch (tune_type) {
case RFF_UP:
@ -320,11 +320,11 @@ void AC_AutoTune_Heli::do_gcs_announcements()
case SP_UP:
case TUNE_CHECK:
if (is_equal(start_freq,stop_freq)) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Dwell");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Dwell");
} else {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Sweep");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Sweep");
if (settle_time == 0) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test.freq), (double)(curr_test.gain), (double)(curr_test.phase));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test.freq), (double)(curr_test.gain), (double)(curr_test.phase));
}
}
break;
@ -377,20 +377,20 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() {
case MAX_GAINS:
if (is_equal(start_freq,stop_freq)) {
// announce results of dwell
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(curr_data.freq), (double)(curr_data.gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f", (double)(curr_data.phase));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(curr_data.freq), (double)(curr_data.gain));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: ph=%f", (double)(curr_data.phase));
if (tune_type == RP_UP) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rate_p=%f", (double)(tune_rp));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: rate_p=%f", (double)(tune_rp));
} else if (tune_type == RD_UP) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rate_d=%f", (double)(tune_rd));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: rate_d=%f", (double)(tune_rd));
} else if (tune_type == RFF_UP) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rate_ff=%f", (double)(tune_rff));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: rate_ff=%f", (double)(tune_rff));
} else if (tune_type == SP_UP) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: angle_p=%f tune_accel=%f max_accel=%f", (double)(tune_sp), (double)(tune_accel), (double)(test_accel_max));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: angle_p=%f tune_accel=%f max_accel=%f", (double)(tune_sp), (double)(tune_accel), (double)(test_accel_max));
}
} else {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep_tgt.maxgain.freq), (double)(sweep_tgt.maxgain.gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep_tgt.ph180.freq), (double)(sweep_tgt.ph180.gain));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep_tgt.maxgain.freq), (double)(sweep_tgt.maxgain.gain));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep_tgt.ph180.freq), (double)(sweep_tgt.ph180.gain));
}
break;
default:
@ -731,9 +731,9 @@ void AC_AutoTune_Heli::report_final_gains(AxisType test_axis) const
// report gain formatting helper
void AC_AutoTune_Heli::report_axis_gains(const char* axis_string, float rate_P, float rate_I, float rate_D, float rate_ff, float angle_P, float max_accel) const
{
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s complete", axis_string);
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s Rate: P:%0.4f, I:%0.4f, D:%0.5f, FF:%0.4f",axis_string,rate_P,rate_I,rate_D,rate_ff);
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s Angle P:%0.2f, Max Accel:%0.0f",axis_string,angle_P,max_accel);
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s complete", axis_string);
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s Rate: P:%0.4f, I:%0.4f, D:%0.5f, FF:%0.4f",axis_string,rate_P,rate_I,rate_D,rate_ff);
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s Angle P:%0.2f, Max Accel:%0.0f",axis_string,angle_P,max_accel);
}
void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float amplitude, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type)
@ -1013,7 +1013,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
} else {
if (now - step_start_time_ms >= step_time_limit_ms || (freqresp_tgt.is_cycle_complete() && freqresp_mtr.is_cycle_complete())) {
if (now - step_start_time_ms >= step_time_limit_ms) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Step time limit exceeded");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Step time limit exceeded");
}
cycle_complete_tgt = false;
cycle_complete_tgt = false;
@ -1432,10 +1432,10 @@ void AC_AutoTune_Heli::updating_max_gains(sweep_info &test_data, float &next_fre
counter = AUTOTUNE_SUCCESS_COUNT;
// reset variables for next test
next_freq = 0.0f; //initializes next test that uses dwell test
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max rate P freq=%f gain=%f", (double)(max_rate_p.freq), (double)(max_rate_p.gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_p=%f", (double)(max_rate_p.phase), (double)(max_rate_p.max_allowed));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Rate D freq=%f gain=%f", (double)(max_rate_d.freq), (double)(max_rate_d.gain));
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_d=%f", (double)(max_rate_d.phase), (double)(max_rate_d.max_allowed));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Max rate P freq=%f gain=%f", (double)(max_rate_p.freq), (double)(max_rate_p.gain));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_p=%f", (double)(max_rate_p.phase), (double)(max_rate_p.max_allowed));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Max Rate D freq=%f gain=%f", (double)(max_rate_d.freq), (double)(max_rate_d.gain));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_d=%f", (double)(max_rate_d.phase), (double)(max_rate_d.max_allowed));
}
}

22
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -127,7 +127,7 @@ void AC_AutoTune_Multi::do_gcs_announcements()
if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) {
return;
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: %s %s %u%%", axis_string(), type_string(), (counter * (100/AUTOTUNE_SUCCESS_COUNT)));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: %s %s %u%%", axis_string(), type_string(), (counter * (100/AUTOTUNE_SUCCESS_COUNT)));
announce_time = now;
}
@ -520,9 +520,9 @@ void AC_AutoTune_Multi::report_final_gains(AxisType test_axis) const
// report gain formatting helper
void AC_AutoTune_Multi::report_axis_gains(const char* axis_string, float rate_P, float rate_I, float rate_D, float angle_P, float max_accel) const
{
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s complete", axis_string);
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s Rate: P:%0.3f, I:%0.3f, D:%0.4f",axis_string,rate_P,rate_I,rate_D);
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s Angle P:%0.3f, Max Accel:%0.0f",axis_string,angle_P,max_accel);
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s complete", axis_string);
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s Rate: P:%0.3f, I:%0.3f, D:%0.4f",axis_string,rate_P,rate_I,rate_D);
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s Angle P:%0.3f, Max Accel:%0.0f",axis_string,angle_P,max_accel);
}
// twitching_test_rate - twitching tests
@ -581,7 +581,7 @@ void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angl
step_scaler *= 0.9f;
} else {
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: Twitch Size Determination Failed");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AutoTune: Twitch Size Determination Failed");
mode = FAILED;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
}
@ -836,7 +836,7 @@ void AC_AutoTune_Multi::updating_rate_d_up(float &tune_d, float tune_d_min, floa
counter = AUTOTUNE_SUCCESS_COUNT;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
// This may be mean AGGR should be increased or MIN_D decreased
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached");
}
}
} else if ((meas_rate_max < rate_target * (1.0 - AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
@ -893,7 +893,7 @@ void AC_AutoTune_Multi::updating_rate_d_down(float &tune_d, float tune_d_min, fl
counter = AUTOTUNE_SUCCESS_COUNT;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
// This may be mean AGGR should be increased or MIN_D decreased
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached");
}
}
} else if ((meas_rate_max < rate_target*(1.0 - AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
@ -928,7 +928,7 @@ void AC_AutoTune_Multi::updating_rate_d_down(float &tune_d, float tune_d_min, fl
counter = AUTOTUNE_SUCCESS_COUNT;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
// This may be mean AGGR should be increased or MIN_D decreased
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached");
}
}
}
@ -955,7 +955,7 @@ void AC_AutoTune_Multi::updating_rate_p_up_d_down(float &tune_d, float tune_d_mi
tune_d = tune_d_min;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
if (fail_min_d) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: Rate D Gain Determination Failed");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AutoTune: Rate D Gain Determination Failed");
mode = FAILED;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
}
@ -965,7 +965,7 @@ void AC_AutoTune_Multi::updating_rate_p_up_d_down(float &tune_d, float tune_d_mi
// do not decrease the P term past the minimum
if (tune_p <= tune_p_min) {
tune_p = tune_p_min;
gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: Rate P Gain Determination Failed");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AutoTune: Rate P Gain Determination Failed");
mode = FAILED;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
}
@ -1014,7 +1014,7 @@ void AC_AutoTune_Multi::updating_angle_p_down(float &tune_p, float tune_p_min, f
tune_p = tune_p_min;
counter = AUTOTUNE_SUCCESS_COUNT;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: Angle P Gain Determination Failed");
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AutoTune: Angle P Gain Determination Failed");
mode = FAILED;
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
}