Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Leonard's AutoTuning for Roll and Pitch

Browse Source
This commit is contained in:
Randy Mackay 2013-09-03 18:31:06 +09:00
parent ed7f831c18
commit 31cea0140d
7 changed files with 669 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/ArduCopter.pde
View File

@ -1684,6 +1684,11 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
roll_pitch_initialised = true; roll_pitch_initialised = true;
} }
break; break;
case ROLL_PITCH_AUTOTUNE:
// indicate we can enter this mode successfully
roll_pitch_initialised = true;
break;
} }
// if initialisation has been successful update the yaw mode // if initialisation has been successful update the yaw mode
@ -1802,6 +1807,23 @@ void update_roll_pitch_mode(void)
get_roll_rate_stabilized_ef(g.rc_1.control_in); get_roll_rate_stabilized_ef(g.rc_1.control_in);
get_pitch_rate_stabilized_ef(g.rc_2.control_in); get_pitch_rate_stabilized_ef(g.rc_2.control_in);
break; break;
case ROLL_PITCH_AUTOTUNE:
// apply SIMPLE mode transform
if(ap.simple_mode && ap_system.new_radio_frame) {
update_simple_mode();
}
// convert pilot input to lean angles
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
// pass desired roll, pitch to stabilize attitude controllers
get_stabilize_roll(control_roll);
get_stabilize_pitch(control_pitch);
// copy user input for reporting purposes
get_autotune_roll_pitch_controller(g.rc_1.control_in, g.rc_2.control_in);
break;
} }
#if FRAME_CONFIG != HELI_FRAME #if FRAME_CONFIG != HELI_FRAME

50
ArduCopter/Log.pde
View File

@ -160,6 +160,50 @@ process_logs(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
struct PACKED log_AutoTune {
LOG_PACKET_HEADER;
uint8_t axis; // roll or pitch
uint8_t tune_step; // tuning PI or D up or down
float rate_min; // maximum achieved rotation rate
float rate_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
};
// Write an Current data packet
static void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float rate_min, float rate_max, float new_gain_rp, float new_gain_rd, float new_gain_sp)
{
struct log_AutoTune pkt = {
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG),
axis : axis,
tune_step : tune_step,
rate_min : rate_min,
rate_max : rate_max,
new_gain_rp : new_gain_rp,
new_gain_rd : new_gain_rd,
new_gain_sp : new_gain_sp
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_AutoTuneDetails {
LOG_PACKET_HEADER;
int16_t angle_cd; // lean angle in centi-degrees
float rate_cds; // current rotation rate in centi-degrees / second
};
// Write an Current data packet
static void Log_Write_AutoTuneDetails(int16_t angle_cd, float rate_cds)
{
struct log_AutoTuneDetails pkt = {
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG),
angle_cd : angle_cd,
rate_cds : rate_cds
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Current { struct PACKED log_Current {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
int16_t throttle_in; int16_t throttle_in;
@ -714,6 +758,10 @@ static void Log_Write_Error(uint8_t sub_system, uint8_t error_code)
static const struct LogStructure log_structure[] PROGMEM = { static const struct LogStructure log_structure[] PROGMEM = {
LOG_COMMON_STRUCTURES, LOG_COMMON_STRUCTURES,
{ LOG_AUTOTUNE_MSG, sizeof(log_AutoTune),
"ATUN", "BBfffff", "Axis,TuneStep,RateMin,RateMax,RPGain,RDGain,SPGain" },
{ LOG_AUTOTUNEDETAILS_MSG, sizeof(log_AutoTuneDetails),
"ATDE", "cf", "Angle,Rate" },
{ LOG_CURRENT_MSG, sizeof(log_Current), { LOG_CURRENT_MSG, sizeof(log_Current),
"CURR", "hIhhhf", "Thr,ThrInt,Volt,Curr,Vcc,CurrTot" }, "CURR", "hIhhhf", "Thr,ThrInt,Volt,Curr,Vcc,CurrTot" },
@ -807,6 +855,8 @@ static void Log_Write_Cmd(uint8_t num, const struct Location *wp) {}
static void Log_Write_Mode(uint8_t mode) {} static void Log_Write_Mode(uint8_t mode) {}
static void Log_Write_IMU() {} static void Log_Write_IMU() {}
static void Log_Write_GPS() {} static void Log_Write_GPS() {}
static void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float rate_min, float rate_max, float new_gain_rp, float new_gain_rd, float new_gain_sp) {}
static void Log_Write_AutoTuneDetails(int16_t angle_cd, float rate_cds) {}
static void Log_Write_Current() {} static void Log_Write_Current() {}
static void Log_Write_Compass() {} static void Log_Write_Compass() {}
static void Log_Write_Attitude() {} static void Log_Write_Attitude() {}

4
ArduCopter/Parameters.pde
View File

@ -388,14 +388,14 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: CH7_OPT // @Param: CH7_OPT
// @DisplayName: Channel 7 option // @DisplayName: Channel 7 option
// @Description: Select which function if performed when CH7 is above 1800 pwm // @Description: Select which function if performed when CH7 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:Sonar, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:Sonar, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune
// @User: Standard // @User: Standard
GSCALAR(ch7_option, "CH7_OPT", CH7_OPTION), GSCALAR(ch7_option, "CH7_OPT", CH7_OPTION),
// @Param: CH8_OPT // @Param: CH8_OPT
// @DisplayName: Channel 8 option // @DisplayName: Channel 8 option
// @Description: Select which function if performed when CH8 is above 1800 pwm // @Description: Select which function if performed when CH8 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:Sonar, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:Sonar, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune
// @User: Standard // @User: Standard
GSCALAR(ch8_option, "CH8_OPT", CH8_OPTION), GSCALAR(ch8_option, "CH8_OPT", CH8_OPTION),

566
ArduCopter/auto_tune.pde Normal file
View File

@ -0,0 +1,566 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
Auto tuning works in this way:
i) set up 3-position ch7 or ch8 switch to "AutoTune"
2) take-off in stabilize mode, put the copter into a level hover and switch ch7/ch8 to high position to start tuning
3) auto tuner brings roll and pitch level
4) auto tuner begins tuning roll right
a) invokes 90 deg/sec roll right
b) records maximum "forward" roll rate
c) when copter reaches 20 degrees or 1 second has passed, it commands level
d) records maximum "return" roll rate
e) tries to maintain level for 1 second
f) records maximum overshoot angle
g) roll rate gains are adjusted
h) state is updated to tune roll left
5) step #4 is repeated for roll-right, pitch-forward, pitch-back before returning to roll-right
If pilot inputs any stick inputs these becomes the desired roll, pitch angles sent to the stabilize controller and the tuner is disabled until the sticks are put back into the middle for 1 second
*/
#define AUTO_TUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds
#define AUTO_TUNE_TARGET_RATE_TIMEOUT_MS 500 // timeout for rate test step
#define AUTO_TUNE_TARGET_RATE_CDS 9000 // target roll/pitch rate during AUTO_TUNE_STEP_TESTING step
#define AUTO_TUNE_LEVEL_ANGLE_CD 300 // angle which qualifies as level
#define AUTO_TUNE_TARGET_ANGLE_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTO_TUNE_REQUIRED_LEVEL_TIME_MS 250 // time we require the copter to be level
#define AUTO_TUNE_AGGRESSIVENESS 0.1f // tuning for 10% overshoot
#define AUTO_TUNE_RD_STEP 0.0005f // minimum increment when increasing/decreasing Rate D term
#define AUTO_TUNE_RP_STEP 0.005f // minimum increment when increasing/decreasing Rate P term
#define AUTO_TUNE_SP_STEP 0.5f // minimum increment when increasing/decreasing Stab P term
#define AUTO_TUNE_SP_BACKOFF 0.75f // back off on the Stab P tune
#define AUTO_TUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing
#define AUTO_TUNE_RP_RATIO_FINAL 1.0f // I is set 1x P after testing
#define AUTO_TUNE_RD_MIN 0.0f // minimum Rate D value
#define AUTO_TUNE_RD_MAX 0.1f // maximum Rate D value
#define AUTO_TUNE_RP_MIN 0.05f // minimum Rate P value
#define AUTO_TUNE_RP_MAX 1.0f // maximum Rate P value
#define AUTO_TUNE_SP_MIN 1.0f // minimum Stab P value
#define AUTO_TUNE_SP_MAX 15.0f // maximum Stab P value
#define AUTO_TUNE_SUCCESS_COUNT 4 // how many successful iterations we need to freeze at current gains
// things that can be tuned
enum AutoTuneAxisType {
AUTO_TUNE_AXIS_ROLL = 0,
AUTO_TUNE_AXIS_PITCH = 1
};
// steps performed during tuning
enum AutoTuneStepType {
AUTO_TUNE_STEP_WAITING_FOR_LEVEL = 0,
AUTO_TUNE_STEP_TESTING = 1,
AUTO_TUNE_STEP_UPDATE_GAINS = 2
};
// steps performed during tuning
enum AutoTuneTuneType {
AUTO_TUNE_TYPE_RD_UP = 0,
AUTO_TUNE_TYPE_RD_DOWN = 1,
AUTO_TUNE_TYPE_RP_UP = 2,
AUTO_TUNE_TYPE_SP_UP = 3
};
// state
struct auto_tune_state_struct {
uint8_t enabled : 1; // 0 = disabled, 1 = enabled
uint8_t active : 1; // 0 = inactive (temporarily suspended), 1 = actively tuning
uint8_t pilot_override : 1; // 1 = pilot is overriding controls so we suspend tuning temporarily
AutoTuneAxisType axis : 1; // see AutoTuneAxisType for which things can be tuned
uint8_t positive_direction : 1; // 0 = tuning in negative direction (i.e. left for roll), 1 = positive direction (i.e. right for roll)
AutoTuneStepType step : 2; // see AutoTuneStepType for what steps are performed
AutoTuneTuneType tune_type : 2; // see AutoTuneTuneType
} auto_tune_state;
// variables
uint32_t auto_tune_override_time; // the last time the pilot overrode the controls
float auto_tune_test_min; // the minimum angular rate achieved during TESTING_RATE step
float auto_tune_test_max; // the maximum angular rate achieved during TESTING_RATE step
uint32_t auto_tune_timer; // generic timer variable
int8_t auto_tune_counter; // counter for tuning gains
float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_sp; // backup of currently being tuned parameter values
float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_sp; // backup of currently being tuned parameter values
float tune_roll_rp, tune_roll_rd, tune_roll_sp; // currently being tuned parameter values
float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp; // currently being tuned parameter values
// store current pids as originals
void auto_tune_save_orig_pids()
{
orig_roll_rp = g.pid_rate_roll.kP();
orig_roll_ri = g.pid_rate_roll.kI();
orig_roll_rd = g.pid_rate_roll.kD();
orig_roll_sp = g.pi_stabilize_roll.kP();
orig_pitch_rp = g.pid_rate_pitch.kP();
orig_pitch_ri = g.pid_rate_pitch.kI();
orig_pitch_rd = g.pid_rate_pitch.kD();
orig_pitch_sp = g.pi_stabilize_pitch.kP();
}
// auto_tune_restore_orig_pids - restore pids to their original values
void auto_tune_restore_orig_pids()
{
g.pid_rate_roll.kP(orig_roll_rp);
g.pid_rate_roll.kI(orig_roll_ri);
g.pid_rate_roll.kD(orig_roll_rd);
g.pi_stabilize_roll.kP(orig_roll_sp);
g.pid_rate_pitch.kP(orig_pitch_rp);
g.pid_rate_pitch.kI(orig_pitch_ri);
g.pid_rate_pitch.kD(orig_pitch_rd);
g.pi_stabilize_pitch.kP(orig_pitch_sp);
}
// auto_tune_load_tuned_pids - restore pids to their original values
void auto_tune_load_tuned_pids()
{
if (tune_roll_rp != 0 && tune_pitch_rp != 0) {
g.pid_rate_roll.kP(tune_roll_rp);
g.pid_rate_roll.kI(tune_roll_rp*AUTO_TUNE_RP_RATIO_FINAL);
g.pid_rate_roll.kD(tune_roll_rd);
g.pi_stabilize_roll.kP(tune_roll_sp);
g.pid_rate_pitch.kP(tune_pitch_rp);
g.pid_rate_pitch.kI(tune_pitch_rp*AUTO_TUNE_RP_RATIO_FINAL);
g.pid_rate_pitch.kD(tune_pitch_rd);
g.pi_stabilize_pitch.kP(tune_pitch_sp);
}
}
// start an auto tuning session
void auto_tune_start()
{
// check we are in stabilize mode
if (control_mode == STABILIZE || control_mode == ALT_HOLD) {
// reset axis if this is our first time (i.e. we were not just suspended)
if (!auto_tune_state.active) {
auto_tune_state.active = true;
auto_tune_state.axis = AUTO_TUNE_AXIS_ROLL;
auto_tune_state.positive_direction = false;
auto_tune_state.step = AUTO_TUNE_STEP_WAITING_FOR_LEVEL;
auto_tune_timer = millis();
auto_tune_state.tune_type = AUTO_TUNE_TYPE_RD_UP;
auto_tune_save_orig_pids();
// initialise tuned pid values
tune_roll_rp = g.pid_rate_roll.kP();
tune_roll_rd = g.pid_rate_roll.kD();
tune_roll_sp = g.pi_stabilize_roll.kP();
tune_pitch_rp = g.pid_rate_pitch.kP();
tune_pitch_rd = g.pid_rate_pitch.kD();
tune_pitch_sp = g.pi_stabilize_pitch.kP();
}else{
// restarting from suspended state
auto_tune_restore_orig_pids();
}
// set roll-pitch mode to our special auto tuning stabilize roll-pitch mode
if (set_roll_pitch_mode(ROLL_PITCH_AUTOTUNE)) {
auto_tune_state.enabled = true;
Log_Write_Event(DATA_AUTOTUNE_ON);
}
}
}
// turn off tuning and return to standard pids
void auto_tune_stop()
{
if (auto_tune_state.enabled) {
auto_tune_state.enabled = false;
auto_tune_state.active = false;
if (roll_pitch_mode == ROLL_PITCH_AUTOTUNE) {
set_roll_pitch_mode(ROLL_PITCH_STABLE); // restore roll-pitch mode
rate_targets_frame = EARTH_FRAME; // regular stabilize mode frame
}
// restore pids to their original values
auto_tune_restore_orig_pids();
Log_Write_Event(DATA_AUTOTUNE_OFF);
}
}
// stop tuning but remain with tuned pids
void auto_tune_suspend()
{
auto_tune_load_tuned_pids();
if (auto_tune_state.active) {
auto_tune_state.active = false;
Log_Write_Event(DATA_AUTOTUNE_SUSPENDED);
}
}
// save discovered gains to eeprom if auto tuner is enabled (i.e. switch is in middle or high position)
void auto_tune_save_tuning_gains()
{
if (auto_tune_state.enabled) {
auto_tune_load_tuned_pids();
g.pid_rate_roll.save_gains();
g.pid_rate_pitch.save_gains();
g.pi_stabilize_roll.save_gains();
g.pi_stabilize_pitch.save_gains();
Log_Write_Event(DATA_AUTOTUNE_SAVEDGAINS);
}
}
// Auto tuning roll-pitch controller
static void
get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch_angle)
{
int32_t target_roll_rate, target_pitch_rate;
float rotation_rate; // rotation rate in radians/second
int32_t lean_angle;
// exit immediately if not enabled or not actively tuning
if (!auto_tune_state.enabled || !auto_tune_state.active) {
return;
}
// check for pilot override
if (pilot_roll_angle != 0 || pilot_pitch_angle != 0 ) {
if (!auto_tune_state.pilot_override) {
// restore pids to their original values
auto_tune_restore_orig_pids();
}
auto_tune_state.pilot_override = true;
auto_tune_override_time = millis();
}else if (auto_tune_state.pilot_override) {
// check if we should resume tuning after pilot's override
if (millis() - auto_tune_override_time > AUTO_TUNE_PILOT_OVERRIDE_TIMEOUT_MS) {
auto_tune_state.pilot_override = false; // turn off pilot override
auto_tune_state.step = AUTO_TUNE_STEP_WAITING_FOR_LEVEL; // set tuning step back from beginning
auto_tune_timer = millis();
}
}
// check tuning step
if (!auto_tune_state.pilot_override) {
switch (auto_tune_state.step) {
case AUTO_TUNE_STEP_WAITING_FOR_LEVEL:
// reset counter if we are no longer level
if ((labs(ahrs.roll_sensor) > AUTO_TUNE_LEVEL_ANGLE_CD) || (labs(ahrs.pitch_sensor) > AUTO_TUNE_LEVEL_ANGLE_CD)) {
auto_tune_timer = millis();
}
// if we have been level for a sufficient amount of time (0.5 seconds) move onto next step
if (millis() - auto_tune_timer >= AUTO_TUNE_REQUIRED_LEVEL_TIME_MS) {
auto_tune_state.step = AUTO_TUNE_STEP_TESTING;
// init variables for next step
auto_tune_test_max = 0;
auto_tune_test_min = 0;
rotation_rate = 0;
auto_tune_timer = millis();
// initialise rate controller targets
acro_roll_rate = roll_rate_target_bf;
acro_pitch_rate = pitch_rate_target_bf;
acro_yaw_rate = yaw_rate_target_bf;
// restore pids to their tuning values
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
g.pid_rate_roll.kP(tune_roll_rp);
g.pid_rate_roll.kI(tune_roll_rp*0.01f);
g.pid_rate_roll.kD(tune_roll_rd);
g.pi_stabilize_roll.kP(tune_roll_sp);
}else{
g.pid_rate_pitch.kP(tune_pitch_rp);
g.pid_rate_pitch.kI(tune_pitch_rp*0.01f);
g.pid_rate_pitch.kD(tune_pitch_rd);
g.pi_stabilize_pitch.kP(tune_pitch_sp);
}
}
break;
case AUTO_TUNE_STEP_TESTING:
// Run the test
// update rotation targets in body-earth frame
if(auto_tune_state.tune_type == AUTO_TUNE_TYPE_SP_UP){
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
// override roll angle
if (auto_tune_state.positive_direction) {
control_roll = AUTO_TUNE_TARGET_ANGLE_CD;
}else{
control_roll = -AUTO_TUNE_TARGET_ANGLE_CD;
}
get_stabilize_roll(control_roll);
}else{
// override pitch angle
if (auto_tune_state.positive_direction) {
control_pitch = AUTO_TUNE_TARGET_ANGLE_CD;
}else{
control_pitch = -AUTO_TUNE_TARGET_ANGLE_CD;
}
get_stabilize_pitch(control_pitch);
}
} else {
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
// override roll rate
if (auto_tune_state.positive_direction) {
target_roll_rate = AUTO_TUNE_TARGET_RATE_CDS;
}else{
target_roll_rate = -AUTO_TUNE_TARGET_RATE_CDS;
}
// set body frame targets for rate controller
set_roll_rate_target(target_roll_rate, BODY_FRAME);
set_pitch_rate_target(acro_pitch_rate, BODY_FRAME);
set_yaw_rate_target(acro_yaw_rate, BODY_FRAME);
}else{
// override pitch rate
if (auto_tune_state.positive_direction) {
target_pitch_rate = AUTO_TUNE_TARGET_RATE_CDS;
}else{
target_pitch_rate = -AUTO_TUNE_TARGET_RATE_CDS;
}
// set body frame targets for rate controller
set_pitch_rate_target(target_pitch_rate, BODY_FRAME);
set_roll_rate_target(acro_roll_rate, BODY_FRAME);
set_yaw_rate_target(acro_yaw_rate, BODY_FRAME);
}
rate_targets_frame = BODY_EARTH_FRAME;
}
// Get Rate and Angle
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
// 20 Hz filter on rate
rotation_rate = ToDeg(fabs(ahrs.get_gyro().x)) * 100.0f;
lean_angle = labs(ahrs.roll_sensor);
}else{
// 20 Hz filter on rate
// rotation_rate = rotation_rate + 0.55686f*(ToDeg(fabs(ahrs.get_gyro().y))*100.0f-rotation_rate);
rotation_rate = ToDeg(fabs(ahrs.get_gyro().y)) * 100.0f;
lean_angle = labs(ahrs.pitch_sensor);
}
// Make measurements
if(auto_tune_state.tune_type == AUTO_TUNE_TYPE_SP_UP){
// capture max angle
if (lean_angle > auto_tune_test_max) {
auto_tune_test_max = lean_angle;
auto_tune_test_min = lean_angle;
}
// capture min rotation rate
if (lean_angle < auto_tune_test_min && auto_tune_test_max > AUTO_TUNE_TARGET_ANGLE_CD*(1-AUTO_TUNE_AGGRESSIVENESS)) {
auto_tune_test_min = lean_angle;
}
}else{
// capture max rotation rate
if (rotation_rate > auto_tune_test_max) {
auto_tune_test_max = rotation_rate;
auto_tune_test_min = rotation_rate;
}
// capture min rotation rate
if (rotation_rate < auto_tune_test_min && auto_tune_test_max > AUTO_TUNE_TARGET_RATE_CDS*(1-2*AUTO_TUNE_AGGRESSIVENESS)) {
auto_tune_test_min = rotation_rate;
}
}
// check for end of test conditions
if(millis() - auto_tune_timer >= AUTO_TUNE_TARGET_RATE_TIMEOUT_MS) {
auto_tune_state.step = AUTO_TUNE_STEP_UPDATE_GAINS;
}
if(auto_tune_state.tune_type == AUTO_TUNE_TYPE_SP_UP){
if ((lean_angle >= AUTO_TUNE_TARGET_ANGLE_CD*(1+AUTO_TUNE_AGGRESSIVENESS)) ||
(auto_tune_test_max-auto_tune_test_min > AUTO_TUNE_TARGET_ANGLE_CD*AUTO_TUNE_AGGRESSIVENESS)) {
auto_tune_state.step = AUTO_TUNE_STEP_UPDATE_GAINS;
}
}else{
if (lean_angle >= AUTO_TUNE_TARGET_ANGLE_CD) {
auto_tune_state.step = AUTO_TUNE_STEP_UPDATE_GAINS;
}
if (auto_tune_state.tune_type == AUTO_TUNE_TYPE_RD_UP || auto_tune_state.tune_type == AUTO_TUNE_TYPE_RD_DOWN) {
if(auto_tune_test_max-auto_tune_test_min > AUTO_TUNE_TARGET_RATE_CDS*AUTO_TUNE_AGGRESSIVENESS) {
auto_tune_state.step = AUTO_TUNE_STEP_UPDATE_GAINS;
}
}
}
// logging
Log_Write_AutoTuneDetails((int16_t)lean_angle, rotation_rate);
break;
case AUTO_TUNE_STEP_UPDATE_GAINS:
// restore pids to their original values
//auto_tune_restore_orig_pids();
g.pid_rate_roll.kP(orig_roll_rp);
g.pid_rate_roll.kI(orig_roll_rp*AUTO_TUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_roll.kD(orig_roll_rd);
g.pi_stabilize_roll.kP(orig_roll_sp);
g.pid_rate_pitch.kP(orig_pitch_rp);
g.pid_rate_pitch.kI(orig_pitch_rp*AUTO_TUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_pitch.kD(orig_pitch_rd);
g.pi_stabilize_pitch.kP(orig_pitch_sp);
// logging
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
Log_Write_AutoTune(auto_tune_state.axis, auto_tune_state.tune_type, auto_tune_test_min, auto_tune_test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp);
}else{
Log_Write_AutoTune(auto_tune_state.axis, auto_tune_state.tune_type, auto_tune_test_min, auto_tune_test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp);
}
// do gain updates
if (auto_tune_state.tune_type == AUTO_TUNE_TYPE_RD_UP) {
if (auto_tune_test_max > AUTO_TUNE_TARGET_RATE_CDS &&
((auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL && tune_roll_rp >= AUTO_TUNE_RP_MIN) ||
(auto_tune_state.axis == AUTO_TUNE_AXIS_PITCH && tune_pitch_rp >= AUTO_TUNE_RP_MIN)) ) {
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rp -= AUTO_TUNE_RP_STEP;
}else{
tune_pitch_rp -= AUTO_TUNE_RP_STEP;
}
}else if(auto_tune_test_max < AUTO_TUNE_TARGET_RATE_CDS*(1.0f-AUTO_TUNE_AGGRESSIVENESS*2.0f) &&
((auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL && tune_roll_rp <= AUTO_TUNE_RP_MAX) ||
(auto_tune_state.axis == AUTO_TUNE_AXIS_PITCH && tune_pitch_rp <= AUTO_TUNE_RP_MAX)) ) {
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rp += AUTO_TUNE_RP_STEP*2.0f;
}else{
tune_pitch_rp += AUTO_TUNE_RP_STEP*2.0f;
}
}else{
if (auto_tune_test_max-auto_tune_test_min > AUTO_TUNE_TARGET_RATE_CDS*AUTO_TUNE_AGGRESSIVENESS) {
auto_tune_counter++;
}else{
if (auto_tune_counter > 0 ) {
auto_tune_counter--;
}
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rd += AUTO_TUNE_RD_STEP*2.0f;
// stop tuning if we hit max D
if (tune_roll_rd >= AUTO_TUNE_RD_MAX) {
tune_roll_rd = AUTO_TUNE_RD_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
}
}else{
tune_pitch_rd += AUTO_TUNE_RD_STEP*2.0f;
// stop tuning if we hit max D
if (tune_pitch_rd >= AUTO_TUNE_RD_MAX) {
tune_pitch_rd = AUTO_TUNE_RD_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
}
}
}
}
} else if (auto_tune_state.tune_type == AUTO_TUNE_TYPE_RD_DOWN) {
if (auto_tune_test_max > AUTO_TUNE_TARGET_RATE_CDS &&
((auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL && tune_roll_rp >= AUTO_TUNE_RP_MIN) ||
(auto_tune_state.axis == AUTO_TUNE_AXIS_PITCH && tune_pitch_rp >= AUTO_TUNE_RP_MIN)) ) {
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rp -= AUTO_TUNE_RP_STEP;
}else{
tune_pitch_rp -= AUTO_TUNE_RP_STEP;
}
}else if(auto_tune_test_max < AUTO_TUNE_TARGET_RATE_CDS*(1-AUTO_TUNE_AGGRESSIVENESS*2.0f) &&
((auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL && tune_roll_rp <= AUTO_TUNE_RP_MAX) ||
(auto_tune_state.axis == AUTO_TUNE_AXIS_PITCH && tune_pitch_rp <= AUTO_TUNE_RP_MAX)) ) {
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rp += AUTO_TUNE_RP_STEP;
}else{
tune_pitch_rp += AUTO_TUNE_RP_STEP;
}
}else{
if (auto_tune_test_max-auto_tune_test_min < AUTO_TUNE_TARGET_RATE_CDS*AUTO_TUNE_AGGRESSIVENESS) {
auto_tune_counter++;
}else{
if (auto_tune_counter > 0 ) {
auto_tune_counter--;
}
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rd -= AUTO_TUNE_RD_STEP;
// stop tuning if we hit max D
if (tune_roll_rd <= AUTO_TUNE_RD_MIN) {
tune_roll_rd = AUTO_TUNE_RD_MIN;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
}
}else{
tune_pitch_rd -= AUTO_TUNE_RD_STEP;
// stop tuning if we hit max D
if (tune_pitch_rd <= AUTO_TUNE_RD_MIN) {
tune_pitch_rd = AUTO_TUNE_RD_MIN;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
}
}
}
}
} else if (auto_tune_state.tune_type == AUTO_TUNE_TYPE_RP_UP) {
if (auto_tune_test_max > AUTO_TUNE_TARGET_RATE_CDS) {
auto_tune_counter++;
}else{
if (auto_tune_counter > 0 ) {
auto_tune_counter--;
}
// increase P & I or D term
// update PI term
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rp += AUTO_TUNE_RP_STEP;
// stop tuning if we hit max P
if (tune_roll_rp >= AUTO_TUNE_RP_MAX) {
tune_roll_rp = AUTO_TUNE_RP_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
}
}else{
tune_pitch_rp += AUTO_TUNE_RP_STEP;
// stop tuning if we hit max P
if (tune_pitch_rp >= AUTO_TUNE_RP_MAX) {
tune_pitch_rp = AUTO_TUNE_RP_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
}
}
}
} else if (auto_tune_state.tune_type == AUTO_TUNE_TYPE_SP_UP) {
if (auto_tune_test_max > AUTO_TUNE_TARGET_ANGLE_CD*(1+AUTO_TUNE_AGGRESSIVENESS) ||
(auto_tune_test_max-auto_tune_test_min > AUTO_TUNE_TARGET_ANGLE_CD*AUTO_TUNE_AGGRESSIVENESS)) {
auto_tune_counter++;
}else{
if (auto_tune_counter > 0 ) {
auto_tune_counter--;
}
// increase P & I or D term
// update PI term
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_sp += AUTO_TUNE_SP_STEP;
// stop tuning if we hit max P
if (tune_roll_sp >= AUTO_TUNE_SP_MAX) {
tune_roll_sp = AUTO_TUNE_SP_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
}
}else{
tune_pitch_sp += AUTO_TUNE_SP_STEP;
// stop tuning if we hit max P
if (tune_pitch_sp >= AUTO_TUNE_SP_MAX) {
tune_pitch_sp = AUTO_TUNE_SP_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
}
}
}
}
// reverse direction
auto_tune_state.positive_direction = !auto_tune_state.positive_direction;
// we've complete this step, finalise pids and move to next step
if (auto_tune_counter >= AUTO_TUNE_SUCCESS_COUNT) {
// reset counter
auto_tune_counter = 0;
// move to the next tuning type
if (auto_tune_state.tune_type < AUTO_TUNE_TYPE_SP_UP) {
auto_tune_state.tune_type++;
}else{
// we've reached the end of a D-up-down PI-up-down tune type cycle
auto_tune_state.tune_type = AUTO_TUNE_TYPE_RD_UP;
// if we've just completed roll move onto pitch
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_sp = tune_roll_sp * AUTO_TUNE_SP_BACKOFF;
auto_tune_state.axis = AUTO_TUNE_AXIS_PITCH;
}else{
tune_pitch_sp = tune_pitch_sp * AUTO_TUNE_SP_BACKOFF;
tune_roll_sp = min(tune_roll_sp, tune_pitch_sp);
tune_pitch_sp = min(tune_roll_sp, tune_pitch_sp);
// if we've just completed pitch we are done so suspend tuning
auto_tune_suspend();
}
}
}
// reset testing step
auto_tune_state.step = AUTO_TUNE_STEP_WAITING_FOR_LEVEL;
auto_tune_timer = millis();
break;
}
}
}

18
ArduCopter/control_modes.pde
View File

@ -282,6 +282,24 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
set_mode(AUTO); set_mode(AUTO);
} }
break; break;
case AUX_SWITCH_AUTOTUNE:
// turn on auto tuner
switch(ch_flag) {
case AUX_SWITCH_LOW:
// turn off tuning and return to standard pids
auto_tune_stop();
break;
case AUX_SWITCH_MIDDLE:
// stop tuning but remain with tuned pids
auto_tune_suspend();
break;
case AUX_SWITCH_HIGH:
// start an auto tuning session
auto_tune_start();
break;
}
break;
} }
} }

8
ArduCopter/defines.h
View File

@ -33,6 +33,7 @@
#define ROLL_PITCH_TOY 4 // THOR This is the Roll and Pitch mode #define ROLL_PITCH_TOY 4 // THOR This is the Roll and Pitch mode
#define ROLL_PITCH_LOITER 5 // pilot inputs the desired horizontal velocities #define ROLL_PITCH_LOITER 5 // pilot inputs the desired horizontal velocities
#define ROLL_PITCH_SPORT 6 // pilot inputs roll, pitch rotation rates in earth frame #define ROLL_PITCH_SPORT 6 // pilot inputs roll, pitch rotation rates in earth frame
#define ROLL_PITCH_AUTOTUNE 7 // description of new roll-pitch mode
#define THROTTLE_MANUAL 0 // manual throttle mode - pilot input goes directly to motors #define THROTTLE_MANUAL 0 // manual throttle mode - pilot input goes directly to motors
#define THROTTLE_MANUAL_TILT_COMPENSATED 1 // mostly manual throttle but with some tilt compensation #define THROTTLE_MANUAL_TILT_COMPENSATED 1 // mostly manual throttle but with some tilt compensation
@ -68,6 +69,7 @@
#define AUX_SWITCH_ACRO_TRAINER 14 // low = disabled, middle = leveled, high = leveled and limited #define AUX_SWITCH_ACRO_TRAINER 14 // low = disabled, middle = leveled, high = leveled and limited
#define AUX_SWITCH_SPRAYER 15 // enable/disable the crop sprayer #define AUX_SWITCH_SPRAYER 15 // enable/disable the crop sprayer
#define AUX_SWITCH_AUTO 16 // change to auto flight mode #define AUX_SWITCH_AUTO 16 // change to auto flight mode
#define AUX_SWITCH_AUTOTUNE 17 // auto tune
// values used by the ap.ch7_opt and ap.ch8_opt flags // values used by the ap.ch7_opt and ap.ch8_opt flags
#define AUX_SWITCH_LOW 0 // indicates auxiliar switch is in the low position (pwm <1200) #define AUX_SWITCH_LOW 0 // indicates auxiliar switch is in the low position (pwm <1200)
@ -291,6 +293,8 @@ enum ap_message {
#define LOG_DATA_INT32_MSG 0x16 #define LOG_DATA_INT32_MSG 0x16
#define LOG_DATA_UINT32_MSG 0x17 #define LOG_DATA_UINT32_MSG 0x17
#define LOG_DATA_FLOAT_MSG 0x18 #define LOG_DATA_FLOAT_MSG 0x18
#define LOG_AUTOTUNE_MSG 0x19
#define LOG_AUTOTUNEDETAILS_MSG 0x1A
#define LOG_INDEX_MSG 0xF0 #define LOG_INDEX_MSG 0xF0
#define MAX_NUM_LOGS 50 #define MAX_NUM_LOGS 50
@ -332,6 +336,10 @@ enum ap_message {
#define DATA_SET_SIMPLE_ON 26 #define DATA_SET_SIMPLE_ON 26
#define DATA_SET_SIMPLE_OFF 27 #define DATA_SET_SIMPLE_OFF 27
#define DATA_SET_SUPERSIMPLE_ON 28 #define DATA_SET_SUPERSIMPLE_ON 28
#define DATA_AUTOTUNE_ON 29
#define DATA_AUTOTUNE_SUSPENDED 30
#define DATA_AUTOTUNE_OFF 31
#define DATA_AUTOTUNE_SAVEDGAINS 32
/* ************************************************************** */ /* ************************************************************** */
/* Expansion PIN's that people can use for various things. */ /* Expansion PIN's that people can use for various things. */

3
ArduCopter/motors.pde
View File

@ -398,6 +398,9 @@ static void init_disarm_motors()
g.throttle_cruise.save(); g.throttle_cruise.save();
// save auto tuned parameters
auto_tune_save_tuning_gains();
// we are not in the air // we are not in the air
set_takeoff_complete(false); set_takeoff_complete(false);