From 586a5df81463231482e5e451a49eeb2fd26b87b6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 8 Feb 2017 11:35:31 +1100 Subject: [PATCH] Copter: add GCS messaging during autotune --- ArduCopter/Copter.h | 5 ++ ArduCopter/control_autotune.cpp | 106 +++++++++++++++++++++++++++++++- 2 files changed, 108 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c42417af59..c82be03012 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -835,6 +835,11 @@ private: void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max); void autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd); void avoidance_adsb_update(void); + const char * autotune_step_string() const; + const char * autotune_type_string() const; + void autotune_announce_state_to_gcs(); + void autotune_do_gcs_announcements(); + #if ADVANCED_FAILSAFE == ENABLED void afs_fs_check(void); #endif diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index 1214f5e977..aac57e877e 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -90,6 +90,8 @@ #define AUTOTUNE_MESSAGE_FAILED 3 #define AUTOTUNE_MESSAGE_SAVED_GAINS 4 +#define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000 + // autotune modes (high level states) enum AutoTuneTuneMode { AUTOTUNE_MODE_UNINITIALISED = 0, // autotune has never been run @@ -117,7 +119,7 @@ enum AutoTuneTuneType { AUTOTUNE_TYPE_RD_UP = 0, // rate D is being tuned up AUTOTUNE_TYPE_RD_DOWN = 1, // rate D is being tuned down AUTOTUNE_TYPE_RP_UP = 2, // rate P is being tuned up - AUTOTUNE_TYPE_SP_DOWN = 3, // angle P is being tuned up + AUTOTUNE_TYPE_SP_DOWN = 3, // angle P is being tuned down AUTOTUNE_TYPE_SP_UP = 4 // angle P is being tuned up }; @@ -160,6 +162,11 @@ static float tune_roll_rp, tune_roll_rd, tune_roll_sp, tune_roll_accel; static float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel; static float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel; +static uint32_t autotune_announce_time; +static float lean_angle; +static float rotation_rate; + + // autotune_init - should be called when autotune mode is selected bool Copter::autotune_init(bool ignore_checks) { @@ -260,6 +267,95 @@ bool Copter::autotune_start(bool ignore_checks) return true; } +const char *Copter::autotune_step_string() const +{ + if (autotune_state.pilot_override) { + return "Paused: Pilot Override Active"; + } + switch (autotune_state.step) { + case AUTOTUNE_STEP_WAITING_FOR_LEVEL: + return "WAITING_FOR_LEVEL"; + case AUTOTUNE_STEP_UPDATE_GAINS: + return "UPDATING_GAINS"; + case AUTOTUNE_STEP_TWITCHING: + return "TWITCHING"; + } + return "Unknown"; +} + +const char *Copter::autotune_type_string() const +{ + switch (autotune_state.tune_type) { + case AUTOTUNE_TYPE_RD_UP: + return "Rate D Up"; + case AUTOTUNE_TYPE_RD_DOWN: + return "Rate D Down"; + case AUTOTUNE_TYPE_RP_UP: + return "Rate P Up"; + case AUTOTUNE_TYPE_SP_DOWN: + return "Angle P Down"; + case AUTOTUNE_TYPE_SP_UP: + return "Angle P Up"; + } + return "Unknown"; +} + +void Copter::autotune_do_gcs_announcements() +{ + const uint32_t now = millis(); + if (now - autotune_announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { + return; + } + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "AutoTune: %s.%s", autotune_step_string(), autotune_type_string()); + if (!is_zero(lean_angle)) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "AutoTune: lean=%f target=%f", lean_angle, autotune_target_angle); + } + 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); + } + 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: + case AUTOTUNE_TYPE_RP_UP: + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", tune_rp, tune_rd); + break; + case AUTOTUNE_TYPE_SP_DOWN: + case AUTOTUNE_TYPE_SP_UP: + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "AutoTune: p=%f accel=%f", tune_sp, tune_accel); + break; + } + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "AutoTune: success %u/%u", autotune_counter, AUTOTUNE_SUCCESS_COUNT); + + autotune_announce_time = now; +} + // autotune_run - runs the autotune flight mode // should be called at 100hz or more void Copter::autotune_run() @@ -268,6 +364,9 @@ void Copter::autotune_run() float target_yaw_rate; int16_t target_climb_rate; + // tell the user what's going on + autotune_do_gcs_announcements(); + // initialize vertical speeds and acceleration pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control->set_accel_z(g.pilot_accel_z); @@ -366,8 +465,8 @@ void Copter::autotune_run() // autotune_attitude_controller - sets attitude control targets during tuning void Copter::autotune_attitude_control() { - float rotation_rate = 0.0f; // rotation rate in radians/second - float lean_angle = 0.0f; + rotation_rate = 0.0f; // rotation rate in radians/second + lean_angle = 0.0f; const float direction_sign = autotune_state.positive_direction ? 1.0f : -1.0f; // check tuning step @@ -397,6 +496,7 @@ void Copter::autotune_attitude_control() // if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step if (millis() - autotune_step_start_time >= AUTOTUNE_REQUIRED_LEVEL_TIME_MS) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "AutoTune: Twitch"); // initiate variables for next step autotune_state.step = AUTOTUNE_STEP_TWITCHING; autotune_step_start_time = millis();