From 34eae9d9cba81bdd5b5403220d2c7f8878f82420 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 16 May 2017 15:20:38 +1000 Subject: [PATCH] Copter: AutoTune: include axis being tuned in output --- ArduCopter/control_autotune.cpp | 55 ++++++++++++++++++--------------- 1 file changed, 30 insertions(+), 25 deletions(-) diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index 7dd20e10f7..7df1658a4a 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -337,7 +337,36 @@ void Copter::autotune_do_gcs_announcements() if (now - autotune_announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { return; } - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "AutoTune: %s", autotune_type_string()); + float tune_rp = 0.0f; + float tune_rd = 0.0f; + float tune_sp = 0.0f; + float tune_accel = 0.0f; + char axis = '?'; + switch (autotune_state.axis) { + case AUTOTUNE_AXIS_ROLL: + tune_rp = tune_roll_rp; + tune_rd = tune_roll_rd; + tune_sp = tune_roll_sp; + tune_accel = tune_roll_accel; + axis = 'R'; + break; + case AUTOTUNE_AXIS_PITCH: + tune_rp = tune_pitch_rp; + tune_rd = tune_pitch_rd; + tune_sp = tune_pitch_sp; + tune_accel = tune_pitch_accel; + axis = 'P'; + break; + case AUTOTUNE_AXIS_YAW: + tune_rp = tune_yaw_rp; + tune_rd = tune_yaw_rLPF; + tune_sp = tune_yaw_sp; + tune_accel = tune_yaw_accel; + axis = 'Y'; + break; + } + + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "AutoTune: (%c) %s", axis, autotune_type_string()); autotune_send_step_string(); if (!is_zero(lean_angle)) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "AutoTune: lean=%f target=%f", lean_angle, autotune_target_angle); @@ -348,30 +377,6 @@ void Copter::autotune_do_gcs_announcements() if (!is_zero(rotation_rate)) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "AutoTune: rotation=%f target=%f", rotation_rate*0.01f, autotune_target_rate*0.01f); } - float tune_rp = 0.0f; - float tune_rd = 0.0f; - float tune_sp = 0.0f; - float tune_accel = 0.0f; - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: - tune_rp = tune_roll_rp; - tune_rd = tune_roll_rd; - tune_sp = tune_roll_sp; - tune_accel = tune_roll_accel; - break; - case AUTOTUNE_AXIS_PITCH: - tune_rp = tune_pitch_rp; - tune_rd = tune_pitch_rd; - tune_sp = tune_pitch_sp; - tune_accel = tune_pitch_accel; - break; - case AUTOTUNE_AXIS_YAW: - tune_rp = tune_yaw_rp; - tune_rd = tune_yaw_rLPF; - tune_sp = tune_yaw_sp; - tune_accel = tune_yaw_accel; - break; - } switch (autotune_state.tune_type) { case AUTOTUNE_TYPE_RD_UP: case AUTOTUNE_TYPE_RD_DOWN: