Copter: convert to use AC_AutoTune library

this maintains existing behaviour
This commit is contained in:
Andrew Tridgell 2018-12-13 14:53:56 +11:00 committed by Randy Mackay
parent 1b90ef34cc
commit 89d54767b1
9 changed files with 156 additions and 1743 deletions

View File

@ -82,6 +82,7 @@
#include <AP_Arming/AP_Arming.h>
#include <AP_SmartRTL/AP_SmartRTL.h>
#include <AP_TempCalibration/AP_TempCalibration.h>
#include <AC_AutoTune/AC_AutoTune.h>
// Configuration
#include "defines.h"
@ -263,6 +264,7 @@ private:
NavEKF2 EKF2{&ahrs, rangefinder};
NavEKF3 EKF3{&ahrs, rangefinder};
AP_AHRS_NavEKF ahrs{EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
AP_AHRS_View *ahrs_view;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL::SITL sitl;
@ -912,6 +914,7 @@ private:
ModeAuto mode_auto;
#endif
#if AUTOTUNE_ENABLED == ENABLED
AutoTune autotune;
ModeAutoTune mode_autotune;
#endif
#if MODE_BRAKE_ENABLED == ENABLED

View File

@ -5,60 +5,6 @@
// Code to Write and Read packets from DataFlash log memory
// Code to interact with the user to dump or erase logs
#if AUTOTUNE_ENABLED == ENABLED
struct PACKED log_AutoTune {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t axis; // roll or pitch
uint8_t tune_step; // tuning PI or D up or down
float meas_target; // target achieved rotation rate
float meas_min; // maximum achieved rotation rate
float meas_max; // maximum achieved rotation rate
float new_gain_rp; // newly calculated gain
float new_gain_rd; // newly calculated gain
float new_gain_sp; // newly calculated gain
float new_ddt; // newly calculated gain
};
// Write an Autotune data packet
void Copter::ModeAutoTune::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt)
{
struct log_AutoTune pkt = {
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG),
time_us : AP_HAL::micros64(),
axis : _axis,
tune_step : tune_step,
meas_target : meas_target,
meas_min : meas_min,
meas_max : meas_max,
new_gain_rp : new_gain_rp,
new_gain_rd : new_gain_rd,
new_gain_sp : new_gain_sp,
new_ddt : new_ddt
};
copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_AutoTuneDetails {
LOG_PACKET_HEADER;
uint64_t time_us;
float angle_cd; // lean angle in centi-degrees
float rate_cds; // current rotation rate in centi-degrees / second
};
// Write an Autotune data packet
void Copter::ModeAutoTune::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
{
struct log_AutoTuneDetails pkt = {
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG),
time_us : AP_HAL::micros64(),
angle_cd : angle_cd,
rate_cds : rate_cds
};
copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
#endif
struct PACKED log_Control_Tuning {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -472,12 +418,6 @@ void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_tar
// units and "Format characters" for field type information
const struct LogStructure Copter::log_structure[] = {
LOG_COMMON_STRUCTURES,
#if AUTOTUNE_ENABLED == ENABLED
{ LOG_AUTOTUNE_MSG, sizeof(log_AutoTune),
"ATUN", "QBBfffffff", "TimeUS,Axis,TuneStep,Targ,Min,Max,RP,RD,SP,ddt", "s--ddd---o", "F--BBB---0" },
{ LOG_AUTOTUNEDETAILS_MSG, sizeof(log_AutoTuneDetails),
"ATDE", "Qff", "TimeUS,Angle,Rate", "sdk", "FBB" },
#endif
{ LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
"PTUN", "QBfHHH", "TimeUS,Param,TunVal,CtrlIn,TunLo,TunHi", "s-----", "F-----" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),

View File

@ -707,30 +707,6 @@ const AP_Param::Info Copter::var_info[] = {
GOBJECT(avoidance_adsb, "AVD_", AP_Avoidance_Copter),
#endif
#if AUTOTUNE_ENABLED == ENABLED
// @Param: AUTOTUNE_AXES
// @DisplayName: Autotune axis bitmask
// @Description: 1-byte bitmap of axes to autotune
// @Values: 7:All,1:Roll Only,2:Pitch Only,4:Yaw Only,3:Roll and Pitch,5:Roll and Yaw,6:Pitch and Yaw
// @Bitmask: 0:Roll,1:Pitch,2:Yaw
// @User: Standard
GSCALAR(autotune_axis_bitmask, "AUTOTUNE_AXES", 7), // AUTOTUNE_AXIS_BITMASK_DEFAULT
// @Param: AUTOTUNE_AGGR
// @DisplayName: Autotune aggressiveness
// @Description: Autotune aggressiveness. Defines the bounce back used to detect size of the D term.
// @Range: 0.05 0.10
// @User: Standard
GSCALAR(autotune_aggressiveness, "AUTOTUNE_AGGR", 0.1f),
// @Param: AUTOTUNE_MIN_D
// @DisplayName: AutoTune minimum D
// @Description: Defines the minimum D gain
// @Range: 0.001 0.006
// @User: Standard
GSCALAR(autotune_min_d, "AUTOTUNE_MIN_D", 0.001f),
#endif
// @Group: NTF_
// @Path: ../libraries/AP_Notify/AP_Notify.cpp
GOBJECT(notify, "NTF_", AP_Notify),
@ -948,6 +924,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(user_parameters, "USR", 28, ParametersG2, UserParameters),
#endif
#if AUTOTUNE_ENABLED == ENABLED
// @Group: AUTOTUNE_
// @Path: ../libraries/AC_AutoTune/AC_AutoTune.cpp
AP_SUBGROUPPTR(autotune_ptr, "AUTOTUNE_", 29, ParametersG2, Copter::AutoTune),
#endif
AP_GROUPEND
};
@ -1016,6 +998,9 @@ ParametersG2::ParametersG2(void)
#ifdef USER_PARAMS_ENABLED
,user_parameters()
#endif
#if AUTOTUNE_ENABLED == ENABLED
,autotune_ptr(&copter.autotune)
#endif
{
AP_Param::setup_object_defaults(this, var_info);
}

View File

@ -350,13 +350,13 @@ public:
k_param_acro_balance_roll,
k_param_acro_balance_pitch,
k_param_acro_yaw_p,
k_param_autotune_axis_bitmask,
k_param_autotune_aggressiveness,
k_param_autotune_axis_bitmask, // remove
k_param_autotune_aggressiveness, // remove
k_param_pi_vel_xy, // remove
k_param_fs_ekf_action,
k_param_rtl_climb_min,
k_param_rpm_sensor,
k_param_autotune_min_d, // 251
k_param_autotune_min_d, // remove
k_param_arming, // 252 - AP_Arming
k_param_DataFlash = 253, // 253 - Logging Group
@ -458,13 +458,6 @@ public:
AP_Int8 acro_trainer;
AP_Float acro_rp_expo;
// Autotune
#if AUTOTUNE_ENABLED == ENABLED
AP_Int8 autotune_axis_bitmask;
AP_Float autotune_aggressiveness;
AP_Float autotune_min_d;
#endif
// Note: keep initializers here in the same order as they are declared
// above.
Parameters()
@ -586,6 +579,10 @@ public:
UserParameters user_parameters;
#endif
#if AUTOTUNE_ENABLED == ENABLED
// we need a pointer to autotune for the G2 table
void *autotune_ptr;
#endif
};
extern const AP_Param::Info var_info[];

View File

@ -247,8 +247,6 @@ enum LoggingParameters {
LOG_DATA_INT32_MSG,
LOG_DATA_UINT32_MSG,
LOG_DATA_FLOAT_MSG,
LOG_AUTOTUNE_MSG,
LOG_AUTOTUNEDETAILS_MSG,
LOG_MOTBATT_MSG,
LOG_PARAMTUNE_MSG,
LOG_HELI_MSG,

View File

@ -439,6 +439,24 @@ private:
};
#if AUTOTUNE_ENABLED == ENABLED
/*
wrapper class for AC_AutoTune
*/
class AutoTune : public AC_AutoTune
{
public:
bool init() override;
void run() override;
protected:
bool start(void) override;
bool position_ok() override;
float get_pilot_desired_climb_rate_cms(void) const override;
void get_pilot_desired_rp_yrate_cd(int32_t &roll_cd, int32_t &pitch_cd, int32_t &yaw_rate_cds) override;
void init_z_limits() override;
void Log_Write_Event(enum at_event id) override;
};
class ModeAutoTune : public Mode {
public:
@ -446,150 +464,19 @@ public:
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
void run() override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return false; }
bool is_autopilot() const override { return false; }
void save_tuning_gains();
void stop();
protected:
const char *name() const override { return "AUTOTUNE"; }
const char *name4() const override { return "ATUN"; }
private:
bool start(bool ignore_checks);
void autotune_attitude_control();
void backup_gains_and_initialise();
void load_orig_gains();
void load_tuned_gains();
void load_intra_test_gains();
void load_twitch_gains();
void update_gcs(uint8_t message_id);
bool roll_enabled();
bool pitch_enabled();
bool yaw_enabled();
void twitching_test_rate(float rate, float rate_target, float &meas_rate_min, float &meas_rate_max);
void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max);
void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max);
void updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max);
void updating_rate_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max);
void updating_rate_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max);
void updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max);
void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max);
void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
#if LOGGING_ENABLED == ENABLED
void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);
#endif
void send_step_string();
const char *level_issue_string() const;
const char * type_string() const;
void announce_state_to_gcs();
void do_gcs_announcements();
enum LEVEL_ISSUE {
LEVEL_ISSUE_NONE,
LEVEL_ISSUE_ANGLE_ROLL,
LEVEL_ISSUE_ANGLE_PITCH,
LEVEL_ISSUE_ANGLE_YAW,
LEVEL_ISSUE_RATE_ROLL,
LEVEL_ISSUE_RATE_PITCH,
LEVEL_ISSUE_RATE_YAW,
};
bool check_level(const enum LEVEL_ISSUE issue, const float current, const float maximum);
bool currently_level();
// autotune modes (high level states)
enum TuneMode {
UNINITIALISED = 0, // autotune has never been run
TUNING = 1, // autotune is testing gains
SUCCESS = 2, // tuning has completed, user is flight testing the new gains
FAILED = 3, // tuning has failed, user is flying on original gains
};
// steps performed while in the tuning mode
enum StepType {
WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch
TWITCHING = 1, // autotune has begun a twitch and is watching the resulting vehicle movement
UPDATE_GAINS = 2 // autotune has completed a twitch and is updating the gains based on the results
};
// things that can be tuned
enum AxisType {
ROLL = 0, // roll axis is being tuned (either angle or rate)
PITCH = 1, // pitch axis is being tuned (either angle or rate)
YAW = 2, // pitch axis is being tuned (either angle or rate)
};
// mini steps performed while in Tuning mode, Testing step
enum TuneType {
RD_UP = 0, // rate D is being tuned up
RD_DOWN = 1, // rate D is being tuned down
RP_UP = 2, // rate P is being tuned up
SP_DOWN = 3, // angle P is being tuned down
SP_UP = 4 // angle P is being tuned up
};
TuneMode mode : 2; // see TuneMode for what modes are allowed
bool pilot_override : 1; // true = pilot is overriding controls so we suspend tuning temporarily
AxisType axis : 2; // see AxisType for which things can be tuned
bool positive_direction : 1; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll)
StepType step : 2; // see StepType for what steps are performed
TuneType tune_type : 3; // see TuneType
bool ignore_next : 1; // true = ignore the next test
bool twitch_first_iter : 1; // true on first iteration of a twitch (used to signal we must step the attitude or rate target)
bool use_poshold : 1; // true = enable position hold
bool have_position : 1; // true = start_position is value
Vector3f start_position;
// variables
uint32_t override_time; // the last time the pilot overrode the controls
float test_rate_min; // the minimum angular rate achieved during TESTING_RATE step
float test_rate_max; // the maximum angular rate achieved during TESTING_RATE step
float test_angle_min; // the minimum angle achieved during TESTING_ANGLE step
float test_angle_max; // the maximum angle achieved during TESTING_ANGLE step
uint32_t step_start_time; // start time of current tuning step (used for timeout checks)
uint32_t step_stop_time; // start time of current tuning step (used for timeout checks)
int8_t counter; // counter for tuning gains
float target_rate, start_rate; // target and start rate
float target_angle, start_angle; // target and start angles
float desired_yaw; // yaw heading during tune
float rate_max, test_accel_max; // maximum acceleration variables
LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second
// backup of currently being tuned parameter values
float orig_roll_rp = 0, orig_roll_ri, orig_roll_rd, orig_roll_sp, orig_roll_accel;
float orig_pitch_rp = 0, orig_pitch_ri, orig_pitch_rd, orig_pitch_sp, orig_pitch_accel;
float orig_yaw_rp = 0, orig_yaw_ri, orig_yaw_rd, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel;
bool orig_bf_feedforward;
// currently being tuned parameter values
float tune_roll_rp, tune_roll_rd, tune_roll_sp, tune_roll_accel;
float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel;
float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel;
uint32_t announce_time;
float lean_angle;
float rotation_rate;
float roll_cd, pitch_cd;
struct {
LEVEL_ISSUE issue{LEVEL_ISSUE_NONE};
float maximum;
float current;
} level_problem;
};
#endif

File diff suppressed because it is too large Load Diff

View File

@ -551,7 +551,7 @@ void Copter::allocate_motors(void)
}
AP_Param::load_object_from_eeprom(motors, motors_var_info);
AP_AHRS_View *ahrs_view = ahrs.create_view(ROTATION_NONE);
ahrs_view = ahrs.create_view(ROTATION_NONE);
if (ahrs_view == nullptr) {
AP_HAL::panic("Unable to allocate AP_AHRS_View");
}

View File

@ -32,6 +32,7 @@ def build(bld):
'AP_Follow',
'AP_Devo_Telem',
'AP_OSD',
'AC_AutoTune',
],
)