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
Converted to a library by Andrew Tridgell
*/
#pragma once
#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 <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:
// constructor
AC_AutoTune();
@ -32,7 +53,7 @@ public:
virtual void run();
// save gained, called on disarm
void save_tuning_gains();
virtual void save_tuning_gains();
// stop tune, reverting gains
void stop();
@ -67,7 +88,7 @@ protected:
// internal init function, should be called from init()
bool init_internals(bool use_poshold,
AC_AttitudeControl_Multi *attitude_control,
AC_AttitudeControl *attitude_control,
AC_PosControl *pos_control,
AP_AHRS_View *ahrs_view,
AP_InertialNav *inertial_nav);
@ -75,36 +96,57 @@ protected:
// initialise 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 backup_gains_and_initialise();
void load_orig_gains();
void load_tuned_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;
bool roll_enabled();
bool pitch_enabled();
bool yaw_enabled();
bool roll_enabled() const;
bool pitch_enabled() const;
bool yaw_enabled() const;
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_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 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);
// Added generic twitch test functions for multi
void twitch_test_init();
void twitch_test_run(AxisType test_axis, const float dir_sign);
// 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 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);
virtual void Log_AutoTune() = 0;
virtual void Log_AutoTuneDetails() = 0;
void send_step_string();
const char *level_issue_string() const;
const char * type_string() const;
void announce_state_to_gcs();
void do_gcs_announcements();
virtual void do_gcs_announcements() = 0;
enum struct LevelIssue {
NONE,
@ -129,15 +171,8 @@ private:
// 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)
TESTING = 1, // autotune has begun a test and is watching the resulting vehicle movement
UPDATE_GAINS = 2 // autotune has completed a test and is updating the gains based on the results
};
// mini steps performed while in Tuning mode, Testing step
@ -145,14 +180,19 @@ private:
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
RP_DOWN = 3, // rate P is being tuned down
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
enum GainType {
GAIN_ORIGINAL = 0,
GAIN_TWITCH = 1,
GAIN_TEST = 1,
GAIN_INTRA_TEST = 2,
GAIN_TUNED = 3,
};
@ -201,6 +241,7 @@ private:
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;
float tune_roll_rff, tune_pitch_rff, tune_yaw_rd, tune_yaw_rff;
uint32_t announce_time;
float lean_angle;
@ -220,9 +261,75 @@ private:
AP_Float min_d;
// copies of object pointers to make code a bit clearer
AC_AttitudeControl_Multi *attitude_control;
AC_AttitudeControl *attitude_control;
AC_PosControl *pos_control;
AP_AHRS_View *ahrs_view;
AP_InertialNav *inertial_nav;
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);
};