mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
Copter: convert to use AC_AutoTune library
this maintains existing behaviour
This commit is contained in:
parent
1b90ef34cc
commit
89d54767b1
@ -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
|
||||
|
@ -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),
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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[];
|
||||
|
@ -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,
|
||||
|
@ -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
@ -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");
|
||||
}
|
||||
|
@ -32,6 +32,7 @@ def build(bld):
|
||||
'AP_Follow',
|
||||
'AP_Devo_Telem',
|
||||
'AP_OSD',
|
||||
'AC_AutoTune',
|
||||
],
|
||||
)
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user