mirror of https://github.com/ArduPilot/ardupilot
AC_AutoTune: formatting and comment improvements
This commit is contained in:
parent
fae1917aa7
commit
873924d6cd
|
@ -347,6 +347,7 @@ void AC_AutoTune::run()
|
|||
|
||||
}
|
||||
|
||||
// check if current is greater than maximum and update level_problem structure
|
||||
bool AC_AutoTune::check_level(const LevelIssue issue, const float current, const float maximum)
|
||||
{
|
||||
if (current > maximum) {
|
||||
|
@ -358,6 +359,7 @@ bool AC_AutoTune::check_level(const LevelIssue issue, const float current, const
|
|||
return true;
|
||||
}
|
||||
|
||||
// return true if vehicle is close to level
|
||||
bool AC_AutoTune::currently_level()
|
||||
{
|
||||
float threshold_mul = 1.0;
|
||||
|
@ -410,7 +412,8 @@ bool AC_AutoTune::currently_level()
|
|||
return true;
|
||||
}
|
||||
|
||||
// attitude_controller - sets attitude control targets during tuning
|
||||
// main state machine to level vehicle, perform a test and update gains
|
||||
// directly updates attitude controller with targets
|
||||
void AC_AutoTune::control_attitude()
|
||||
{
|
||||
rotation_rate = 0.0f; // rotation rate in radians/second
|
||||
|
@ -509,7 +512,6 @@ void AC_AutoTune::control_attitude()
|
|||
attitude_control->use_sqrt_controller(true);
|
||||
|
||||
// log the latest gains
|
||||
|
||||
Log_AutoTune();
|
||||
|
||||
switch (tune_type) {
|
||||
|
@ -570,7 +572,7 @@ void AC_AutoTune::control_attitude()
|
|||
tune_pitch_rp = MAX(get_rp_min(), tune_pitch_rp * AUTOTUNE_RD_BACKOFF);
|
||||
break;
|
||||
case YAW:
|
||||
tune_yaw_rLPF = MAX(get_rlpf_min(), tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF);
|
||||
tune_yaw_rLPF = MAX(get_yaw_rate_filt_min(), tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF);
|
||||
tune_yaw_rp = MAX(get_rp_min(), tune_yaw_rp * AUTOTUNE_RD_BACKOFF);
|
||||
break;
|
||||
}
|
||||
|
@ -816,7 +818,7 @@ void AC_AutoTune::load_tuned_gains()
|
|||
if (roll_enabled()) {
|
||||
if (!is_zero(tune_roll_rp) || allow_zero_rate_p()) {
|
||||
attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
|
||||
attitude_control->get_rate_roll_pid().kI(get_load_tuned_ri(axis));
|
||||
attitude_control->get_rate_roll_pid().kI(get_tuned_ri(axis));
|
||||
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
|
||||
attitude_control->get_rate_roll_pid().ff(tune_roll_rff);
|
||||
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
|
||||
|
@ -826,7 +828,7 @@ void AC_AutoTune::load_tuned_gains()
|
|||
if (pitch_enabled()) {
|
||||
if (!is_zero(tune_pitch_rp) || allow_zero_rate_p()) {
|
||||
attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
|
||||
attitude_control->get_rate_pitch_pid().kI(get_load_tuned_ri(axis));
|
||||
attitude_control->get_rate_pitch_pid().kI(get_tuned_ri(axis));
|
||||
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
|
||||
attitude_control->get_rate_pitch_pid().ff(tune_pitch_rff);
|
||||
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
|
||||
|
@ -836,8 +838,8 @@ void AC_AutoTune::load_tuned_gains()
|
|||
if (yaw_enabled()) {
|
||||
if (!is_zero(tune_yaw_rp)) {
|
||||
attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
|
||||
attitude_control->get_rate_yaw_pid().kI(get_load_tuned_ri(axis));
|
||||
attitude_control->get_rate_yaw_pid().kD(get_load_tuned_yaw_rd());
|
||||
attitude_control->get_rate_yaw_pid().kI(get_tuned_ri(axis));
|
||||
attitude_control->get_rate_yaw_pid().kD(get_tuned_yaw_rd());
|
||||
attitude_control->get_rate_yaw_pid().ff(tune_yaw_rff);
|
||||
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
|
||||
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
|
||||
|
|
|
@ -41,8 +41,6 @@
|
|||
|
||||
#define AUTOTUNE_DWELL_CYCLES 10
|
||||
|
||||
|
||||
|
||||
class AC_AutoTune
|
||||
{
|
||||
public:
|
||||
|
@ -68,10 +66,20 @@ public:
|
|||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
protected:
|
||||
|
||||
// axis 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)
|
||||
};
|
||||
|
||||
//
|
||||
// methods that must be supplied by the vehicle specific subclass
|
||||
//
|
||||
virtual bool init(void) = 0;
|
||||
|
||||
// get pilot input for desired cimb rate
|
||||
// get pilot input for desired climb rate
|
||||
virtual float get_pilot_desired_climb_rate_cms(void) const = 0;
|
||||
|
||||
// get pilot input for designed roll and pitch, and yaw rate
|
||||
|
@ -96,56 +104,105 @@ protected:
|
|||
// initialise position controller
|
||||
bool init_position_controller();
|
||||
|
||||
// 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)
|
||||
};
|
||||
|
||||
// main state machine to level vehicle, perform a test and update gains
|
||||
// directly updates attitude controller with targets
|
||||
void control_attitude();
|
||||
|
||||
//
|
||||
// methods to load and save gains
|
||||
//
|
||||
|
||||
// backup original gains and prepare for start of tuning
|
||||
void backup_gains_and_initialise();
|
||||
|
||||
// switch to use original gains
|
||||
void load_orig_gains();
|
||||
|
||||
// switch to gains found by last successful autotune
|
||||
void load_tuned_gains();
|
||||
|
||||
// load gains used between tests. called during testing mode's update-gains step to set gains ahead of return-to-level step
|
||||
void load_intra_test_gains();
|
||||
|
||||
// load gains for next test. relies on axis variable being set
|
||||
virtual void load_test_gains();
|
||||
|
||||
// get intra test rate I gain for the specified axis
|
||||
virtual float get_intra_test_ri(AxisType test_axis) = 0;
|
||||
|
||||
// get tuned rate I gain for the specified axis
|
||||
virtual float get_tuned_ri(AxisType test_axis) = 0;
|
||||
|
||||
// get tuned yaw rate d gain
|
||||
virtual float get_tuned_yaw_rd() = 0;
|
||||
|
||||
// test init and run methods that should be overridden for each vehicle
|
||||
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;
|
||||
|
||||
// return true if user has enabled autotune for roll, pitch or yaw axis
|
||||
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;
|
||||
|
||||
// Added generic twitch test functions for multi
|
||||
// twitch test functions for multicopter
|
||||
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
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// update gains for the angle p down tune type
|
||||
virtual void updating_angle_p_down_all(AxisType test_axis)=0;
|
||||
|
||||
// returns true if rate P gain of zero is acceptable for this vehicle
|
||||
virtual bool allow_zero_rate_p() = 0;
|
||||
|
||||
// get minimum rate P (for any axis)
|
||||
virtual float get_rp_min() const = 0;
|
||||
|
||||
// get minimum angle P (for any axis)
|
||||
virtual float get_sp_min() const = 0;
|
||||
|
||||
// get minimum rate Yaw filter value
|
||||
virtual float get_yaw_rate_filt_min() const = 0;
|
||||
|
||||
// get attitude for slow position hold in autotune mode
|
||||
void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
|
||||
|
||||
virtual void Log_AutoTune() = 0;
|
||||
virtual void Log_AutoTuneDetails() = 0;
|
||||
|
||||
// send message with high level status (e.g. Started, Stopped)
|
||||
void update_gcs(uint8_t message_id) const;
|
||||
|
||||
// send lower level step status (e.g. Pilot overrides Active)
|
||||
void send_step_string();
|
||||
|
||||
// convert latest level issue to string for reporting
|
||||
const char *level_issue_string() const;
|
||||
const char * type_string() const;
|
||||
void announce_state_to_gcs();
|
||||
|
||||
// convert tune type to string for reporting
|
||||
const char *type_string() const;
|
||||
|
||||
// send intermittant updates to user on status of tune
|
||||
virtual void do_gcs_announcements() = 0;
|
||||
|
||||
enum struct LevelIssue {
|
||||
|
@ -157,7 +214,11 @@ protected:
|
|||
RATE_PITCH,
|
||||
RATE_YAW,
|
||||
};
|
||||
|
||||
// check if current is greater than maximum and update level_problem structure
|
||||
bool check_level(const enum LevelIssue issue, const float current, const float maximum);
|
||||
|
||||
// returns true if vehicle is close to level
|
||||
bool currently_level();
|
||||
|
||||
// autotune modes (high level states)
|
||||
|
@ -188,6 +249,8 @@ protected:
|
|||
MAX_GAINS = 8, // max allowable stable gains are determined
|
||||
TUNE_COMPLETE = 9 // Reached end of tuning
|
||||
};
|
||||
TuneType tune_seq[6]; // holds sequence of tune_types to be performed
|
||||
uint8_t tune_seq_curr; // current tune sequence step
|
||||
|
||||
// type of gains to load
|
||||
enum GainType {
|
||||
|
@ -200,7 +263,7 @@ protected:
|
|||
|
||||
TuneMode mode; // see TuneMode for what modes are allowed
|
||||
bool pilot_override; // true = pilot is overriding controls so we suspend tuning temporarily
|
||||
AxisType axis; // see AxisType for which things can be tuned
|
||||
AxisType axis; // current axis being tuned. see AxisType enum
|
||||
bool positive_direction; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll)
|
||||
StepType step; // see StepType for what steps are performed
|
||||
TuneType tune_type; // see TuneType
|
||||
|
@ -208,7 +271,7 @@ protected:
|
|||
bool twitch_first_iter; // true on first iteration of a twitch (used to signal we must step the attitude or rate target)
|
||||
bool use_poshold; // true = enable position hold
|
||||
bool have_position; // true = start_position is value
|
||||
Vector3f start_position;
|
||||
Vector3f start_position; // target when holding position as an offset from EKF origin in cm in NEU frame
|
||||
uint8_t axes_completed; // bitmask of completed axes
|
||||
|
||||
// variables
|
||||
|
@ -256,6 +319,7 @@ protected:
|
|||
float current;
|
||||
} level_problem;
|
||||
|
||||
// parameters
|
||||
AP_Int8 axis_bitmask;
|
||||
AP_Float aggressiveness;
|
||||
AP_Float min_d;
|
||||
|
@ -267,16 +331,6 @@ protected:
|
|||
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
|
||||
|
@ -331,5 +385,4 @@ protected:
|
|||
|
||||
max_gain_data max_rate_p;
|
||||
max_gain_data max_rate_d;
|
||||
|
||||
};
|
||||
|
|
|
@ -17,6 +17,8 @@
|
|||
Converted to a library by Andrew Tridgell, and rewritten to include helicopters by Bill Geyer
|
||||
*/
|
||||
|
||||
#include "AC_AutoTune_Heli.h"
|
||||
|
||||
#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
|
||||
|
@ -36,8 +38,6 @@
|
|||
#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()
|
||||
{
|
||||
|
||||
|
@ -262,7 +262,7 @@ void AC_AutoTune_Heli::save_tuning_gains()
|
|||
reset();
|
||||
}
|
||||
|
||||
// generic method used to update gains for the rate p up tune type
|
||||
// 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;
|
||||
|
@ -294,7 +294,7 @@ void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis)
|
|||
}
|
||||
}
|
||||
|
||||
// generic method used to update gains for the rate d up tune type
|
||||
// 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;
|
||||
|
@ -326,7 +326,7 @@ void AC_AutoTune_Heli::updating_rate_d_up_all(AxisType test_axis)
|
|||
}
|
||||
}
|
||||
|
||||
// generic method used to update gains for the rate ff up tune type
|
||||
// update gains for the rate ff up tune type
|
||||
void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis)
|
||||
{
|
||||
switch (test_axis) {
|
||||
|
@ -342,7 +342,7 @@ void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis)
|
|||
}
|
||||
}
|
||||
|
||||
// generic method used to update gains for the angle p up tune type
|
||||
// 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
|
||||
|
@ -361,7 +361,7 @@ void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
|
|||
}
|
||||
}
|
||||
|
||||
// generic method used to update gains for the max gain tune type
|
||||
// update gains for the max gain tune type
|
||||
void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis)
|
||||
{
|
||||
|
||||
|
@ -675,6 +675,7 @@ void AC_AutoTune_Heli::Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate
|
|||
rate_rads*57.3f);
|
||||
}
|
||||
|
||||
// get intra test rate I gain for the specified axis
|
||||
float AC_AutoTune_Heli::get_intra_test_ri(AxisType test_axis)
|
||||
{
|
||||
float ret = 0.0f;
|
||||
|
@ -692,7 +693,8 @@ float AC_AutoTune_Heli::get_intra_test_ri(AxisType test_axis)
|
|||
return ret;
|
||||
}
|
||||
|
||||
float AC_AutoTune_Heli::get_load_tuned_ri(AxisType test_axis)
|
||||
// get tuned rate I gain for the specified axis
|
||||
float AC_AutoTune_Heli::get_tuned_ri(AxisType test_axis)
|
||||
{
|
||||
float ret = 0.0f;
|
||||
switch (test_axis) {
|
||||
|
@ -709,16 +711,21 @@ float AC_AutoTune_Heli::get_load_tuned_ri(AxisType test_axis)
|
|||
return ret;
|
||||
}
|
||||
|
||||
// get minimum rate P (for any axis)
|
||||
float AC_AutoTune_Heli::get_rp_min() const
|
||||
{
|
||||
return AUTOTUNE_RP_MIN;
|
||||
}
|
||||
|
||||
// get minimum angle P (for any axis)
|
||||
float AC_AutoTune_Heli::get_sp_min() const
|
||||
{
|
||||
return AUTOTUNE_SP_MIN;
|
||||
}
|
||||
|
||||
float AC_AutoTune_Heli::get_rlpf_min() const
|
||||
// get minimum rate Yaw filter value
|
||||
float AC_AutoTune_Heli::get_yaw_rate_filt_min() const
|
||||
{
|
||||
return AUTOTUNE_RLPF_MIN;
|
||||
}
|
||||
|
||||
|
|
|
@ -13,8 +13,7 @@
|
|||
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
|
||||
support for autotune of helicopters
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
@ -40,41 +39,67 @@ public:
|
|||
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
|
||||
// get intra test rate I gain for the specified axis
|
||||
float get_intra_test_ri(AxisType test_axis) override;
|
||||
|
||||
// get tuned rate I gain for the specified axis
|
||||
float get_tuned_ri(AxisType test_axis) override;
|
||||
|
||||
// get tuned yaw rate d gain
|
||||
float get_tuned_yaw_rd() override { return tune_yaw_rd; }
|
||||
|
||||
void test_init() override;
|
||||
void test_run(AxisType test_axis, const float dir_sign) override;
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// update gains for the max gain tune type
|
||||
void updating_max_gains_all(AxisType test_axis) override;
|
||||
|
||||
// get minimum rate P (for any axis)
|
||||
float get_rp_min() const override;
|
||||
|
||||
// get minimum angle P (for any axis)
|
||||
float get_sp_min() const override;
|
||||
|
||||
// get minimum rate Yaw filter value
|
||||
float get_yaw_rate_filt_min() const 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;
|
||||
|
||||
// returns true if rate P gain of zero is acceptable for this vehicle
|
||||
bool allow_zero_rate_p() override { return true; }
|
||||
|
||||
// send intermittant updates to user on status of tune
|
||||
void do_gcs_announcements() override;
|
||||
|
||||
private:
|
||||
// updating_rate_ff_up - adjust FF to ensure the target is reached
|
||||
|
|
|
@ -36,6 +36,17 @@
|
|||
|
||||
#include "AC_AutoTune_Multi.h"
|
||||
|
||||
// constructor
|
||||
AC_AutoTune_Multi::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;
|
||||
}
|
||||
|
||||
void AC_AutoTune_Multi::do_gcs_announcements()
|
||||
{
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
|
@ -199,7 +210,7 @@ void AC_AutoTune_Multi::save_tuning_gains()
|
|||
reset();
|
||||
}
|
||||
|
||||
// generic method used to update gains for the rate p up tune type
|
||||
// update gains for the rate p up tune type
|
||||
void AC_AutoTune_Multi::updating_rate_p_up_all(AxisType test_axis)
|
||||
{
|
||||
switch (test_axis) {
|
||||
|
@ -215,7 +226,7 @@ void AC_AutoTune_Multi::updating_rate_p_up_all(AxisType test_axis)
|
|||
}
|
||||
}
|
||||
|
||||
// generic method used to update gains for the rate d up tune type
|
||||
// update gains for the rate d up tune type
|
||||
void AC_AutoTune_Multi::updating_rate_d_up_all(AxisType test_axis)
|
||||
{
|
||||
switch (test_axis) {
|
||||
|
@ -231,7 +242,7 @@ void AC_AutoTune_Multi::updating_rate_d_up_all(AxisType test_axis)
|
|||
}
|
||||
}
|
||||
|
||||
// generic method used to update gains for the rate d down tune type
|
||||
// update gains for the rate d down tune type
|
||||
void AC_AutoTune_Multi::updating_rate_d_down_all(AxisType test_axis)
|
||||
{
|
||||
switch (test_axis) {
|
||||
|
@ -247,7 +258,7 @@ void AC_AutoTune_Multi::updating_rate_d_down_all(AxisType test_axis)
|
|||
}
|
||||
}
|
||||
|
||||
// generic method used to update gains for the angle p up tune type
|
||||
// update gains for the angle p up tune type
|
||||
void AC_AutoTune_Multi::updating_angle_p_up_all(AxisType test_axis)
|
||||
{
|
||||
switch (test_axis) {
|
||||
|
@ -263,7 +274,7 @@ void AC_AutoTune_Multi::updating_angle_p_up_all(AxisType test_axis)
|
|||
}
|
||||
}
|
||||
|
||||
// generic method used to update gains for the angle p down tune type
|
||||
// update gains for the angle p down tune type
|
||||
void AC_AutoTune_Multi::updating_angle_p_down_all(AxisType test_axis)
|
||||
{
|
||||
switch (test_axis) {
|
||||
|
@ -587,6 +598,7 @@ void AC_AutoTune_Multi::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds
|
|||
rate_cds*0.01f);
|
||||
}
|
||||
|
||||
// get intra test rate I gain for the specified axis
|
||||
float AC_AutoTune_Multi::get_intra_test_ri(AxisType test_axis)
|
||||
{
|
||||
float ret = 0.0f;
|
||||
|
@ -604,7 +616,8 @@ float AC_AutoTune_Multi::get_intra_test_ri(AxisType test_axis)
|
|||
return ret;
|
||||
}
|
||||
|
||||
float AC_AutoTune_Multi::get_load_tuned_ri(AxisType test_axis)
|
||||
// get tuned rate I gain for the specified axis
|
||||
float AC_AutoTune_Multi::get_tuned_ri(AxisType test_axis)
|
||||
{
|
||||
float ret = 0.0f;
|
||||
switch (test_axis) {
|
||||
|
@ -621,17 +634,20 @@ float AC_AutoTune_Multi::get_load_tuned_ri(AxisType test_axis)
|
|||
return ret;
|
||||
}
|
||||
|
||||
// get minimum rate P (for any axis)
|
||||
float AC_AutoTune_Multi::get_rp_min() const
|
||||
{
|
||||
return AUTOTUNE_RP_MIN;
|
||||
}
|
||||
|
||||
// get minimum angle P (for any axis)
|
||||
float AC_AutoTune_Multi::get_sp_min() const
|
||||
{
|
||||
return AUTOTUNE_SP_MIN;
|
||||
}
|
||||
|
||||
float AC_AutoTune_Multi::get_rlpf_min() const
|
||||
// get minimum rate Yaw filter value
|
||||
float AC_AutoTune_Multi::get_yaw_rate_filt_min() const
|
||||
{
|
||||
return AUTOTUNE_RLPF_MIN;
|
||||
}
|
||||
|
|
|
@ -25,69 +25,90 @@ 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;
|
||||
};
|
||||
AC_AutoTune_Multi();
|
||||
|
||||
// save gained, called on disarm
|
||||
void save_tuning_gains() override;
|
||||
|
||||
protected:
|
||||
// load gains
|
||||
void load_test_gains() override;
|
||||
|
||||
// get intra test rate I gain for the specified axis
|
||||
float get_intra_test_ri(AxisType test_axis) override;
|
||||
|
||||
// get tuned rate I gain for the specified axis
|
||||
float get_tuned_ri(AxisType test_axis) override;
|
||||
|
||||
// get tuned yaw rate D gain
|
||||
float get_tuned_yaw_rd() override { return 0.0f; }
|
||||
|
||||
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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
// update gains for the max gain tune type
|
||||
void updating_max_gains_all(AxisType test_axis) override {};
|
||||
|
||||
// returns true if rate P gain of zero is acceptable for this vehicle
|
||||
bool allow_zero_rate_p() override { return false; }
|
||||
|
||||
// get minimum rate P (for any axis)
|
||||
float get_rp_min() const override;
|
||||
|
||||
// get minimum angle P (for any axis)
|
||||
float get_sp_min() const override;
|
||||
|
||||
// get minimum yaw rate filter value
|
||||
float get_yaw_rate_filt_min() const 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);
|
||||
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue