Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Autotune: add heli autotune with review comments incorporated

Browse Source
This commit is contained in:
bnsgeyer 2020-10-19 22:51:05 -04:00 committed by Bill Geyer
parent 9bec232c8a
commit fae1917aa7
6 changed files with 2393 additions and 684 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1365
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

File diff suppressed because it is too large Load Diff

169
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -16,14 +16,35 @@
support for autotune of multirotors. Based on original autotune code from ArduCopter, written by Leonard Hall support for autotune of multirotors. Based on original autotune code from ArduCopter, written by Leonard Hall
Converted to a library by Andrew Tridgell Converted to a library by Andrew Tridgell
*/ */
#pragma once #pragma once
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> #include <AC_AttitudeControl/AC_AttitudeControl.h>
#include <AC_AttitudeControl/AC_PosControl.h> #include <AC_AttitudeControl/AC_PosControl.h>
#include <AP_Math/AP_Math.h>
class AC_AutoTune { #define AUTOTUNE_AXIS_BITMASK_ROLL 1
#define AUTOTUNE_AXIS_BITMASK_PITCH 2
#define AUTOTUNE_AXIS_BITMASK_YAW 4
#define AUTOTUNE_SUCCESS_COUNT 4 // The number of successful iterations we need to freeze at current gains
// 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_MESSAGE_TESTING 5
#define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000
#define AUTOTUNE_DWELL_CYCLES 10
class AC_AutoTune
{
public: public:
// constructor // constructor
AC_AutoTune(); AC_AutoTune();
@ -32,7 +53,7 @@ public:
virtual void run(); virtual void run();
// save gained, called on disarm // save gained, called on disarm
void save_tuning_gains(); virtual void save_tuning_gains();
// stop tune, reverting gains // stop tune, reverting gains
void stop(); void stop();
@ -67,7 +88,7 @@ protected:
// internal init function, should be called from init() // internal init function, should be called from init()
bool init_internals(bool use_poshold, bool init_internals(bool use_poshold,
AC_AttitudeControl_Multi *attitude_control, AC_AttitudeControl *attitude_control,
AC_PosControl *pos_control, AC_PosControl *pos_control,
AP_AHRS_View *ahrs_view, AP_AHRS_View *ahrs_view,
AP_InertialNav *inertial_nav); AP_InertialNav *inertial_nav);
@ -75,36 +96,57 @@ protected:
// initialise position controller // initialise position controller
bool init_position_controller(); bool init_position_controller();
private: // 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)
};
void control_attitude(); void control_attitude();
void backup_gains_and_initialise(); void backup_gains_and_initialise();
void load_orig_gains(); void load_orig_gains();
void load_tuned_gains(); void load_tuned_gains();
void load_intra_test_gains(); void load_intra_test_gains();
void load_twitch_gains(); virtual void load_test_gains();
virtual void test_init() = 0;
virtual void test_run(AxisType test_axis, const float dir_sign) = 0;
void update_gcs(uint8_t message_id) const; void update_gcs(uint8_t message_id) const;
bool roll_enabled(); bool roll_enabled() const;
bool pitch_enabled(); bool pitch_enabled() const;
bool yaw_enabled(); bool yaw_enabled() const;
void twitching_test_rate(float rate, float rate_target, float &meas_rate_min, float &meas_rate_max); void twitching_test_rate(float rate, float rate_target, float &meas_rate_min, float &meas_rate_max);
void twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min); void twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min);
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_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) const; void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) const;
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); // Added generic twitch test functions for multi
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 twitch_test_init();
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 twitch_test_run(AxisType test_axis, const float dir_sign);
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);
// replace multi specific updating gain functions with generic forms that covers all axes
// generic method used by subclasses to update gains for the rate p up tune type
virtual void updating_rate_p_up_all(AxisType test_axis)=0;
// generic method used by subclasses to update gains for the rate p down tune type
virtual void updating_rate_p_down_all(AxisType test_axis)=0;
// generic method used by subclasses to update gains for the rate d up tune type
virtual void updating_rate_d_up_all(AxisType test_axis)=0;
// generic method used by subclasses to update gains for the rate d down tune type
virtual void updating_rate_d_down_all(AxisType test_axis)=0;
// generic method used by subclasses to update gains for the angle p up tune type
virtual void updating_angle_p_up_all(AxisType test_axis)=0;
// generic method used by subclasses to update gains for the angle p down tune type
virtual void updating_angle_p_down_all(AxisType test_axis)=0;
void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd); void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
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); virtual void Log_AutoTune() = 0;
void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds); virtual void Log_AutoTuneDetails() = 0;
void send_step_string(); void send_step_string();
const char *level_issue_string() const; const char *level_issue_string() const;
const char * type_string() const; const char * type_string() const;
void announce_state_to_gcs(); void announce_state_to_gcs();
void do_gcs_announcements(); virtual void do_gcs_announcements() = 0;
enum struct LevelIssue { enum struct LevelIssue {
NONE, NONE,
@ -129,15 +171,8 @@ private:
// steps performed while in the tuning mode // steps performed while in the tuning mode
enum StepType { enum StepType {
WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch 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 TESTING = 1, // autotune has begun a test and is watching the resulting vehicle movement
UPDATE_GAINS = 2 // autotune has completed a twitch and is updating the gains based on the results UPDATE_GAINS = 2 // autotune has completed a test 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 // mini steps performed while in Tuning mode, Testing step
@ -145,14 +180,19 @@ private:
RD_UP = 0, // rate D is being tuned up RD_UP = 0, // rate D is being tuned up
RD_DOWN = 1, // rate D is being tuned down RD_DOWN = 1, // rate D is being tuned down
RP_UP = 2, // rate P is being tuned up RP_UP = 2, // rate P is being tuned up
SP_DOWN = 3, // angle P is being tuned down RP_DOWN = 3, // rate P is being tuned down
SP_UP = 4 // angle P is being tuned up RFF_UP = 4, // rate FF is being tuned up
RFF_DOWN = 5, // rate FF is being tuned down
SP_UP = 6, // angle P is being tuned up
SP_DOWN = 7, // angle P is being tuned down
MAX_GAINS = 8, // max allowable stable gains are determined
TUNE_COMPLETE = 9 // Reached end of tuning
}; };
// type of gains to load // type of gains to load
enum GainType { enum GainType {
GAIN_ORIGINAL = 0, GAIN_ORIGINAL = 0,
GAIN_TWITCH = 1, GAIN_TEST = 1,
GAIN_INTRA_TEST = 2, GAIN_INTRA_TEST = 2,
GAIN_TUNED = 3, GAIN_TUNED = 3,
}; };
@ -201,6 +241,7 @@ private:
float tune_roll_rp, tune_roll_rd, tune_roll_sp, tune_roll_accel; 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_pitch_rp, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel;
float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel; float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel;
float tune_roll_rff, tune_pitch_rff, tune_yaw_rd, tune_yaw_rff;
uint32_t announce_time; uint32_t announce_time;
float lean_angle; float lean_angle;
@ -220,9 +261,75 @@ private:
AP_Float min_d; AP_Float min_d;
// copies of object pointers to make code a bit clearer // copies of object pointers to make code a bit clearer
AC_AttitudeControl_Multi *attitude_control; AC_AttitudeControl *attitude_control;
AC_PosControl *pos_control; AC_PosControl *pos_control;
AP_AHRS_View *ahrs_view; AP_AHRS_View *ahrs_view;
AP_InertialNav *inertial_nav; AP_InertialNav *inertial_nav;
AP_Motors *motors; AP_Motors *motors;
TuneType tune_seq[6]; // holds sequence of tune_types to be performed
uint8_t tune_seq_curr; // current tune sequence step
virtual bool allow_zero_rate_p() = 0;
virtual float get_intra_test_ri(AxisType test_axis) = 0;
virtual float get_load_tuned_ri(AxisType test_axis) = 0;
virtual float get_load_tuned_yaw_rd() = 0;
virtual float get_rp_min() const = 0;
virtual float get_sp_min() const = 0;
virtual float get_rlpf_min() const = 0;
// Functions added for heli autotune
// Add additional updating gain functions specific to heli
// generic method used by subclasses to update gains for the rate ff up tune type
virtual void updating_rate_ff_up_all(AxisType test_axis)=0;
// generic method used by subclasses to update gains for the rate ff down tune type
virtual void updating_rate_ff_down_all(AxisType test_axis)=0;
// generic method used by subclasses to update gains for the max gain tune type
virtual void updating_max_gains_all(AxisType test_axis)=0;
// Feedforward test used to determine Rate FF gain
void rate_ff_test_init();
void rate_ff_test_run(float max_angle_cds, float target_rate_cds);
// dwell test used to perform frequency dwells for rate gains
void dwell_test_init(float filt_freq);
void dwell_test_run(uint8_t freq_resp_input, float dwell_freq, float &dwell_gain, float &dwell_phase);
// dwell test used to perform frequency dwells for angle gains
void angle_dwell_test_init(float filt_freq);
void angle_dwell_test_run(float dwell_freq, float &dwell_gain, float &dwell_phase);
// determines the gain and phase for a dwell
void determine_gain(float tgt_rate, float meas_rate, float freq, float &gain, float &phase, bool &cycles_complete, bool funct_reset);
uint8_t ff_test_phase; // phase of feedforward test
float test_command_filt; // filtered commanded output
float test_rate_filt; // filtered rate output
float command_out;
float test_tgt_rate_filt; // filtered target rate
float filt_target_rate;
bool ff_up_first_iter : 1; //true on first iteration of ff up testing
float test_gain[20]; // gain of output to input
float test_freq[20];
float test_phase[20];
float dwell_start_time_ms;
uint8_t freq_cnt;
uint8_t freq_cnt_max;
float curr_test_freq;
bool dwell_complete;
Vector3f start_angles;
LowPassFilterFloat command_filt; // filtered command
LowPassFilterFloat target_rate_filt; // filtered target rotation rate in radians/second
struct max_gain_data {
float freq;
float phase;
float gain;
float max_allowed;
};
max_gain_data max_rate_p;
max_gain_data max_rate_d;
}; };

724
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp Normal file
View File

@ -0,0 +1,724 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
support for autotune of helicopters. Based on original autotune code from ArduCopter, written by Leonard Hall
Converted to a library by Andrew Tridgell, and rewritten to include helicopters by Bill Geyer
*/
#define AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD 2000 // target roll/pitch angle during AUTOTUNE FeedForward rate test
#define AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS 5000 // target roll/pitch rate during AUTOTUNE FeedForward rate test
#define AUTOTUNE_FFI_RATIO_FOR_TESTING 0.5f // I is set 2x smaller than VFF during testing
#define AUTOTUNE_FFI_RATIO_FINAL 0.5f // I is set 0.5x VFF after 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_STEP 0.0005f // minimum increment when increasing/decreasing Rate D term
#define AUTOTUNE_RP_STEP 0.005f // 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_RD_MAX 0.020f // maximum Rate D value
#define AUTOTUNE_RLPF_MIN 1.0f // minimum Rate Yaw filter value
#define AUTOTUNE_RLPF_MAX 20.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 10.0f // maximum Stab P value
#define AUTOTUNE_SP_MIN 0.5f // maximum Stab P value
#define AUTOTUNE_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in
#include "AC_AutoTune_Heli.h"
void AC_AutoTune_Heli::test_init()
{
if ((tune_type == RFF_UP) || (tune_type == RFF_DOWN)) {
rate_ff_test_init();
step_time_limit_ms = 10000;
} else if (tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP) {
// initialize start frequency and determine gain function when dwell test is used
if (freq_cnt == 0) {
test_freq[0] = 2.0f * 3.14159f * 2.0f;
curr_test_freq = test_freq[0];
// reset determine_gain function for first use in the event autotune is restarted
determine_gain(0.0f, 0.0f, curr_test_freq, test_gain[freq_cnt], test_phase[freq_cnt], dwell_complete, true);
}
dwell_test_init(curr_test_freq);
if (!is_zero(curr_test_freq)) {
// 4 seconds is added to allow aircraft to achieve start attitude. Then the time to conduct the dwells is added to it.
step_time_limit_ms = (uint32_t)(4000 + (float)(AUTOTUNE_DWELL_CYCLES + 2) * 1000.0f * 6.28f / curr_test_freq);
}
} else if (tune_type == SP_UP) {
// initialize start frequency and determine gain function when dwell test is used
if (freq_cnt == 0) {
test_freq[0] = 0.5f * 3.14159f * 2.0f;
curr_test_freq = test_freq[0];
// reset determine_gain function for first use in the event autotune is restarted
determine_gain(0.0f, 0.0f, curr_test_freq, test_gain[freq_cnt], test_phase[freq_cnt], dwell_complete, true);
}
angle_dwell_test_init(curr_test_freq);
if (!is_zero(curr_test_freq)) {
// 1 seconds is added for a little buffer. Then the time to conduct the dwells is added to it.
step_time_limit_ms = (uint32_t)(2000 + (float)(AUTOTUNE_DWELL_CYCLES + 2) * 1000.0f * 6.28f / curr_test_freq);
}
} else {
}
start_angles = Vector3f(roll_cd, pitch_cd, desired_yaw_cd); // heli specific
}
void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign)
{
if (tune_type == SP_UP) {
angle_dwell_test_run(curr_test_freq, test_gain[freq_cnt], test_phase[freq_cnt]);
} else if ((tune_type == RFF_UP) || (tune_type == RFF_DOWN)) {
rate_ff_test_run(AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD, AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS);
} else if (tune_type == RP_UP || tune_type == RD_UP) {
dwell_test_run(1, curr_test_freq, test_gain[freq_cnt], test_phase[freq_cnt]);
} else if (tune_type == MAX_GAINS) {
dwell_test_run(0, curr_test_freq, test_gain[freq_cnt], test_phase[freq_cnt]);
} else {
step = UPDATE_GAINS;
}
}
void AC_AutoTune_Heli::do_gcs_announcements()
{
const uint32_t now = AP_HAL::millis();
if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) {
return;
}
float tune_rp = 0.0f;
float tune_rd = 0.0f;
float tune_rff = 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_rff = tune_roll_rff;
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_rff = tune_pitch_rff;
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_rd;
tune_rff = tune_yaw_rff;
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();
switch (tune_type) {
case RD_UP:
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f d=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]), (double)tune_rd);
break;
case RD_DOWN:
case RP_DOWN:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd);
break;
case RP_UP:
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f p=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)tune_rp);
break;
case RFF_UP:
if (!is_zero(test_rate_filt)) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: target=%f rotation=%f command=%f", (double)(test_tgt_rate_filt*57.3f), (double)(test_rate_filt*57.3f), (double)(test_command_filt));
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ff=%f", (double)tune_rff);
break;
case RFF_DOWN:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ff=%f", (double)tune_rff);
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;
case MAX_GAINS:
case TUNE_COMPLETE:
break;
}
// gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: success %u/%u", counter, AUTOTUNE_SUCCESS_COUNT);
announce_time = now;
}
// load_test_gains - load the to-be-tested gains for a single axis
// called by control_attitude() just before it beings testing a gain (i.e. just before it twitches)
void AC_AutoTune_Heli::load_test_gains()
{
AC_AutoTune::load_test_gains();
switch (axis) {
case ROLL:
if (tune_type == SP_UP) {
attitude_control->get_rate_roll_pid().kI(orig_roll_ri);
} else {
attitude_control->get_rate_roll_pid().kI(0.0f);
}
attitude_control->get_rate_roll_pid().ff(tune_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_rate_roll_pid().slew_limit(0.0f);
break;
case PITCH:
if (tune_type == SP_UP) {
attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri);
} else {
attitude_control->get_rate_pitch_pid().kI(0.0f);
}
attitude_control->get_rate_pitch_pid().ff(tune_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_rate_pitch_pid().slew_limit(0.0f);
break;
case YAW:
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*0.01f);
attitude_control->get_rate_yaw_pid().kD(tune_yaw_rd);
attitude_control->get_rate_yaw_pid().ff(tune_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().slew_limit(0.0f);
break;
}
}
// save_tuning_gains - save the final tuned gains for each axis
// save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position)
void AC_AutoTune_Heli::save_tuning_gains()
{
AC_AutoTune::save_tuning_gains();
// sanity check the rate P values
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled() && !is_zero(tune_roll_rp)) {
// rate roll gains
attitude_control->get_rate_roll_pid().ff(tune_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax);
attitude_control->get_rate_roll_pid().kI(tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL);
attitude_control->get_rate_roll_pid().save_gains();
// resave pids to originals in case the autotune is run again
orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled() && !is_zero(tune_pitch_rp)) {
// rate pitch gains
attitude_control->get_rate_pitch_pid().ff(tune_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax);
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL);
attitude_control->get_rate_pitch_pid().save_gains();
// resave pids to originals in case the autotune is run again
orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) {
// rate yaw gains
attitude_control->get_rate_yaw_pid().kD(tune_yaw_rd);
attitude_control->get_rate_yaw_pid().ff(tune_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax);
attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
attitude_control->get_rate_yaw_pid().save_gains();
// resave pids to originals in case the autotune is run again
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
}
// update GCS and log save gains event
update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS);
AP::logger().Write_Event(LogEvent::AUTOTUNE_SAVEDGAINS);
reset();
}
// generic method used to update gains for the rate p up tune type
void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis)
{
float p_gain = 0.0f;
switch (test_axis) {
case ROLL:
p_gain = tune_roll_rp;
break;
case PITCH:
p_gain = tune_pitch_rp;
break;
case YAW:
p_gain = tune_yaw_rp;
break;
}
// announce results of dwell and update
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f rate_p=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]), (double)(p_gain));
switch (test_axis) {
case ROLL:
updating_rate_d_up(tune_roll_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p);
break;
case PITCH:
updating_rate_d_up(tune_pitch_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p);
break;
case YAW:
updating_rate_d_up(tune_yaw_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p);
break;
}
}
// generic method used to update gains for the rate d up tune type
void AC_AutoTune_Heli::updating_rate_d_up_all(AxisType test_axis)
{
float d_gain = 0.0f;
switch (test_axis) {
case ROLL:
d_gain = tune_roll_rd;
break;
case PITCH:
d_gain = tune_pitch_rd;
break;
case YAW:
d_gain = tune_yaw_rd;
break;
}
// announce results of dwell and update
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f rate_d=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]), (double)(d_gain));
switch (test_axis) {
case ROLL:
updating_rate_d_up(tune_roll_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d);
break;
case PITCH:
updating_rate_d_up(tune_pitch_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d);
break;
case YAW:
updating_rate_d_up(tune_yaw_rd, test_freq, test_gain, test_phase, freq_cnt, max_rate_d);
break;
}
}
// generic method used to update gains for the rate ff up tune type
void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis)
{
switch (test_axis) {
case ROLL:
updating_rate_ff_up(tune_roll_rff, test_tgt_rate_filt*5730.0f, test_rate_filt*5730.0f, test_command_filt);
break;
case PITCH:
updating_rate_ff_up(tune_pitch_rff, test_tgt_rate_filt*5730.0f, test_rate_filt*5730.0f, test_command_filt);
break;
case YAW:
updating_rate_ff_up(tune_yaw_rff, test_tgt_rate_filt*5730.0f, test_rate_filt*5730.0f, test_command_filt);
break;
}
}
// generic method used to update gains for the angle p up tune type
void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
{
// announce results of dwell and update
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]));
switch (test_axis) {
case ROLL:
updating_angle_p_up(tune_roll_sp, test_freq, test_gain, test_phase, freq_cnt);
break;
case PITCH:
updating_angle_p_up(tune_pitch_sp, test_freq, test_gain, test_phase, freq_cnt);
break;
case YAW:
updating_angle_p_up(tune_yaw_sp, test_freq, test_gain, test_phase, freq_cnt);
break;
}
}
// generic method used to update gains for the max gain tune type
void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis)
{
// announce results of dwell and update
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt]));
switch (test_axis) {
case ROLL:
updating_max_gains(&test_freq[0], &test_gain[0], &test_phase[0], freq_cnt, max_rate_p, max_rate_d, tune_roll_rp, tune_roll_rd);
break;
case PITCH:
updating_max_gains(&test_freq[0], &test_gain[0], &test_phase[0], freq_cnt, max_rate_p, max_rate_d, tune_pitch_rp, tune_pitch_rd);
break;
case YAW:
updating_max_gains(&test_freq[0], &test_gain[0], &test_phase[0], freq_cnt, max_rate_p, max_rate_d, tune_yaw_rp, tune_yaw_rd);
break;
}
}
// updating_rate_ff_up - adjust FF to ensure the target is reached
// FF is adjusted until rate requested is acheived
void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, float rate_target, float meas_rate, float meas_command)
{
if (ff_up_first_iter) {
if (!is_zero(meas_rate)) {
tune_ff = 5730.0f * meas_command / meas_rate;
}
tune_ff = constrain_float(tune_ff, 0.01, 1);
ff_up_first_iter = false;
} else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) < 1.05f * fabsf(rate_target) &&
fabsf(meas_rate) > 0.95f * fabsf(rate_target)) {
counter = AUTOTUNE_SUCCESS_COUNT;
tune_ff = 0.75f * tune_ff;
} else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) > 1.05f * fabsf(rate_target)) {
tune_ff = 0.98f * tune_ff;
} else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) < 0.95f * fabsf(rate_target)) {
tune_ff = 1.02f * tune_ff;
} else {
if (!is_zero(meas_rate)) {
tune_ff = 5730.0f * meas_command / meas_rate;
}
tune_ff = constrain_float(tune_ff, 0.01, 1);
}
}
void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, float gain_incr, float max_gain)
{
float test_freq_incr = 0.5f * 3.14159f * 2.0f;
if (freq_cnt < 12) {
if (freq_cnt == 0) {
freq_cnt_max = 0;
} else if (gain[freq_cnt] > gain[freq_cnt_max]) {
freq_cnt_max = freq_cnt;
}
freq_cnt++;
freq[freq_cnt] = freq[freq_cnt-1] + test_freq_incr;
curr_test_freq = freq[freq_cnt];
} else {
if (gain[freq_cnt] < max_gain) {
tune_p += gain_incr;
curr_test_freq = freq[freq_cnt_max];
freq[freq_cnt] = curr_test_freq;
} else {
counter = AUTOTUNE_SUCCESS_COUNT;
// reset curr_test_freq and freq_cnt for next test
curr_test_freq = freq[0];
freq_cnt = 0;
}
}
// reset determine_gain function
determine_gain(0.0f, 0.0f, curr_test_freq, gain[freq_cnt], phase[freq_cnt], dwell_complete, true);
}
void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d)
{
float test_freq_incr = 0.5f * 3.14159f * 2.0f;
static uint8_t prev_good_frq_cnt;
float max_gain = 1.2f;
if (frq_cnt < 12) {
if (frq_cnt == 0) {
tune_d = max_gain_d.max_allowed * 0.5f;
freq_cnt_max = 0;
} else if (phase[frq_cnt] <= 180.0f && !is_zero(phase[frq_cnt])) {
prev_good_frq_cnt = frq_cnt;
} else if (frq_cnt > 1 && phase[frq_cnt] > phase[frq_cnt-1] + 360.0f && !is_zero(phase[frq_cnt])) {
if (phase[frq_cnt] - 360.0f < 180.0f) {
prev_good_frq_cnt = frq_cnt;
}
} else if (frq_cnt > 1 && phase[frq_cnt] > 300.0f && !is_zero(phase[frq_cnt])) {
frq_cnt = 11;
}
frq_cnt++;
if (frq_cnt == 12) {
freq[frq_cnt] = freq[prev_good_frq_cnt];
curr_test_freq = freq[frq_cnt];
} else {
freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr;
curr_test_freq = freq[frq_cnt];
}
} else {
/* if (!is_zero(phase[prev_good_frq_cnt + 1])) {
freq_cnt_max = prev_good_frq_cnt + 2;
} else {
freq_cnt_max = prev_good_frq_cnt + 1;
}
float phase_freq = (180.0f - phase[prev_good_frq_cnt]) / (phase[freq_cnt_max] - phase[prev_good_frq_cnt]);
phase_freq = freq[prev_good_frq_cnt] + phase_freq * (freq[freq_cnt_max] - freq[prev_good_frq_cnt]); */
if (gain[frq_cnt] < max_gain && phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f && tune_d < 0.8f * max_gain_d.max_allowed) {
tune_d += 0.1f * max_gain_d.max_allowed;
} else if (gain[frq_cnt] < max_gain && phase[frq_cnt] > 180.0f) {
curr_test_freq = curr_test_freq - 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test_freq;
} else if (gain[frq_cnt] < max_gain && phase[frq_cnt] < 160.0f) {
curr_test_freq = curr_test_freq + 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test_freq;
} else if (gain[frq_cnt] >= max_gain || tune_d > 0.8f * max_gain_d.max_allowed) {
counter = AUTOTUNE_SUCCESS_COUNT;
tune_d = 0.5f * tune_d;
// reset curr_test_freq and frq_cnt for next test
curr_test_freq = freq[0];
frq_cnt = 0;
}
}
// reset determine_gain function
determine_gain(0.0f, 0.0f, curr_test_freq, gain[frq_cnt], phase[frq_cnt], dwell_complete, true);
}
void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt)
{
float test_freq_incr = 0.5f * 3.14159f * 2.0f;
static uint8_t prev_good_frq_cnt;
float max_gain = 1.2f;
if (frq_cnt < 12) {
if (frq_cnt == 0) {
freq_cnt_max = 0;
} else if (phase[frq_cnt] <= 180.0f && !is_zero(phase[frq_cnt])) {
prev_good_frq_cnt = frq_cnt;
} else if (frq_cnt > 1 && phase[frq_cnt] > phase[frq_cnt-1] + 360.0f && !is_zero(phase[frq_cnt])) {
if (phase[frq_cnt] - 360.0f < 180.0f) {
prev_good_frq_cnt = frq_cnt;
}
// } else if (frq_cnt > 1 && phase[frq_cnt] > 300.0f && !is_zero(phase[frq_cnt])) {
// frq_cnt = 11;
}
frq_cnt++;
if (frq_cnt == 12) {
freq[frq_cnt] = freq[prev_good_frq_cnt];
curr_test_freq = freq[frq_cnt];
} else {
freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr;
curr_test_freq = freq[frq_cnt];
}
} else {
if (gain[frq_cnt] < max_gain && phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) {
tune_p += 0.5f;
} else if (gain[frq_cnt] < max_gain && phase[frq_cnt] > 180.0f) {
curr_test_freq = curr_test_freq - 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test_freq;
} else if (gain[frq_cnt] < max_gain && phase[frq_cnt] < 160.0f) {
curr_test_freq = curr_test_freq + 0.5 * test_freq_incr;
freq[frq_cnt] = curr_test_freq;
} else if (gain[frq_cnt] >= max_gain || tune_p > 10.0f) {
counter = AUTOTUNE_SUCCESS_COUNT;
// reset curr_test_freq and frq_cnt for next test
curr_test_freq = freq[0];
frq_cnt = 0;
}
}
// reset determine_gain function
determine_gain(0.0f, 0.0f, curr_test_freq, gain[frq_cnt], phase[frq_cnt], dwell_complete, true);
}
// updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur
void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d)
{
float test_freq_incr = 0.5f * 3.14159f * 2.0f;
static uint8_t find_max_p = 0;
static uint8_t find_max_d = 0;
if (frq_cnt < 12) {
if (frq_cnt > 1 && phase[frq_cnt] > 161.0f && phase[frq_cnt] < 200.0f &&
phase[frq_cnt-1] > 90.0f && phase[frq_cnt-1] < 161.0f &&
!is_zero(phase[frq_cnt]) && find_max_p == 0) {
max_gain_p.freq = linear_interpolate(freq[frq_cnt-1],freq[frq_cnt],161.0f,phase[frq_cnt-1],phase[frq_cnt]);
max_gain_p.gain = linear_interpolate(gain[frq_cnt-1],gain[frq_cnt],161.0f,phase[frq_cnt-1],phase[frq_cnt]);
max_gain_p.phase = 161.0f;
max_gain_p.max_allowed = powf(10.0f,-1 * (log10f(max_gain_p.gain) * 20.0f + 2.42) / 20.0f);
find_max_p = 1;
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max rate P freq=%f gain=%f ph=%f rate_d=%f", (double)(max_gain_p.freq), (double)(max_gain_p.gain), (double)(max_gain_p.phase), (double)(max_gain_p.max_allowed));
}
if (frq_cnt > 1 && phase[frq_cnt] > 251.0f && phase[frq_cnt] < 300.0f &&
phase[frq_cnt-1] > 180.0f && phase[frq_cnt-1] < 251.0f &&
!is_zero(phase[frq_cnt]) && find_max_d == 0) {
max_gain_d.freq = linear_interpolate(freq[frq_cnt-1],freq[frq_cnt],251.0f,phase[frq_cnt-1],phase[frq_cnt]);
max_gain_d.gain = linear_interpolate(gain[frq_cnt-1],gain[frq_cnt],251.0f,phase[frq_cnt-1],phase[frq_cnt]);
max_gain_d.phase = 251.0f;
max_gain_d.max_allowed = powf(10.0f,-1 * (log10f(max_gain_d.freq * max_gain_d.gain) * 20.0f + 2.42) / 20.0f);
find_max_d = 1;
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Rate D freq=%f gain=%f ph=%f rate_d=%f", (double)(max_gain_d.freq), (double)(max_gain_d.gain), (double)(max_gain_d.phase), (double)(max_gain_d.max_allowed));
}
if (frq_cnt > 1 && phase[frq_cnt] > 300.0f && !is_zero(phase[frq_cnt])) {
frq_cnt = 11;
}
frq_cnt++;
if (frq_cnt == 12) {
counter = AUTOTUNE_SUCCESS_COUNT;
// reset curr_test_freq and frq_cnt for next test
curr_test_freq = freq[0];
frq_cnt = 0;
tune_p = 0.35f * max_gain_p.max_allowed;
tune_d = 0.25f * max_gain_d.max_allowed;
} else {
freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr;
curr_test_freq = freq[frq_cnt];
}
}
// reset determine_gain function
determine_gain(0.0f, 0.0f, curr_test_freq, gain[frq_cnt], phase[frq_cnt], dwell_complete, true);
}
void AC_AutoTune_Heli::Log_AutoTune()
{
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
switch (axis) {
case ROLL:
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp);
break;
case PITCH:
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp);
break;
case YAW:
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp);
break;
}
} else {
switch (axis) {
case ROLL:
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp);
break;
case PITCH:
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp);
break;
case YAW:
Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp);
break;
}
}
}
void AC_AutoTune_Heli::Log_AutoTuneDetails()
{
Log_Write_AutoTuneDetails(command_out, filt_target_rate, rotation_rate);
}
// @LoggerMessage: ATUN
// @Description: Copter/QuadPlane AutoTune
// @Vehicles: Copter, Plane
// @Field: TimeUS: Time since system startup
// @Field: Axis: which axis is currently being tuned
// @Field: TuneStep: step in autotune process
// @Field: Freq: target dwell frequency
// @Field: Gain: measured gain of dwell
// @Field: Phase: measured phase of dwell
// @Field: RFF: new rate gain FF term
// @Field: RP: new rate gain P term
// @Field: RD: new rate gain D term
// @Field: SP: new angle P term
// Write an Autotune data packet
void AC_AutoTune_Heli::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp)
{
AP::logger().Write(
"ATUN",
"TimeUS,Axis,TuneStep,Freq,Gain,Phase,RFF,RP,RD,SP",
"s---------",
"F--000----",
"QBBfffffff",
AP_HAL::micros64(),
axis,
tune_step,
dwell_freq,
meas_gain,
meas_phase,
new_gain_rff,
new_gain_rp,
new_gain_rd,
new_gain_sp);
}
// Write an Autotune data packet
void AC_AutoTune_Heli::Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads)
{
// @LoggerMessage: ATDE
// @Description: AutoTune data packet
// @Field: TimeUS: Time since system startup
// @Field: Cmd: current motor command
// @Field: TRate: current target angular rate
// @Field: Rate: current angular rate
AP::logger().WriteStreaming(
"ATDE",
"TimeUS,Cmd,TRate,Rate",
"s-kk",
"F000",
"Qfff",
AP_HAL::micros64(),
motor_cmd,
tgt_rate_rads*57.3f,
rate_rads*57.3f);
}
float AC_AutoTune_Heli::get_intra_test_ri(AxisType test_axis)
{
float ret = 0.0f;
switch (test_axis) {
case ROLL:
ret = orig_roll_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING;
break;
case PITCH:
ret = orig_pitch_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING;
break;
case YAW:
ret = orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING;
break;
}
return ret;
}
float AC_AutoTune_Heli::get_load_tuned_ri(AxisType test_axis)
{
float ret = 0.0f;
switch (test_axis) {
case ROLL:
ret = tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL;
break;
case PITCH:
ret = tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL;
break;
case YAW:
ret = tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL;
break;
}
return ret;
}
float AC_AutoTune_Heli::get_rp_min() const
{
return AUTOTUNE_RP_MIN;
}
float AC_AutoTune_Heli::get_sp_min() const
{
return AUTOTUNE_SP_MIN;
}
float AC_AutoTune_Heli::get_rlpf_min() const
{
return AUTOTUNE_RLPF_MIN;
}

89
libraries/AC_AutoTune/AC_AutoTune_Heli.h Normal file
View File

@ -0,0 +1,89 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
support for autotune of multirotors. Based on original autotune code from ArduCopter, written by Leonard Hall
Converted to a library by Andrew Tridgell
*/
#pragma once
#include "AC_AutoTune.h"
class AC_AutoTune_Heli : public AC_AutoTune
{
public:
// constructor
AC_AutoTune_Heli()
{
// tune_seq[0] = 4; // RFF_UP
// tune_seq[1] = 8; // MAX_GAINS
// tune_seq[2] = 0; // RD_UP
// tune_seq[3] = 2; // RP_UP
// tune_seq[2] = 6; // SP_UP
// tune_seq[3] = 9; // tune complete
tune_seq[0] = SP_UP;
tune_seq[1] = TUNE_COMPLETE;
};
// save gained, called on disarm
void save_tuning_gains() override;
protected:
void test_init() override;
void test_run(AxisType test_axis, const float dir_sign) override;
void do_gcs_announcements() override;
void load_test_gains() override;
// generic method used to update gains for the rate p up tune type
void updating_rate_p_up_all(AxisType test_axis) override;
// generic method used to update gains for the rate p down tune type
void updating_rate_p_down_all(AxisType test_axis) override {};
// generic method used to update gains for the rate d up tune type
void updating_rate_d_up_all(AxisType test_axis) override;
// generic method used to update gains for the rate d down tune type
void updating_rate_d_down_all(AxisType test_axis) override {};
// generic method used to update gains for the rate ff up tune type
void updating_rate_ff_up_all(AxisType test_axis) override;
// generic method used to update gains for the rate ff down tune type
void updating_rate_ff_down_all(AxisType test_axis) override {};
// generic method used to update gains for the angle p up tune type
void updating_angle_p_up_all(AxisType test_axis) override;
// generic method used to update gains for the angle p down tune type
void updating_angle_p_down_all(AxisType test_axis) override {};
// generic method used to update gains for the max gain tune type
void updating_max_gains_all(AxisType test_axis) override;
void Log_AutoTune() override;
void Log_AutoTuneDetails() override;
void Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp);
void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads);
bool allow_zero_rate_p() override {return true;}
float get_intra_test_ri(AxisType test_axis) override;
float get_load_tuned_ri(AxisType test_axis) override;
float get_load_tuned_yaw_rd() override {return tune_yaw_rd;}
float get_rp_min() const override;
float get_sp_min() const override;
float get_rlpf_min() const override;
private:
// updating_rate_ff_up - adjust FF to ensure the target is reached
// FF is adjusted until rate requested is acheived
void updating_rate_ff_up(float &tune_ff, float rate_target, float meas_rate, float meas_command);
void updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, float gain_incr, float max_gain);
void updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d);
void updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt);
// updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur
void updating_max_gains(float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d);
};

637
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp Normal file
View File

@ -0,0 +1,637 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
support for autotune of multirotors. Based on original autotune code from ArduCopter, written by Leonard Hall
Converted to a library by Andrew Tridgell
*/
#define AUTOTUNE_FFI_RATIO_FOR_TESTING 0.5f // I is set 2x smaller than VFF during testing
#define AUTOTUNE_FFI_RATIO_FINAL 0.5f // I is set 0.5x VFF after 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_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_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_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in
#include "AC_AutoTune_Multi.h"
void AC_AutoTune_Multi::do_gcs_announcements()
{
const uint32_t now = AP_HAL::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:
case RP_DOWN:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd);
break;
case RFF_UP:
case RFF_DOWN:
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;
case MAX_GAINS:
case TUNE_COMPLETE:
break;
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: success %u/%u", counter, AUTOTUNE_SUCCESS_COUNT);
announce_time = now;
}
void AC_AutoTune_Multi::test_init()
{
twitch_test_init();
}
void AC_AutoTune_Multi::test_run(AxisType test_axis, const float dir_sign)
{
twitch_test_run(test_axis, dir_sign);
}
// load_test_gains - load the to-be-tested gains for a single axis
// called by control_attitude() just before it beings testing a gain (i.e. just before it twitches)
void AC_AutoTune_Multi::load_test_gains()
{
AC_AutoTune::load_test_gains();
switch (axis) {
case ROLL:
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*0.01f);
attitude_control->get_rate_roll_pid().ff(0.0f);
attitude_control->get_rate_roll_pid().filt_T_hz(0.0f);
attitude_control->get_rate_roll_pid().slew_limit(0.0f);
break;
case PITCH:
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*0.01f);
attitude_control->get_rate_pitch_pid().ff(0.0f);
attitude_control->get_rate_pitch_pid().filt_T_hz(0.0f);
attitude_control->get_rate_pitch_pid().slew_limit(0.0f);
break;
case YAW:
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().ff(0.0f);
attitude_control->get_rate_yaw_pid().filt_T_hz(0.0f);
attitude_control->get_rate_yaw_pid().slew_limit(0.0f);
break;
}
}
// save_tuning_gains - save the final tuned gains for each axis
// save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position)
void AC_AutoTune_Multi::save_tuning_gains()
{
AC_AutoTune::save_tuning_gains();
// sanity check the rate P values
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled() && !is_zero(tune_roll_rp)) {
// rate roll gains
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax);
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control->get_rate_roll_pid().save_gains();
// resave pids to originals in case the autotune is run again
orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled() && !is_zero(tune_pitch_rp)) {
// rate pitch gains
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax);
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control->get_rate_pitch_pid().save_gains();
// resave pids to originals in case the autotune is run again
orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
}
if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) {
// rate yaw gains
attitude_control->get_rate_yaw_pid().kD(0.0f);
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax);
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
attitude_control->get_rate_yaw_pid().save_gains();
// resave pids to originals in case the autotune is run again
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
}
// update GCS and log save gains event
update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS);
AP::logger().Write_Event(LogEvent::AUTOTUNE_SAVEDGAINS);
reset();
}
// generic method used to update gains for the rate p up tune type
void AC_AutoTune_Multi::updating_rate_p_up_all(AxisType test_axis)
{
switch (test_axis) {
case ROLL:
updating_rate_p_up_d_down(tune_roll_rd, 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, 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;
}
}
// generic method used to update gains for the rate d up tune type
void AC_AutoTune_Multi::updating_rate_d_up_all(AxisType test_axis)
{
switch (test_axis) {
case ROLL:
updating_rate_d_up(tune_roll_rd, 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, 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;
}
}
// generic method used to update gains for the rate d down tune type
void AC_AutoTune_Multi::updating_rate_d_down_all(AxisType test_axis)
{
switch (test_axis) {
case ROLL:
updating_rate_d_down(tune_roll_rd, 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, 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;
}
}
// generic method used to update gains for the angle p up tune type
void AC_AutoTune_Multi::updating_angle_p_up_all(AxisType test_axis)
{
switch (test_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;
}
}
// generic method used to update gains for the angle p down tune type
void AC_AutoTune_Multi::updating_angle_p_down_all(AxisType test_axis)
{
switch (test_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;
}
}
// 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 AC_AutoTune_Multi::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;
AP::logger().Write_Event(LogEvent::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;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
} else {
// we have a good measurement of bounce back
if (meas_rate_max-meas_rate_min > meas_rate_max*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;
AP::logger().Write_Event(LogEvent::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 AC_AutoTune_Multi::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;
AP::logger().Write_Event(LogEvent::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;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
} else {
// we have a good measurement of bounce back
if (meas_rate_max-meas_rate_min < meas_rate_max*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;
AP::logger().Write_Event(LogEvent::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 AC_AutoTune_Multi::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*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*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;
AP::logger().Write_Event(LogEvent::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;
AP::logger().Write_Event(LogEvent::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;
AP::logger().Write_Event(LogEvent::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 AC_AutoTune_Multi::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*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;
AP::logger().Write_Event(LogEvent::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 AC_AutoTune_Multi::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*aggressiveness)) ||
((meas_angle_max > angle_target) && (meas_rate_min < -meas_rate_max*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;
AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT);
}
} else {
ignore_next = false;
}
}
}
void AC_AutoTune_Multi::Log_AutoTune()
{
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;
}
}
}
void AC_AutoTune_Multi::Log_AutoTuneDetails()
{
Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
}
// @LoggerMessage: ATUN
// @Description: Copter/QuadPlane AutoTune
// @Vehicles: Copter, Plane
// @Field: TimeUS: Time since system startup
// @Field: Axis: which axis is currently being tuned
// @Field: TuneStep: step in autotune process
// @Field: Targ: target angle or rate, depending on tuning step
// @Field: Min: measured minimum target angle or rate
// @Field: Max: measured maximum target angle or rate
// @Field: RP: new rate gain P term
// @Field: RD: new rate gain D term
// @Field: SP: new angle P term
// @Field: ddt: maximum measured twitching acceleration
// Write an Autotune data packet
void AC_AutoTune_Multi::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)
{
AP::logger().Write(
"ATUN",
"TimeUS,Axis,TuneStep,Targ,Min,Max,RP,RD,SP,ddt",
"s--ddd---o",
"F--000---0",
"QBBfffffff",
AP_HAL::micros64(),
axis,
tune_step,
meas_target*0.01f,
meas_min*0.01f,
meas_max*0.01f,
new_gain_rp,
new_gain_rd,
new_gain_sp,
new_ddt);
}
// Write an Autotune data packet
void AC_AutoTune_Multi::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
{
// @LoggerMessage: ATDE
// @Description: AutoTune data packet
// @Field: TimeUS: Time since system startup
// @Field: Angle: current angle
// @Field: Rate: current angular rate
AP::logger().WriteStreaming(
"ATDE",
"TimeUS,Angle,Rate",
"sdk",
"F00",
"Qff",
AP_HAL::micros64(),
angle_cd*0.01f,
rate_cds*0.01f);
}
float AC_AutoTune_Multi::get_intra_test_ri(AxisType test_axis)
{
float ret = 0.0f;
switch (test_axis) {
case ROLL:
ret = orig_roll_rp * AUTOTUNE_PI_RATIO_FOR_TESTING;
break;
case PITCH:
ret = orig_pitch_rp * AUTOTUNE_PI_RATIO_FOR_TESTING;
break;
case YAW:
ret = orig_yaw_rp * AUTOTUNE_PI_RATIO_FOR_TESTING;
break;
}
return ret;
}
float AC_AutoTune_Multi::get_load_tuned_ri(AxisType test_axis)
{
float ret = 0.0f;
switch (test_axis) {
case ROLL:
ret = tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL;
break;
case PITCH:
ret = tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL;
break;
case YAW:
ret = tune_yaw_rp*AUTOTUNE_PI_RATIO_FINAL;
break;
}
return ret;
}
float AC_AutoTune_Multi::get_rp_min() const
{
return AUTOTUNE_RP_MIN;
}
float AC_AutoTune_Multi::get_sp_min() const
{
return AUTOTUNE_SP_MIN;
}
float AC_AutoTune_Multi::get_rlpf_min() const
{
return AUTOTUNE_RLPF_MIN;
}

93
libraries/AC_AutoTune/AC_AutoTune_Multi.h Normal file
View File

@ -0,0 +1,93 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
support for autotune of multirotors. Based on original autotune code from ArduCopter, written by Leonard Hall
Converted to a library by Andrew Tridgell
*/
#pragma once
#include "AC_AutoTune.h"
class AC_AutoTune_Multi : public AC_AutoTune
{
public:
// constructor
AC_AutoTune_Multi()
{
tune_seq[0] = RD_UP;
tune_seq[1] = RD_DOWN;
tune_seq[2] = RP_UP;
tune_seq[3] = SP_DOWN;
tune_seq[4] = SP_UP;
tune_seq[5] = TUNE_COMPLETE;
};
// save gained, called on disarm
void save_tuning_gains() override;
protected:
void test_init() override;
void test_run(AxisType test_axis, const float dir_sign) override;
void do_gcs_announcements() override;
void load_test_gains() override;
// generic method used to update gains for the rate p up tune type
void updating_rate_p_up_all(AxisType test_axis) override;
// generic method used to update gains for the rate p down tune type
void updating_rate_p_down_all(AxisType test_axis) override {};
// generic method used to update gains for the rate d up tune type
void updating_rate_d_up_all(AxisType test_axis) override;
// generic method used to update gains for the rate d down tune type
void updating_rate_d_down_all(AxisType test_axis) override;
// generic method used to update gains for the rate ff up tune type
void updating_rate_ff_up_all(AxisType test_axis) override {};
// generic method used to update gains for the rate ff down tune type
void updating_rate_ff_down_all(AxisType test_axis) override {};
// generic method used to update gains for the angle p up tune type
void updating_angle_p_up_all(AxisType test_axis) override;
// generic method used to update gains for the angle p down tune type
void updating_angle_p_down_all(AxisType test_axis) override;
// generic method used to update gains for the max gain tune type
void updating_max_gains_all(AxisType test_axis) override {};
void Log_AutoTune() override;
void Log_AutoTuneDetails() override;
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);
bool allow_zero_rate_p() override {return false;}
float get_intra_test_ri(AxisType test_axis) override;
float get_load_tuned_ri(AxisType test_axis) override;
float get_load_tuned_yaw_rd() override {return 0.0f;}
float get_rp_min() const override;
float get_sp_min() const override;
float get_rlpf_min() const override;
private:
// 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 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);
// 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 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);
// 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 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);
// 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 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);
// 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 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);
};