From 89d54767b15960fbe3cdcdbfd06453a540aefd6b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Dec 2018 14:53:56 +1100 Subject: [PATCH] Copter: convert to use AC_AutoTune library this maintains existing behaviour --- ArduCopter/Copter.h | 3 + ArduCopter/Log.cpp | 60 -- ArduCopter/Parameters.cpp | 33 +- ArduCopter/Parameters.h | 17 +- ArduCopter/defines.h | 2 - ArduCopter/mode.h | 151 +--- ArduCopter/mode_autotune.cpp | 1630 +++------------------------------- ArduCopter/system.cpp | 2 +- ArduCopter/wscript | 1 + 9 files changed, 156 insertions(+), 1743 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 9930d986b6..e88dbbc66a 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -82,6 +82,7 @@ #include #include #include +#include // Configuration #include "defines.h" @@ -263,6 +264,7 @@ private: NavEKF2 EKF2{&ahrs, rangefinder}; NavEKF3 EKF3{&ahrs, rangefinder}; AP_AHRS_NavEKF ahrs{EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF}; + AP_AHRS_View *ahrs_view; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL SITL::SITL sitl; @@ -912,6 +914,7 @@ private: ModeAuto mode_auto; #endif #if AUTOTUNE_ENABLED == ENABLED + AutoTune autotune; ModeAutoTune mode_autotune; #endif #if MODE_BRAKE_ENABLED == ENABLED diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index fb86ab094c..86c550ea38 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -5,60 +5,6 @@ // Code to Write and Read packets from DataFlash log memory // Code to interact with the user to dump or erase logs -#if AUTOTUNE_ENABLED == ENABLED -struct PACKED log_AutoTune { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t axis; // roll or pitch - uint8_t tune_step; // tuning PI or D up or down - float meas_target; // target achieved rotation rate - float meas_min; // maximum achieved rotation rate - float meas_max; // maximum achieved rotation rate - float new_gain_rp; // newly calculated gain - float new_gain_rd; // newly calculated gain - float new_gain_sp; // newly calculated gain - float new_ddt; // newly calculated gain -}; - -// Write an Autotune data packet -void Copter::ModeAutoTune::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt) -{ - struct log_AutoTune pkt = { - LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG), - time_us : AP_HAL::micros64(), - axis : _axis, - tune_step : tune_step, - meas_target : meas_target, - meas_min : meas_min, - meas_max : meas_max, - new_gain_rp : new_gain_rp, - new_gain_rd : new_gain_rd, - new_gain_sp : new_gain_sp, - new_ddt : new_ddt - }; - copter.DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} - -struct PACKED log_AutoTuneDetails { - LOG_PACKET_HEADER; - uint64_t time_us; - float angle_cd; // lean angle in centi-degrees - float rate_cds; // current rotation rate in centi-degrees / second -}; - -// Write an Autotune data packet -void Copter::ModeAutoTune::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) -{ - struct log_AutoTuneDetails pkt = { - LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG), - time_us : AP_HAL::micros64(), - angle_cd : angle_cd, - rate_cds : rate_cds - }; - copter.DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} -#endif - struct PACKED log_Control_Tuning { LOG_PACKET_HEADER; uint64_t time_us; @@ -472,12 +418,6 @@ void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_tar // units and "Format characters" for field type information const struct LogStructure Copter::log_structure[] = { LOG_COMMON_STRUCTURES, -#if AUTOTUNE_ENABLED == ENABLED - { LOG_AUTOTUNE_MSG, sizeof(log_AutoTune), - "ATUN", "QBBfffffff", "TimeUS,Axis,TuneStep,Targ,Min,Max,RP,RD,SP,ddt", "s--ddd---o", "F--BBB---0" }, - { LOG_AUTOTUNEDETAILS_MSG, sizeof(log_AutoTuneDetails), - "ATDE", "Qff", "TimeUS,Angle,Rate", "sdk", "FBB" }, -#endif { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning), "PTUN", "QBfHHH", "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi", "s-----", "F-----" }, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index b8f4564371..e4c8f6e520 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -707,30 +707,6 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(avoidance_adsb, "AVD_", AP_Avoidance_Copter), #endif -#if AUTOTUNE_ENABLED == ENABLED - // @Param: AUTOTUNE_AXES - // @DisplayName: Autotune axis bitmask - // @Description: 1-byte bitmap of axes to autotune - // @Values: 7:All,1:Roll Only,2:Pitch Only,4:Yaw Only,3:Roll and Pitch,5:Roll and Yaw,6:Pitch and Yaw - // @Bitmask: 0:Roll,1:Pitch,2:Yaw - // @User: Standard - GSCALAR(autotune_axis_bitmask, "AUTOTUNE_AXES", 7), // AUTOTUNE_AXIS_BITMASK_DEFAULT - - // @Param: AUTOTUNE_AGGR - // @DisplayName: Autotune aggressiveness - // @Description: Autotune aggressiveness. Defines the bounce back used to detect size of the D term. - // @Range: 0.05 0.10 - // @User: Standard - GSCALAR(autotune_aggressiveness, "AUTOTUNE_AGGR", 0.1f), - - // @Param: AUTOTUNE_MIN_D - // @DisplayName: AutoTune minimum D - // @Description: Defines the minimum D gain - // @Range: 0.001 0.006 - // @User: Standard - GSCALAR(autotune_min_d, "AUTOTUNE_MIN_D", 0.001f), -#endif - // @Group: NTF_ // @Path: ../libraries/AP_Notify/AP_Notify.cpp GOBJECT(notify, "NTF_", AP_Notify), @@ -948,6 +924,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(user_parameters, "USR", 28, ParametersG2, UserParameters), #endif +#if AUTOTUNE_ENABLED == ENABLED + // @Group: AUTOTUNE_ + // @Path: ../libraries/AC_AutoTune/AC_AutoTune.cpp + AP_SUBGROUPPTR(autotune_ptr, "AUTOTUNE_", 29, ParametersG2, Copter::AutoTune), +#endif + AP_GROUPEND }; @@ -1016,6 +998,9 @@ ParametersG2::ParametersG2(void) #ifdef USER_PARAMS_ENABLED ,user_parameters() #endif +#if AUTOTUNE_ENABLED == ENABLED + ,autotune_ptr(&copter.autotune) +#endif { AP_Param::setup_object_defaults(this, var_info); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 5015b6fadf..74ee2e1489 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -350,13 +350,13 @@ public: k_param_acro_balance_roll, k_param_acro_balance_pitch, k_param_acro_yaw_p, - k_param_autotune_axis_bitmask, - k_param_autotune_aggressiveness, + k_param_autotune_axis_bitmask, // remove + k_param_autotune_aggressiveness, // remove k_param_pi_vel_xy, // remove k_param_fs_ekf_action, k_param_rtl_climb_min, k_param_rpm_sensor, - k_param_autotune_min_d, // 251 + k_param_autotune_min_d, // remove k_param_arming, // 252 - AP_Arming k_param_DataFlash = 253, // 253 - Logging Group @@ -458,13 +458,6 @@ public: AP_Int8 acro_trainer; AP_Float acro_rp_expo; - // Autotune -#if AUTOTUNE_ENABLED == ENABLED - AP_Int8 autotune_axis_bitmask; - AP_Float autotune_aggressiveness; - AP_Float autotune_min_d; -#endif - // Note: keep initializers here in the same order as they are declared // above. Parameters() @@ -586,6 +579,10 @@ public: UserParameters user_parameters; #endif +#if AUTOTUNE_ENABLED == ENABLED + // we need a pointer to autotune for the G2 table + void *autotune_ptr; +#endif }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 7c4306a2cf..62d72ebeec 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -247,8 +247,6 @@ enum LoggingParameters { LOG_DATA_INT32_MSG, LOG_DATA_UINT32_MSG, LOG_DATA_FLOAT_MSG, - LOG_AUTOTUNE_MSG, - LOG_AUTOTUNEDETAILS_MSG, LOG_MOTBATT_MSG, LOG_PARAMTUNE_MSG, LOG_HELI_MSG, diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 8cc3e8b0fc..39289674a0 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -439,6 +439,24 @@ private: }; #if AUTOTUNE_ENABLED == ENABLED +/* + wrapper class for AC_AutoTune + */ +class AutoTune : public AC_AutoTune +{ +public: + bool init() override; + void run() override; + +protected: + bool start(void) override; + bool position_ok() override; + float get_pilot_desired_climb_rate_cms(void) const override; + void get_pilot_desired_rp_yrate_cd(int32_t &roll_cd, int32_t &pitch_cd, int32_t &yaw_rate_cds) override; + void init_z_limits() override; + void Log_Write_Event(enum at_event id) override; +}; + class ModeAutoTune : public Mode { public: @@ -446,150 +464,19 @@ public: using Copter::Mode::Mode; bool init(bool ignore_checks) override; - void run() override; + void run() override; bool requires_GPS() const override { return false; } bool has_manual_throttle() const override { return false; } bool allows_arming(bool from_gcs) const override { return false; } bool is_autopilot() const override { return false; } - void save_tuning_gains(); - void stop(); protected: const char *name() const override { return "AUTOTUNE"; } const char *name4() const override { return "ATUN"; } - -private: - - bool start(bool ignore_checks); - - void autotune_attitude_control(); - void backup_gains_and_initialise(); - void load_orig_gains(); - void load_tuned_gains(); - void load_intra_test_gains(); - void load_twitch_gains(); - void update_gcs(uint8_t message_id); - bool roll_enabled(); - bool pitch_enabled(); - bool yaw_enabled(); - void twitching_test_rate(float rate, float rate_target, float &meas_rate_min, float &meas_rate_max); - void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max); - void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max); - void updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max); - void updating_rate_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max); - void updating_rate_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max); - void updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max); - void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max); - void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd); - -#if LOGGING_ENABLED == ENABLED - void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt); - void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds); -#endif - - void send_step_string(); - const char *level_issue_string() const; - const char * type_string() const; - void announce_state_to_gcs(); - void do_gcs_announcements(); - - enum LEVEL_ISSUE { - LEVEL_ISSUE_NONE, - LEVEL_ISSUE_ANGLE_ROLL, - LEVEL_ISSUE_ANGLE_PITCH, - LEVEL_ISSUE_ANGLE_YAW, - LEVEL_ISSUE_RATE_ROLL, - LEVEL_ISSUE_RATE_PITCH, - LEVEL_ISSUE_RATE_YAW, - }; - bool check_level(const enum LEVEL_ISSUE issue, const float current, const float maximum); - bool currently_level(); - - // autotune modes (high level states) - enum TuneMode { - UNINITIALISED = 0, // autotune has never been run - TUNING = 1, // autotune is testing gains - SUCCESS = 2, // tuning has completed, user is flight testing the new gains - FAILED = 3, // tuning has failed, user is flying on original gains - }; - - // steps performed while in the tuning mode - enum StepType { - WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch - TWITCHING = 1, // autotune has begun a twitch and is watching the resulting vehicle movement - UPDATE_GAINS = 2 // autotune has completed a twitch and is updating the gains based on the results - }; - - // things that can be tuned - enum AxisType { - ROLL = 0, // roll axis is being tuned (either angle or rate) - PITCH = 1, // pitch axis is being tuned (either angle or rate) - YAW = 2, // pitch axis is being tuned (either angle or rate) - }; - - // mini steps performed while in Tuning mode, Testing step - enum TuneType { - RD_UP = 0, // rate D is being tuned up - RD_DOWN = 1, // rate D is being tuned down - RP_UP = 2, // rate P is being tuned up - SP_DOWN = 3, // angle P is being tuned down - SP_UP = 4 // angle P is being tuned up - }; - - TuneMode mode : 2; // see TuneMode for what modes are allowed - bool pilot_override : 1; // true = pilot is overriding controls so we suspend tuning temporarily - AxisType axis : 2; // see AxisType for which things can be tuned - bool positive_direction : 1; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll) - StepType step : 2; // see StepType for what steps are performed - TuneType tune_type : 3; // see TuneType - bool ignore_next : 1; // true = ignore the next test - bool twitch_first_iter : 1; // true on first iteration of a twitch (used to signal we must step the attitude or rate target) - bool use_poshold : 1; // true = enable position hold - bool have_position : 1; // true = start_position is value - Vector3f start_position; - -// variables - uint32_t override_time; // the last time the pilot overrode the controls - float test_rate_min; // the minimum angular rate achieved during TESTING_RATE step - float test_rate_max; // the maximum angular rate achieved during TESTING_RATE step - float test_angle_min; // the minimum angle achieved during TESTING_ANGLE step - float test_angle_max; // the maximum angle achieved during TESTING_ANGLE step - uint32_t step_start_time; // start time of current tuning step (used for timeout checks) - uint32_t step_stop_time; // start time of current tuning step (used for timeout checks) - int8_t counter; // counter for tuning gains - float target_rate, start_rate; // target and start rate - float target_angle, start_angle; // target and start angles - float desired_yaw; // yaw heading during tune - float rate_max, test_accel_max; // maximum acceleration variables - - LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second - -// backup of currently being tuned parameter values - float orig_roll_rp = 0, orig_roll_ri, orig_roll_rd, orig_roll_sp, orig_roll_accel; - float orig_pitch_rp = 0, orig_pitch_ri, orig_pitch_rd, orig_pitch_sp, orig_pitch_accel; - float orig_yaw_rp = 0, orig_yaw_ri, orig_yaw_rd, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel; - bool orig_bf_feedforward; - -// currently being tuned parameter values - float tune_roll_rp, tune_roll_rd, tune_roll_sp, tune_roll_accel; - float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel; - float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel; - - uint32_t announce_time; - float lean_angle; - float rotation_rate; - float roll_cd, pitch_cd; - - struct { - LEVEL_ISSUE issue{LEVEL_ISSUE_NONE}; - float maximum; - float current; - } level_problem; - }; #endif diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index ef7ddda021..251a5722fd 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -1,168 +1,27 @@ #include "Copter.h" +/* + autotune mode is a wrapper around the AC_AutoTune library + */ + #if AUTOTUNE_ENABLED == ENABLED +bool Copter::AutoTune::init() +{ + // use position hold while tuning if we were in QLOITER + bool position_hold = (copter.control_mode == LOITER || copter.control_mode == POSHOLD); + + return init_internals(position_hold, + copter.attitude_control, + copter.pos_control, + copter.ahrs_view, + &copter.inertial_nav); +} + /* - * Init and run calls for autotune flight mode - * - * Instructions: - * 1) Set up one flight mode switch position to be AltHold. - * 2) Set the Ch7 Opt or Ch8 Opt to AutoTune to allow you to turn the auto tuning on/off with the ch7 or ch8 switch. - * 3) Ensure the ch7 or ch8 switch is in the LOW position. - * 4) Wait for a calm day and go to a large open area. - * 5) Take off and put the vehicle into AltHold mode at a comfortable altitude. - * 6) Set the ch7/ch8 switch to the HIGH position to engage auto tuning: - * a) You will see it twitch about 20 degrees left and right for a few minutes, then it will repeat forward and back. - * b) Use the roll and pitch stick at any time to reposition the copter if it drifts away (it will use the original PID gains during repositioning and between tests). - * When you release the sticks it will continue auto tuning where it left off. - * c) Move the ch7/ch8 switch into the LOW position at any time to abandon the autotuning and return to the origin PIDs. - * d) Make sure that you do not have any trim set on your transmitter or the autotune may not get the signal that the sticks are centered. - * 7) When the tune completes the vehicle will change back to the original PID gains. - * 8) Put the ch7/ch8 switch into the LOW position then back to the HIGH position to test the tuned PID gains. - * 9) Put the ch7/ch8 switch into the LOW position to fly using the original PID gains. - * 10) If you are happy with the autotuned PID gains, leave the ch7/ch8 switch in the HIGH position, land and disarm to save the PIDs permanently. - * If you DO NOT like the new PIDS, switch ch7/ch8 LOW to return to the original PIDs. The gains will not be saved when you disarm - * - * What it's doing during each "twitch": - * a) invokes 90 deg/sec rate request - * b) records maximum "forward" roll rate and bounce back rate - * c) when copter reaches 20 degrees or 1 second has passed, it commands level - * d) tries to keep max rotation rate between 80% ~ 100% of requested rate (90deg/sec) by adjusting rate P - * e) increases rate D until the bounce back becomes greater than 10% of requested rate (90deg/sec) - * f) decreases rate D until the bounce back becomes less than 10% of requested rate (90deg/sec) - * g) increases rate P until the max rotate rate becomes greater than the request rate (90deg/sec) - * h) invokes a 20deg angle request on roll or pitch - * i) increases stab P until the maximum angle becomes greater than 110% of the requested angle (20deg) - * j) decreases stab P by 25% - * + start autotune mode */ - -#define AUTOTUNE_AXIS_BITMASK_ROLL 1 -#define AUTOTUNE_AXIS_BITMASK_PITCH 2 -#define AUTOTUNE_AXIS_BITMASK_YAW 4 - -#define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds -#define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 1000 // timeout for tuning mode's testing step -#define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level -#define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch -#define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw -#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the copter to be level -#define AUTOTUNE_RD_STEP 0.05f // minimum increment when increasing/decreasing Rate D term -#define AUTOTUNE_RP_STEP 0.05f // minimum increment when increasing/decreasing Rate P term -#define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term -#define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing -#define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing -#define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing -#define AUTOTUNE_RD_MAX 0.200f // maximum Rate D value -#define AUTOTUNE_RLPF_MIN 1.0f // minimum Rate Yaw filter value -#define AUTOTUNE_RLPF_MAX 5.0f // maximum Rate Yaw filter value -#define AUTOTUNE_RP_MIN 0.01f // minimum Rate P value -#define AUTOTUNE_RP_MAX 2.0f // maximum Rate P value -#define AUTOTUNE_SP_MAX 20.0f // maximum Stab P value -#define AUTOTUNE_SP_MIN 0.5f // maximum Stab P value -#define AUTOTUNE_RP_ACCEL_MIN 4000.0f // Minimum acceleration for Roll and Pitch -#define AUTOTUNE_Y_ACCEL_MIN 1000.0f // Minimum acceleration for Yaw -#define AUTOTUNE_Y_FILT_FREQ 10.0f // Autotune filter frequency when testing Yaw -#define AUTOTUNE_SUCCESS_COUNT 4 // The number of successful iterations we need to freeze at current gains -#define AUTOTUNE_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in -#define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning -#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning -#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning -#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration -#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration - -// roll and pitch axes -#define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step -#define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step -#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step -#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step - -// yaw axis -#define AUTOTUNE_TARGET_ANGLE_YAW_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step -#define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step -#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD 500 // minimum target angle during TESTING_RATE step that will cause us to move to next step -#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step - -// Auto Tune message ids for ground station -#define AUTOTUNE_MESSAGE_STARTED 0 -#define AUTOTUNE_MESSAGE_STOPPED 1 -#define AUTOTUNE_MESSAGE_SUCCESS 2 -#define AUTOTUNE_MESSAGE_FAILED 3 -#define AUTOTUNE_MESSAGE_SAVED_GAINS 4 - -#define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000 - -// autotune_init - should be called when autotune mode is selected -bool Copter::ModeAutoTune::init(bool ignore_checks) -{ - bool success = true; - - switch (mode) { - case FAILED: - // autotune has been run but failed so reset state to uninitialized - mode = UNINITIALISED; - // fall through to restart the tuning - FALLTHROUGH; - - case UNINITIALISED: - // autotune has never been run - success = start(false); - if (success) { - // so store current gains as original gains - backup_gains_and_initialise(); - // advance mode to tuning - mode = TUNING; - // send message to ground station that we've started tuning - update_gcs(AUTOTUNE_MESSAGE_STARTED); - } - break; - - case TUNING: - // we are restarting tuning after the user must have switched ch7/ch8 off so we restart tuning where we left off - success = start(false); - if (success) { - // reset gains to tuning-start gains (i.e. low I term) - load_intra_test_gains(); - // write dataflash log even and send message to ground station - Log_Write_Event(DATA_AUTOTUNE_RESTART); - update_gcs(AUTOTUNE_MESSAGE_STARTED); - } - break; - - case SUCCESS: - // we have completed a tune and the pilot wishes to test the new gains in the current flight mode - // so simply apply tuning gains (i.e. do not change flight mode) - load_tuned_gains(); - Log_Write_Event(DATA_AUTOTUNE_PILOT_TESTING); - break; - } - - // only do position hold if starting autotune from LOITER or POSHOLD - use_poshold = (copter.control_mode == LOITER || copter.control_mode == POSHOLD); - have_position = false; - - return success; -} - -// stop - should be called when the ch7/ch8 switch is switched OFF -void Copter::ModeAutoTune::stop() -{ - // set gains to their original values - load_orig_gains(); - - // re-enable angle-to-rate request limits - attitude_control->use_sqrt_controller(true); - - // log off event and send message to ground station - update_gcs(AUTOTUNE_MESSAGE_STOPPED); - Log_Write_Event(DATA_AUTOTUNE_OFF); - - // Note: we leave the mode as it was so that we know how the autotune ended - // we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch -} - -// start - Initialize autotune flight mode -bool Copter::ModeAutoTune::start(bool ignore_checks) +bool Copter::AutoTune::start() { // only allow flip from Stabilize, AltHold, PosHold or Loiter modes if (copter.control_mode != STABILIZE && copter.control_mode != ALT_HOLD && @@ -171,1403 +30,146 @@ bool Copter::ModeAutoTune::start(bool ignore_checks) } // ensure throttle is above zero - if (ap.throttle_zero) { + if (copter.ap.throttle_zero) { return false; } // ensure we are flying - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (!copter.motors->armed() || !copter.ap.auto_armed || copter.ap.land_complete) { return false; } - // initialize vertical speeds and leash lengths - pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); - - // initialise position and desired velocity - if (!pos_control->is_active_z()) { - pos_control->set_alt_target_to_current_alt(); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); - } - - return true; + return AC_AutoTune::start(); } -const char *Copter::ModeAutoTune::level_issue_string() const +void Copter::AutoTune::run() { - switch (level_problem.issue) { - case LEVEL_ISSUE_NONE: - return "None"; - case LEVEL_ISSUE_ANGLE_ROLL: - return "Angle(R)"; - case LEVEL_ISSUE_ANGLE_PITCH: - return "Angle(P)"; - case LEVEL_ISSUE_ANGLE_YAW: - return "Angle(Y)"; - case LEVEL_ISSUE_RATE_ROLL: - return "Rate(R)"; - case LEVEL_ISSUE_RATE_PITCH: - return "Rate(P)"; - case LEVEL_ISSUE_RATE_YAW: - return "Rate(Y)"; - } - return "Bug"; -} - -void Copter::ModeAutoTune::send_step_string() -{ - if (pilot_override) { - 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: WFL (%s) (%f > %f)", level_issue_string(), (double)(level_problem.current*0.01f), (double)(level_problem.maximum*0.01f)); - return; - case UPDATE_GAINS: - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: UPDATING_GAINS"); - return; - case TWITCHING: - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: TWITCHING"); - return; - } - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: unknown step"); -} - -const char *Copter::ModeAutoTune::type_string() const -{ - switch (tune_type) { - case RD_UP: - return "Rate D Up"; - case RD_DOWN: - return "Rate D Down"; - case RP_UP: - return "Rate P Up"; - case SP_DOWN: - return "Angle P Down"; - case SP_UP: - return "Angle P Up"; - } - return "Bug"; -} - -void Copter::ModeAutoTune::do_gcs_announcements() -{ - const uint32_t now = millis(); - if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { - return; - } - float tune_rp = 0.0f; - float tune_rd = 0.0f; - float tune_sp = 0.0f; - float tune_accel = 0.0f; - char axis_char = '?'; - switch (axis) { - case ROLL: - tune_rp = tune_roll_rp; - tune_rd = tune_roll_rd; - tune_sp = tune_roll_sp; - tune_accel = tune_roll_accel; - axis_char = 'R'; - break; - case PITCH: - tune_rp = tune_pitch_rp; - tune_rd = tune_pitch_rd; - tune_sp = tune_pitch_sp; - tune_accel = tune_pitch_accel; - axis_char = 'P'; - break; - case YAW: - tune_rp = tune_yaw_rp; - tune_rd = tune_yaw_rLPF; - tune_sp = tune_yaw_sp; - tune_accel = tune_yaw_accel; - axis_char = 'Y'; - break; - } - - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: (%c) %s", axis_char, type_string()); - send_step_string(); - if (!is_zero(lean_angle)) { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: lean=%f target=%f", (double)lean_angle, (double)target_angle); - } - if (!is_zero(rotation_rate)) { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rotation=%f target=%f", (double)(rotation_rate*0.01f), (double)(target_rate*0.01f)); - } - switch (tune_type) { - case RD_UP: - case RD_DOWN: - case RP_UP: - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd); - break; - case SP_DOWN: - case SP_UP: - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f accel=%f", (double)tune_sp, (double)tune_accel); - break; - } - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: success %u/%u", counter, AUTOTUNE_SUCCESS_COUNT); - - announce_time = now; -} - -// run - runs the autotune flight mode -// should be called at 100hz or more -void Copter::ModeAutoTune::run() -{ - float target_roll, target_pitch; - float target_yaw_rate; - int16_t target_climb_rate; - - // initialize vertical speeds and acceleration - pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); - - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - // this should not actually be possible because of the init() checks - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); - pos_control->relax_alt_hold_controllers(0.0f); - return; - } - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); - - // get pilot desired lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max()); - - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - - // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - - // get avoidance adjusted climb rate - target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - - // check for pilot requested take-off - this should not actually be possible because of init() checks - if (ap.land_complete && target_climb_rate > 0) { - // indicate we are taking off - set_land_complete(false); - // clear i term when we're taking off - set_throttle_takeoff(); - } + copter.update_simple_mode(); // reset target lean angles and heading while landed - if (ap.land_complete) { + if (copter.ap.land_complete) { + // we are landed, shut down + float target_climb_rate = get_pilot_desired_climb_rate_cms(); + // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) if (target_climb_rate < 0.0f) { - motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); + copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); } else { - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - } - attitude_control->reset_rate_controller_I_terms(); - attitude_control->set_yaw_target_to_current_heading(); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - pos_control->relax_alt_hold_controllers(0.0f); - pos_control->update_z_controller(); - }else{ - // check if pilot is overriding the controls - bool zero_rp_input = is_zero(target_roll) && is_zero(target_pitch); - if (!zero_rp_input || !is_zero(target_yaw_rate) || target_climb_rate != 0) { - if (!pilot_override) { - pilot_override = true; - // set gains to their original values - load_orig_gains(); - attitude_control->use_sqrt_controller(true); - } - // reset pilot override time - override_time = millis(); - if (!zero_rp_input) { - // only reset position on roll or pitch input - have_position = false; - } - }else if (pilot_override) { - // check if we should resume tuning after pilot's override - if (millis() - override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) { - pilot_override = false; // turn off pilot override - // set gains to their intra-test values (which are very close to the original gains) - // load_intra_test_gains(); //I think we should be keeping the originals here to let the I term settle quickly - step = WAITING_FOR_LEVEL; // set tuning step back from beginning - desired_yaw = ahrs.yaw_sensor; - } + copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } + copter.attitude_control->reset_rate_controller_I_terms(); + copter.attitude_control->set_yaw_target_to_current_heading(); - if (zero_rp_input) { - // pilot input on throttle and yaw will still use position hold if enabled - get_poshold_attitude(target_roll, target_pitch, desired_yaw); - } + int32_t target_roll, target_pitch, target_yaw_rate; + get_pilot_desired_rp_yrate_cd(target_roll, target_pitch, target_yaw_rate); - // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // if pilot override call attitude controller - if (pilot_override || mode != TUNING) { - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - }else{ - // somehow get attitude requests from autotuning - autotune_attitude_control(); - // tell the user what's going on - do_gcs_announcements(); - } - - // call position controller - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control->update_z_controller(); + copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + copter.pos_control->relax_alt_hold_controllers(0.0f); + copter.pos_control->update_z_controller(); + } else { + // run autotune mode + AC_AutoTune::run(); } } -bool Copter::ModeAutoTune::check_level(const LEVEL_ISSUE issue, const float current, const float maximum) + +/* + get stick input climb rate + */ +float Copter::AutoTune::get_pilot_desired_climb_rate_cms(void) const { - if (current > maximum) { - level_problem.current = current; - level_problem.maximum = maximum; - level_problem.issue = issue; - return false; - } - return true; + float target_climb_rate = copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()); + + // get avoidance adjusted climb rate + target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate); + + return target_climb_rate; } -bool Copter::ModeAutoTune::currently_level() +/* + get stick roll, pitch and yaw rate + */ +void Copter::AutoTune::get_pilot_desired_rp_yrate_cd(int32_t &des_roll_cd, int32_t &des_pitch_cd, int32_t &yaw_rate_cds) { - if (!check_level(LEVEL_ISSUE_ANGLE_ROLL, - fabsf(ahrs.roll_sensor - roll_cd), - AUTOTUNE_LEVEL_ANGLE_CD)) { - return false; - } + // map from int32_t to float + float des_roll, des_pitch; - if (!check_level(LEVEL_ISSUE_ANGLE_PITCH, - fabsf(ahrs.pitch_sensor - pitch_cd), - AUTOTUNE_LEVEL_ANGLE_CD)) { - return false; - } - if (!check_level(LEVEL_ISSUE_ANGLE_YAW, - fabsf(wrap_180_cd(ahrs.yaw_sensor-(int32_t)desired_yaw)), - AUTOTUNE_LEVEL_ANGLE_CD)) { - return false; - } - if (!check_level(LEVEL_ISSUE_RATE_ROLL, - (ToDeg(ahrs.get_gyro().x) * 100.0f), - AUTOTUNE_LEVEL_RATE_RP_CD)) { - return false; - } - if (!check_level(LEVEL_ISSUE_RATE_PITCH, - (ToDeg(ahrs.get_gyro().y) * 100.0f), - AUTOTUNE_LEVEL_RATE_RP_CD)) { - return false; - } - if (!check_level(LEVEL_ISSUE_RATE_YAW, - (ToDeg(ahrs.get_gyro().z) * 100.0f), - AUTOTUNE_LEVEL_RATE_Y_CD)) { - return false; - } - return true; + copter.mode_autotune.get_pilot_desired_lean_angles(des_roll, des_pitch, copter.aparm.angle_max, + copter.attitude_control->get_althold_lean_angle_max()); + yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in()); + + des_roll_cd = des_roll; + des_pitch_cd = des_pitch; } -// attitude_controller - sets attitude control targets during tuning -void Copter::ModeAutoTune::autotune_attitude_control() +/* + setup z controller velocity and accel limits + */ +void Copter::AutoTune::init_z_limits() { - rotation_rate = 0.0f; // rotation rate in radians/second - lean_angle = 0.0f; - const float direction_sign = positive_direction ? 1.0f : -1.0f; - - // check tuning step - switch (step) { - - case WAITING_FOR_LEVEL: - // Note: we should be using intra-test gains (which are very close to the original gains but have lower I) - // re-enable rate limits - attitude_control->use_sqrt_controller(true); - - get_poshold_attitude(roll_cd, pitch_cd, desired_yaw); - - // hold level attitude - attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw, true); - - // hold the copter level for 0.5 seconds before we begin a twitch - // reset counter if we are no longer level - if (!currently_level()) { - step_start_time = millis(); - } - - // if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step - if (millis() - step_start_time >= AUTOTUNE_REQUIRED_LEVEL_TIME_MS) { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Twitch"); - // initiate variables for next step - step = TWITCHING; - step_start_time = millis(); - step_stop_time = step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS; - twitch_first_iter = true; - test_rate_max = 0.0f; - test_rate_min = 0.0f; - test_angle_max = 0.0f; - test_angle_min = 0.0f; - rotation_rate_filt.reset(0.0f); - rate_max = 0.0f; - // set gains to their to-be-tested values - load_twitch_gains(); - } - - switch (axis) { - case ROLL: - target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS); - target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); - start_rate = ToDeg(ahrs.get_gyro().x) * 100.0f; - start_angle = ahrs.roll_sensor; - rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_hz()*2.0f); - break; - case PITCH: - target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS); - target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); - start_rate = ToDeg(ahrs.get_gyro().y) * 100.0f; - start_angle = ahrs.pitch_sensor; - rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_hz()*2.0f); - break; - case YAW: - target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, AUTOTUNE_TARGET_RATE_YAW_CDS); - target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD); - start_rate = ToDeg(ahrs.get_gyro().z) * 100.0f; - start_angle = ahrs.yaw_sensor; - rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ); - break; - } - if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { - rotation_rate_filt.reset(start_rate); - } else { - rotation_rate_filt.reset(0); - } - break; - - case TWITCHING: { - // Run the twitching step - // Note: we should be using intra-test gains (which are very close to the original gains but have lower I) - - // disable rate limits - attitude_control->use_sqrt_controller(false); - // hold current attitude - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - - if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { - // step angle targets on first iteration - if (twitch_first_iter) { - twitch_first_iter = false; - // Testing increasing stabilize P gain so will set lean angle target - switch (axis) { - case ROLL: - // request roll to 20deg - attitude_control->input_angle_step_bf_roll_pitch_yaw(direction_sign * target_angle, 0.0f, 0.0f); - break; - case PITCH: - // request pitch to 20deg - attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, direction_sign * target_angle, 0.0f); - break; - case YAW: - // request pitch to 20deg - attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, 0.0f, direction_sign * target_angle); - break; - } - } - } else { - // Testing rate P and D gains so will set body-frame rate targets. - // Rate controller will use existing body-frame rates and convert to motor outputs - // for all axes except the one we override here. - switch (axis) { - case ROLL: - // override body-frame roll rate - attitude_control->rate_bf_roll_target(direction_sign * target_rate + start_rate); - break; - case PITCH: - // override body-frame pitch rate - attitude_control->rate_bf_pitch_target(direction_sign * target_rate + start_rate); - break; - case YAW: - // override body-frame yaw rate - attitude_control->rate_bf_yaw_target(direction_sign * target_rate + start_rate); - break; - } - } - - // capture this iterations rotation rate and lean angle - float gyro_reading = 0; - switch (axis) { - case ROLL: - gyro_reading = ahrs.get_gyro().x; - lean_angle = direction_sign * (ahrs.roll_sensor - (int32_t)start_angle); - break; - case PITCH: - gyro_reading = ahrs.get_gyro().y; - lean_angle = direction_sign * (ahrs.pitch_sensor - (int32_t)start_angle); - break; - case YAW: - gyro_reading = ahrs.get_gyro().z; - lean_angle = direction_sign * wrap_180_cd(ahrs.yaw_sensor-(int32_t)start_angle); - break; - } - - // Add filter to measurements - float filter_value; - switch (tune_type) { - case SP_DOWN: - case SP_UP: - filter_value = direction_sign * (ToDeg(gyro_reading) * 100.0f); - break; - default: - filter_value = direction_sign * (ToDeg(gyro_reading) * 100.0f - start_rate); - break; - } - rotation_rate = rotation_rate_filt.apply(filter_value, - copter.scheduler.get_loop_period_s()); - - switch (tune_type) { - case RD_UP: - case RD_DOWN: - twitching_test_rate(rotation_rate, target_rate, test_rate_min, test_rate_max); - twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); - if (lean_angle >= target_angle) { - step = UPDATE_GAINS; - } - break; - case RP_UP: - twitching_test_rate(rotation_rate, target_rate*(1+0.5f*g.autotune_aggressiveness), test_rate_min, test_rate_max); - twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); - if (lean_angle >= target_angle) { - step = UPDATE_GAINS; - } - break; - case SP_DOWN: - case SP_UP: - twitching_test_angle(lean_angle, rotation_rate, target_angle*(1+0.5f*g.autotune_aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max); - twitching_measure_acceleration(test_accel_max, rotation_rate - direction_sign * start_rate, rate_max); - break; - } - - // log this iterations lean angle and rotation rate -#if LOGGING_ENABLED == ENABLED - Log_Write_AutoTuneDetails(lean_angle, rotation_rate); - copter.DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control); -#endif - break; - } - case UPDATE_GAINS: - - // re-enable rate limits - attitude_control->use_sqrt_controller(true); - -#if LOGGING_ENABLED == ENABLED - // log the latest gains - if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { - switch (axis) { - case ROLL: - Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); - break; - case PITCH: - Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); - break; - case YAW: - Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max); - break; - } - } else { - switch (axis) { - case ROLL: - Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); - break; - case PITCH: - Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); - break; - case YAW: - Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max); - break; - } - } -#endif - - // Check results after mini-step to increase rate D gain - switch (tune_type) { - case RD_UP: - switch (axis) { - case ROLL: - updating_rate_d_up(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); - break; - case PITCH: - updating_rate_d_up(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); - break; - case YAW: - updating_rate_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); - break; - } - break; - // Check results after mini-step to decrease rate D gain - case RD_DOWN: - switch (axis) { - case ROLL: - updating_rate_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); - break; - case PITCH: - updating_rate_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); - break; - case YAW: - updating_rate_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); - break; - } - break; - // Check results after mini-step to increase rate P gain - case RP_UP: - switch (axis) { - case ROLL: - updating_rate_p_up_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); - break; - case PITCH: - updating_rate_p_up_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); - break; - case YAW: - updating_rate_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); - break; - } - break; - // Check results after mini-step to increase stabilize P gain - case SP_DOWN: - switch (axis) { - case ROLL: - updating_angle_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); - break; - case PITCH: - updating_angle_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); - break; - case YAW: - updating_angle_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); - break; - } - break; - // Check results after mini-step to increase stabilize P gain - case SP_UP: - switch (axis) { - case ROLL: - updating_angle_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); - break; - case PITCH: - updating_angle_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); - break; - case YAW: - updating_angle_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); - break; - } - break; - } - - // we've complete this step, finalize pids and move to next step - if (counter >= AUTOTUNE_SUCCESS_COUNT) { - - // reset counter - counter = 0; - - // move to the next tuning type - switch (tune_type) { - case RD_UP: - tune_type = TuneType(tune_type + 1); - break; - case RD_DOWN: - tune_type = TuneType(tune_type + 1); - switch (axis) { - case ROLL: - tune_roll_rd = MAX(g.autotune_min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF); - tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF); - break; - case PITCH: - tune_pitch_rd = MAX(g.autotune_min_d, tune_pitch_rd * AUTOTUNE_RD_BACKOFF); - tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF); - break; - case YAW: - tune_yaw_rLPF = MAX(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF); - tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF); - break; - } - break; - case RP_UP: - tune_type = TuneType(tune_type + 1); - switch (axis) { - case ROLL: - tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF); - break; - case PITCH: - tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF); - break; - case YAW: - tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF); - break; - } - break; - case SP_DOWN: - tune_type = TuneType(tune_type + 1); - break; - case SP_UP: - // we've reached the end of a D-up-down PI-up-down tune type cycle - tune_type = RD_UP; - - // advance to the next axis - bool complete = false; - switch (axis) { - case ROLL: - tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF); - tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); - if (pitch_enabled()) { - axis = PITCH; - } else if (yaw_enabled()) { - axis = YAW; - } else { - complete = true; - } - break; - case PITCH: - tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF); - tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); - if (yaw_enabled()) { - axis = YAW; - } else { - complete = true; - } - break; - case YAW: - tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF); - tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF); - complete = true; - break; - } - - // if we've just completed all axes we have successfully completed the autotune - // change to TESTING mode to allow user to fly with new gains - if (complete) { - mode = SUCCESS; - update_gcs(AUTOTUNE_MESSAGE_SUCCESS); - Log_Write_Event(DATA_AUTOTUNE_SUCCESS); - AP_Notify::events.autotune_complete = true; - } else { - AP_Notify::events.autotune_next_axis = true; - } - break; - } - } - - // reverse direction - positive_direction = !positive_direction; - - if (axis == YAW) { - attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs.yaw_sensor, false); - } - - // set gains to their intra-test values (which are very close to the original gains) - load_intra_test_gains(); - - // reset testing step - step = WAITING_FOR_LEVEL; - step_start_time = millis(); - break; - } + copter.pos_control->set_max_speed_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up); + copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z); } -// backup_gains_and_initialise - store current gains as originals -// called before tuning starts to backup original gains -void Copter::ModeAutoTune::backup_gains_and_initialise() + +/* + Write an event packet. This maps from AC_AutoTune event IDs to + copter event IDs +*/ +void Copter::AutoTune::Log_Write_Event(enum at_event id) { - // initialise state because this is our first time - if (roll_enabled()) { - axis = ROLL; - } else if (pitch_enabled()) { - axis = PITCH; - } else if (yaw_enabled()) { - axis = YAW; - } - positive_direction = false; - step = WAITING_FOR_LEVEL; - step_start_time = millis(); - tune_type = RD_UP; - - desired_yaw = ahrs.yaw_sensor; - - g.autotune_aggressiveness = constrain_float(g.autotune_aggressiveness, 0.05f, 0.2f); - - orig_bf_feedforward = attitude_control->get_bf_feedforward(); - - // backup original pids and initialise tuned pid values - orig_roll_rp = attitude_control->get_rate_roll_pid().kP(); - orig_roll_ri = attitude_control->get_rate_roll_pid().kI(); - orig_roll_rd = attitude_control->get_rate_roll_pid().kD(); - orig_roll_sp = attitude_control->get_angle_roll_p().kP(); - orig_roll_accel = attitude_control->get_accel_roll_max(); - tune_roll_rp = attitude_control->get_rate_roll_pid().kP(); - tune_roll_rd = attitude_control->get_rate_roll_pid().kD(); - tune_roll_sp = attitude_control->get_angle_roll_p().kP(); - tune_roll_accel = attitude_control->get_accel_roll_max(); - - orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP(); - orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI(); - orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD(); - orig_pitch_sp = attitude_control->get_angle_pitch_p().kP(); - orig_pitch_accel = attitude_control->get_accel_pitch_max(); - tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP(); - tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD(); - tune_pitch_sp = attitude_control->get_angle_pitch_p().kP(); - tune_pitch_accel = attitude_control->get_accel_pitch_max(); - - orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP(); - orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI(); - orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD(); - orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz(); - orig_yaw_accel = attitude_control->get_accel_yaw_max(); - orig_yaw_sp = attitude_control->get_angle_yaw_p().kP(); - tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP(); - tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz(); - tune_yaw_sp = attitude_control->get_angle_yaw_p().kP(); - tune_yaw_accel = attitude_control->get_accel_yaw_max(); - - Log_Write_Event(DATA_AUTOTUNE_INITIALISED); -} - -// load_orig_gains - set gains to their original values -// called by stop and failed functions -void Copter::ModeAutoTune::load_orig_gains() -{ - attitude_control->bf_feedforward(orig_bf_feedforward); - if (roll_enabled()) { - if (!is_zero(orig_roll_rp)) { - attitude_control->get_rate_roll_pid().kP(orig_roll_rp); - attitude_control->get_rate_roll_pid().kI(orig_roll_ri); - attitude_control->get_rate_roll_pid().kD(orig_roll_rd); - attitude_control->get_angle_roll_p().kP(orig_roll_sp); - attitude_control->set_accel_roll_max(orig_roll_accel); - } - } - if (pitch_enabled()) { - if (!is_zero(orig_pitch_rp)) { - attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri); - attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd); - attitude_control->get_angle_pitch_p().kP(orig_pitch_sp); - attitude_control->set_accel_pitch_max(orig_pitch_accel); - } - } - if (yaw_enabled()) { - if (!is_zero(orig_yaw_rp)) { - attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri); - attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd); - attitude_control->get_rate_yaw_pid().filt_hz(orig_yaw_rLPF); - attitude_control->get_angle_yaw_p().kP(orig_yaw_sp); - attitude_control->set_accel_yaw_max(orig_yaw_accel); - } - } -} - -// load_tuned_gains - load tuned gains -void Copter::ModeAutoTune::load_tuned_gains() -{ - if (!attitude_control->get_bf_feedforward()) { - attitude_control->bf_feedforward(true); - attitude_control->set_accel_roll_max(0.0f); - attitude_control->set_accel_pitch_max(0.0f); - } - if (roll_enabled()) { - if (!is_zero(tune_roll_rp)) { - attitude_control->get_rate_roll_pid().kP(tune_roll_rp); - attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); - attitude_control->get_rate_roll_pid().kD(tune_roll_rd); - attitude_control->get_angle_roll_p().kP(tune_roll_sp); - attitude_control->set_accel_roll_max(tune_roll_accel); - } - } - if (pitch_enabled()) { - if (!is_zero(tune_pitch_rp)) { - attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); - attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); - attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); - attitude_control->set_accel_pitch_max(tune_pitch_accel); - } - } - if (yaw_enabled()) { - if (!is_zero(tune_yaw_rp)) { - attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); - attitude_control->get_rate_yaw_pid().kD(0.0f); - attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF); - attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); - attitude_control->set_accel_yaw_max(tune_yaw_accel); - } - } -} - -// load_intra_test_gains - gains used between tests -// called during testing mode's update-gains step to set gains ahead of return-to-level step -void Copter::ModeAutoTune::load_intra_test_gains() -{ - // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term) - // sanity check the gains - attitude_control->bf_feedforward(true); - if (roll_enabled()) { - attitude_control->get_rate_roll_pid().kP(orig_roll_rp); - attitude_control->get_rate_roll_pid().kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); - attitude_control->get_rate_roll_pid().kD(orig_roll_rd); - attitude_control->get_angle_roll_p().kP(orig_roll_sp); - } - if (pitch_enabled()) { - attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); - attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd); - attitude_control->get_angle_pitch_p().kP(orig_pitch_sp); - } - if (yaw_enabled()) { - attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); - attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd); - attitude_control->get_rate_yaw_pid().filt_hz(orig_yaw_rLPF); - attitude_control->get_angle_yaw_p().kP(orig_yaw_sp); - } -} - -// load_twitch_gains - load the to-be-tested gains for a single axis -// called by attitude_control() just before it beings testing a gain (i.e. just before it twitches) -void Copter::ModeAutoTune::load_twitch_gains() -{ - switch (axis) { - case ROLL: - attitude_control->get_rate_roll_pid().kP(tune_roll_rp); - attitude_control->get_rate_roll_pid().kI(tune_roll_rp*0.01f); - attitude_control->get_rate_roll_pid().kD(tune_roll_rd); - attitude_control->get_angle_roll_p().kP(tune_roll_sp); - break; - case PITCH: - attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*0.01f); - attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); - attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); - break; - case YAW: - attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*0.01f); - attitude_control->get_rate_yaw_pid().kD(0.0f); - attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF); - attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); + const struct { + enum at_event eid; + uint8_t id; + } map[] = { + { EVENT_AUTOTUNE_INITIALISED, DATA_AUTOTUNE_INITIALISED }, + { EVENT_AUTOTUNE_OFF, DATA_AUTOTUNE_OFF }, + { EVENT_AUTOTUNE_RESTART, DATA_AUTOTUNE_RESTART }, + { EVENT_AUTOTUNE_SUCCESS, DATA_AUTOTUNE_SUCCESS }, + { EVENT_AUTOTUNE_FAILED, DATA_AUTOTUNE_FAILED }, + { EVENT_AUTOTUNE_REACHED_LIMIT, DATA_AUTOTUNE_REACHED_LIMIT }, + { EVENT_AUTOTUNE_PILOT_TESTING, DATA_AUTOTUNE_PILOT_TESTING }, + { EVENT_AUTOTUNE_SAVEDGAINS, DATA_AUTOTUNE_SAVEDGAINS }, + }; + for (uint8_t i=0; iget_bf_feedforward()) { - attitude_control->bf_feedforward_save(true); - attitude_control->save_accel_roll_max(0.0f); - attitude_control->save_accel_pitch_max(0.0f); - } - - // sanity check the rate P values - if (roll_enabled() && !is_zero(tune_roll_rp)) { - // rate roll gains - attitude_control->get_rate_roll_pid().kP(tune_roll_rp); - attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); - attitude_control->get_rate_roll_pid().kD(tune_roll_rd); - attitude_control->get_rate_roll_pid().save_gains(); - - // stabilize roll - attitude_control->get_angle_roll_p().kP(tune_roll_sp); - attitude_control->get_angle_roll_p().save_gains(); - - // acceleration roll - attitude_control->save_accel_roll_max(tune_roll_accel); - - // resave pids to originals in case the autotune is run again - orig_roll_rp = attitude_control->get_rate_roll_pid().kP(); - orig_roll_ri = attitude_control->get_rate_roll_pid().kI(); - orig_roll_rd = attitude_control->get_rate_roll_pid().kD(); - orig_roll_sp = attitude_control->get_angle_roll_p().kP(); - orig_roll_accel = attitude_control->get_accel_roll_max(); - } - - if (pitch_enabled() && !is_zero(tune_pitch_rp)) { - // rate pitch gains - attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); - attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); - attitude_control->get_rate_pitch_pid().save_gains(); - - // stabilize pitch - attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); - attitude_control->get_angle_pitch_p().save_gains(); - - // acceleration pitch - attitude_control->save_accel_pitch_max(tune_pitch_accel); - - // resave pids to originals in case the autotune is run again - orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP(); - orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI(); - orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD(); - orig_pitch_sp = attitude_control->get_angle_pitch_p().kP(); - orig_pitch_accel = attitude_control->get_accel_pitch_max(); - } - - if (yaw_enabled() && !is_zero(tune_yaw_rp)) { - // rate yaw gains - attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); - attitude_control->get_rate_yaw_pid().kD(0.0f); - attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF); - attitude_control->get_rate_yaw_pid().save_gains(); - - // stabilize yaw - attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); - attitude_control->get_angle_yaw_p().save_gains(); - - // acceleration yaw - attitude_control->save_accel_yaw_max(tune_yaw_accel); - - // resave pids to originals in case the autotune is run again - orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP(); - orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI(); - orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD(); - orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz(); - orig_yaw_sp = attitude_control->get_angle_yaw_p().kP(); - orig_yaw_accel = attitude_control->get_accel_pitch_max(); - } - // update GCS and log save gains event - update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS); - Log_Write_Event(DATA_AUTOTUNE_SAVEDGAINS); - // reset Autotune so that gains are not saved again and autotune can be run again. - mode = UNINITIALISED; - } + copter.autotune.save_tuning_gains(); } -// update_gcs - send message to ground station -void Copter::ModeAutoTune::update_gcs(uint8_t message_id) +void Copter::ModeAutoTune::stop() { - switch (message_id) { - case AUTOTUNE_MESSAGE_STARTED: - gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Started"); - break; - case AUTOTUNE_MESSAGE_STOPPED: - gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Stopped"); - break; - case AUTOTUNE_MESSAGE_SUCCESS: - gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Success"); - break; - case AUTOTUNE_MESSAGE_FAILED: - gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Failed"); - break; - case AUTOTUNE_MESSAGE_SAVED_GAINS: - gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Saved gains"); - break; - } -} - -// axis helper functions -inline bool Copter::ModeAutoTune::roll_enabled() { - return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_ROLL; -} - -inline bool Copter::ModeAutoTune::pitch_enabled() { - return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_PITCH; -} - -inline bool Copter::ModeAutoTune::yaw_enabled() { - return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW; -} - -// twitching_test_rate - twitching tests -// update min and max and test for end conditions -void Copter::ModeAutoTune::twitching_test_rate(float rate, float rate_target_max, float &meas_rate_min, float &meas_rate_max) -{ - // capture maximum rate - if (rate > meas_rate_max) { - // the measurement is continuing to increase without stopping - meas_rate_max = rate; - meas_rate_min = rate; - } - - // capture minimum measurement after the measurement has peaked (aka "bounce back") - if ((rate < meas_rate_min) && (meas_rate_max > rate_target_max * 0.5f)) { - // the measurement is bouncing back - meas_rate_min = rate; - } - - // calculate early stopping time based on the time it takes to get to 75% - if (meas_rate_max < rate_target_max * 0.75f) { - // the measurement not reached the 75% threshold yet - step_stop_time = step_start_time + (millis() - step_start_time) * 3.0f; - step_stop_time = MIN(step_stop_time, step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); - } - - if (meas_rate_max > rate_target_max) { - // the measured rate has passed the maximum target rate - step = UPDATE_GAINS; - } - - if (meas_rate_max-meas_rate_min > meas_rate_max*g.autotune_aggressiveness) { - // the measurement has passed 50% of the maximum rate and bounce back is larger than the threshold - step = UPDATE_GAINS; - } - - if (millis() >= step_stop_time) { - // we have passed the maximum stop time - step = UPDATE_GAINS; - } -} - -// twitching_test_angle - twitching tests -// update min and max and test for end conditions -void Copter::ModeAutoTune::twitching_test_angle(float angle, float rate, float angle_target_max, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max) -{ - // capture maximum angle - if (angle > meas_angle_max) { - // the angle still increasing - meas_angle_max = angle; - meas_angle_min = angle; - } - - // capture minimum angle after we have reached a reasonable maximum angle - if ((angle < meas_angle_min) && (meas_angle_max > angle_target_max * 0.5f)) { - // the measurement is bouncing back - meas_angle_min = angle; - } - - // capture maximum rate - if (rate > meas_rate_max) { - // the measurement is still increasing - meas_rate_max = rate; - meas_rate_min = rate; - } - - // capture minimum rate after we have reached maximum rate - if (rate < meas_rate_min) { - // the measurement is still decreasing - meas_rate_min = rate; - } - - // calculate early stopping time based on the time it takes to get to 75% - if (meas_angle_max < angle_target_max * 0.75f) { - // the measurement not reached the 75% threshold yet - step_stop_time = step_start_time + (millis() - step_start_time) * 3.0f; - step_stop_time = MIN(step_stop_time, step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); - } - - if (meas_angle_max > angle_target_max) { - // the measurement has passed the maximum angle - step = UPDATE_GAINS; - } - - if (meas_angle_max-meas_angle_min > meas_angle_max*g.autotune_aggressiveness) { - // the measurement has passed 50% of the maximum angle and bounce back is larger than the threshold - step = UPDATE_GAINS; - } - - if (millis() >= step_stop_time) { - // we have passed the maximum stop time - step = UPDATE_GAINS; - } -} - -// twitching_measure_acceleration - measure rate of change of measurement -void Copter::ModeAutoTune::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) -{ - if (rate_measurement_max < rate_measurement) { - rate_measurement_max = rate_measurement; - rate_of_change = (1000.0f*rate_measurement_max)/(millis() - step_start_time); - } -} - -// updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back -// optimize D term while keeping the maximum just below the target by adjusting P -void Copter::ModeAutoTune::updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max) -{ - if (meas_rate_max > rate_target) { - // if maximum measurement was higher than target - // reduce P gain (which should reduce maximum) - tune_p -= tune_p*tune_p_step_ratio; - if (tune_p < tune_p_min) { - // P gain is at minimum so start reducing D - tune_p = tune_p_min; - tune_d -= tune_d*tune_d_step_ratio; - if (tune_d <= tune_d_min) { - // We have reached minimum D gain so stop tuning - tune_d = tune_d_min; - counter = AUTOTUNE_SUCCESS_COUNT; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - } - }else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { - // we have not achieved a high enough maximum to get a good measurement of bounce back. - // increase P gain (which should increase maximum) - tune_p += tune_p*tune_p_step_ratio; - if (tune_p >= tune_p_max) { - tune_p = tune_p_max; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - }else{ - // we have a good measurement of bounce back - if (meas_rate_max-meas_rate_min > meas_rate_max*g.autotune_aggressiveness) { - // ignore the next result unless it is the same as this one - ignore_next = true; - // bounce back is bigger than our threshold so increment the success counter - counter++; - }else{ - if (ignore_next == false) { - // bounce back is smaller than our threshold so decrement the success counter - if (counter > 0 ) { - counter--; - } - // increase D gain (which should increase bounce back) - tune_d += tune_d*tune_d_step_ratio*2.0f; - // stop tuning if we hit maximum D - if (tune_d >= tune_d_max) { - tune_d = tune_d_max; - counter = AUTOTUNE_SUCCESS_COUNT; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - } else { - ignore_next = false; - } - } - } -} - -// updating_rate_d_down - decrease D and adjust P to optimize the D term for no bounce back -// optimize D term while keeping the maximum just below the target by adjusting P -void Copter::ModeAutoTune::updating_rate_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max) -{ - if (meas_rate_max > rate_target) { - // if maximum measurement was higher than target - // reduce P gain (which should reduce maximum) - tune_p -= tune_p*tune_p_step_ratio; - if (tune_p < tune_p_min) { - // P gain is at minimum so start reducing D gain - tune_p = tune_p_min; - tune_d -= tune_d*tune_d_step_ratio; - if (tune_d <= tune_d_min) { - // We have reached minimum D so stop tuning - tune_d = tune_d_min; - counter = AUTOTUNE_SUCCESS_COUNT; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - } - }else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { - // we have not achieved a high enough maximum to get a good measurement of bounce back. - // increase P gain (which should increase maximum) - tune_p += tune_p*tune_p_step_ratio; - if (tune_p >= tune_p_max) { - tune_p = tune_p_max; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - }else{ - // we have a good measurement of bounce back - if (meas_rate_max-meas_rate_min < meas_rate_max*g.autotune_aggressiveness) { - if (ignore_next == false) { - // bounce back is less than our threshold so increment the success counter - counter++; - } else { - ignore_next = false; - } - }else{ - // ignore the next result unless it is the same as this one - ignore_next = true; - // bounce back is larger than our threshold so decrement the success counter - if (counter > 0 ) { - counter--; - } - // decrease D gain (which should decrease bounce back) - tune_d -= tune_d*tune_d_step_ratio; - // stop tuning if we hit minimum D - if (tune_d <= tune_d_min) { - tune_d = tune_d_min; - counter = AUTOTUNE_SUCCESS_COUNT; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - } - } -} - -// updating_rate_p_up_d_down - increase P to ensure the target is reached while checking bounce back isn't increasing -// P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold -void Copter::ModeAutoTune::updating_rate_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max) -{ - if (meas_rate_max > rate_target*(1+0.5f*g.autotune_aggressiveness)) { - // ignore the next result unless it is the same as this one - ignore_next = true; - // if maximum measurement was greater than target so increment the success counter - counter++; - } else if ((meas_rate_max < rate_target) && (meas_rate_max > rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (meas_rate_max-meas_rate_min > meas_rate_max*g.autotune_aggressiveness) && (tune_d > tune_d_min)) { - // if bounce back was larger than the threshold so decrement the success counter - if (counter > 0 ) { - counter--; - } - // decrease D gain (which should decrease bounce back) - tune_d -= tune_d*tune_d_step_ratio; - // do not decrease the D term past the minimum - if (tune_d <= tune_d_min) { - tune_d = tune_d_min; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - // decrease P gain to match D gain reduction - tune_p -= tune_p*tune_p_step_ratio; - // do not decrease the P term past the minimum - if (tune_p <= tune_p_min) { - tune_p = tune_p_min; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - // cancel change in direction - positive_direction = !positive_direction; - }else{ - if (ignore_next == false) { - // if maximum measurement was lower than target so decrement the success counter - if (counter > 0 ) { - counter--; - } - // increase P gain (which should increase the maximum) - tune_p += tune_p*tune_p_step_ratio; - // stop tuning if we hit maximum P - if (tune_p >= tune_p_max) { - tune_p = tune_p_max; - counter = AUTOTUNE_SUCCESS_COUNT; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - } else { - ignore_next = false; - } - } -} - -// updating_angle_p_down - decrease P until we don't reach the target before time out -// P is decreased to ensure we are not overshooting the target -void Copter::ModeAutoTune::updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max) -{ - if (meas_angle_max < angle_target*(1+0.5f*g.autotune_aggressiveness)) { - if (ignore_next == false) { - // if maximum measurement was lower than target so increment the success counter - counter++; - } else { - ignore_next = false; - } - }else{ - // ignore the next result unless it is the same as this one - ignore_next = true; - // if maximum measurement was higher than target so decrement the success counter - if (counter > 0 ) { - counter--; - } - // decrease P gain (which should decrease the maximum) - tune_p -= tune_p*tune_p_step_ratio; - // stop tuning if we hit maximum P - if (tune_p <= tune_p_min) { - tune_p = tune_p_min; - counter = AUTOTUNE_SUCCESS_COUNT; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - } -} - -// updating_angle_p_up - increase P to ensure the target is reached -// P is increased until we achieve our target within a reasonable time -void Copter::ModeAutoTune::updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max) -{ - if ((meas_angle_max > angle_target*(1+0.5f*g.autotune_aggressiveness)) || - ((meas_angle_max > angle_target) && (meas_rate_min < -meas_rate_max*g.autotune_aggressiveness))) { - // ignore the next result unless it is the same as this one - ignore_next = true; - // if maximum measurement was greater than target so increment the success counter - counter++; - }else{ - if (ignore_next == false) { - // if maximum measurement was lower than target so decrement the success counter - if (counter > 0 ) { - counter--; - } - // increase P gain (which should increase the maximum) - tune_p += tune_p*tune_p_step_ratio; - // stop tuning if we hit maximum P - if (tune_p >= tune_p_max) { - tune_p = tune_p_max; - counter = AUTOTUNE_SUCCESS_COUNT; - Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); - } - } else { - ignore_next = false; - } - } -} - -// get attitude for slow position hold in autotune mode -void Copter::ModeAutoTune::get_poshold_attitude(float &roll_cd_out, float &pitch_cd_out, float &yaw_cd_out) -{ - roll_cd_out = pitch_cd_out = 0; - - if (!use_poshold) { - // we are not trying to hold position - return; - } - - // do we know where we are? - if (!copter.position_ok()) { - return; - } - - if (!have_position) { - have_position = true; - start_position = inertial_nav.get_position(); - } - - // don't go past 10 degrees, as autotune result would deteriorate too much - const float angle_max_cd = 1000; - - // hit the 10 degree limit at 20 meters position error - const float dist_limit_cm = 2000; - - // we only start adjusting yaw if we are more than 5m from the - // target position. That corresponds to a lean angle of 2.5 degrees - const float yaw_dist_limit_cm = 500; - - Vector3f pdiff = inertial_nav.get_position() - start_position; - pdiff.z = 0; - float dist_cm = pdiff.length(); - if (dist_cm < 10) { - // don't do anything within 10cm - return; - } - - /* - very simple linear controller - */ - float scaling = constrain_float(angle_max_cd * dist_cm / dist_limit_cm, 0, angle_max_cd); - Vector2f angle_ne(pdiff.x, pdiff.y); - angle_ne *= scaling / dist_cm; - - // rotate into body frame - pitch_cd_out = angle_ne.x * ahrs.cos_yaw() + angle_ne.y * ahrs.sin_yaw(); - roll_cd_out = angle_ne.x * ahrs.sin_yaw() - angle_ne.y * ahrs.cos_yaw(); - - if (dist_cm < yaw_dist_limit_cm) { - // no yaw adjustment - return; - } - - /* - also point so that twitching occurs perpendicular to the wind, - if we have drifted more than yaw_dist_limit_cm from the desired - position. This ensures that autotune doesn't have to deal with - more than 2.5 degrees of attitude on the axis it is tuning - */ - float target_yaw_cd = degrees(atan2f(pdiff.y, pdiff.x)) * 100; - if (axis == PITCH) { - // for roll and yaw tuning we point along the wind, for pitch - // we point across the wind - target_yaw_cd += 9000; - } - // go to the nearest 180 degree mark, with 5 degree slop to prevent oscillation - if (fabsf(yaw_cd_out - target_yaw_cd) > 9500) { - target_yaw_cd += 18000; - } - - yaw_cd_out = target_yaw_cd; + copter.autotune.stop(); } #endif // AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 78e9e038a5..df4d35bee1 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -551,7 +551,7 @@ void Copter::allocate_motors(void) } AP_Param::load_object_from_eeprom(motors, motors_var_info); - AP_AHRS_View *ahrs_view = ahrs.create_view(ROTATION_NONE); + ahrs_view = ahrs.create_view(ROTATION_NONE); if (ahrs_view == nullptr) { AP_HAL::panic("Unable to allocate AP_AHRS_View"); } diff --git a/ArduCopter/wscript b/ArduCopter/wscript index cb84c7dd10..05ee8aa444 100644 --- a/ArduCopter/wscript +++ b/ArduCopter/wscript @@ -32,6 +32,7 @@ def build(bld): 'AP_Follow', 'AP_Devo_Telem', 'AP_OSD', + 'AC_AutoTune', ], )