aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: integrate control_autotune

Browse Source
This commit is contained in:
Randy Mackay 2014-02-02 16:58:36 +09:00 committed by Andrew Tridgell
parent 823485f763
commit aec67cd3df
7 changed files with 50 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ArduCopter/ArduCopter.pde
View File

@ -1397,18 +1397,8 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
case ROLL_PITCH_SPORT:
roll_pitch_initialised = true;
break;
#if AUTOTUNE == ENABLED
case ROLL_PITCH_AUTOTUNE:
// only enter autotune mode from stabilized roll-pitch mode when armed and flying
if (roll_pitch_mode == ROLL_PITCH_STABLE && motors.armed() && !ap.land_complete) {
reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in);
// auto_tune_start returns true if it wants the roll-pitch mode changed to autotune
roll_pitch_initialised = auto_tune_start();
}
break;
#endif
}
// if initialisation has been successful update the yaw mode
if( roll_pitch_initialised ) {
@ -1423,11 +1413,6 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
// exit_roll_pitch_mode - peforms any code required when exiting the current roll-pitch mode
void exit_roll_pitch_mode(uint8_t old_roll_pitch_mode)
{
#if AUTOTUNE == ENABLED
if (old_roll_pitch_mode == ROLL_PITCH_AUTOTUNE) {
auto_tune_stop();
}
#endif
}
// update_flight_mode - calls the appropriate attitude controllers based on flight mode
@ -1494,6 +1479,12 @@ static void update_flight_mode()
case FLIP:
flip_run();
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
autotune_run();
break;
#endif
}
}
@ -1579,27 +1570,8 @@ void update_roll_pitch_mode(void)
get_roll_rate_stabilized_ef(g.rc_1.control_in);
get_pitch_rate_stabilized_ef(g.rc_2.control_in);
break;
#if AUTOTUNE == ENABLED
case ROLL_PITCH_AUTOTUNE:
// apply SIMPLE mode transform
if(ap.simple_mode && ap.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, g.rc_4.control_in);
break;
#endif
}
#if FRAME_CONFIG != HELI_FRAME
if(g.rc_3.control_in == 0 && control_mode <= ACRO) {
reset_rate_I();

4
ArduCopter/Log.pde
View File

@ -157,7 +157,7 @@ process_logs(uint8_t argc, const Menu::arg *argv)
return 0;
}
#if AUTOTUNE == ENABLED
#if AUTOTUNE_ENABLED == ENABLED
struct PACKED log_AutoTune {
LOG_PACKET_HEADER;
uint8_t axis; // roll or pitch
@ -774,7 +774,7 @@ static void Log_Write_Cmd(uint8_t num, const struct Location *wp) {}
static void Log_Write_Mode(uint8_t mode) {}
static void Log_Write_IMU() {}
static void Log_Write_GPS() {}
#if AUTOTUNE == ENABLED
#if AUTOTUNE_ENABLED == ENABLED
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) {}
#endif

6
ArduCopter/config.h
View File

@ -130,7 +130,7 @@
# define HELI_STAB_COLLECTIVE_MIN_DEFAULT 0
# define HELI_STAB_COLLECTIVE_MAX_DEFAULT 1000
# define THR_MIN_DEFAULT 0
# define AUTOTUNE DISABLED
# define AUTOTUNE_ENABLED DISABLED
# ifndef HELI_CC_COMP
#define HELI_CC_COMP DISABLED
@ -411,8 +411,8 @@
//////////////////////////////////////////////////////////////////////////////
// Auto Tuning
#ifndef AUTOTUNE
# define AUTOTUNE ENABLED
#ifndef AUTOTUNE_ENABLED
# define AUTOTUNE_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////

15
ArduCopter/control_modes.pde
View File

@ -340,21 +340,22 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
}
break;
#if AUTOTUNE == ENABLED
#if AUTOTUNE_ENABLED == ENABLED
case AUX_SWITCH_AUTOTUNE:
// turn on auto tuner
switch(ch_flag) {
case AUX_SWITCH_LOW:
case AUX_SWITCH_MIDDLE:
// turn off tuning and return to standard pids
if (roll_pitch_mode == ROLL_PITCH_AUTOTUNE) {
set_roll_pitch_mode(ROLL_PITCH_STABLE);
// stop the autotune and return to original gains
autotune_stop();
// restore flight mode based on flight mode switch position
if (control_mode == AUTOTUNE) {
reset_control_switch();
}
break;
case AUX_SWITCH_HIGH:
// start an auto tuning session
// set roll-pitch mode to our special auto tuning stabilize roll-pitch mode
set_roll_pitch_mode(ROLL_PITCH_AUTOTUNE);
// start an autotuning session
autotune_start();
break;
}
break;

12
ArduCopter/defines.h
View File

@ -128,7 +128,8 @@
#define DRIFT 11 // DRIFT mode (Note: 12 is no longer used)
#define SPORT 13 // earth frame rate control
#define FLIP 14 // flip the vehicle on the roll axis
#define NUM_MODES 15
#define AUTOTUNE 15 // autotune the vehicle's roll and pitch gains
#define NUM_MODES 16
// CH_6 Tuning
@ -313,10 +314,10 @@ enum FlipState {
#define DATA_AUTOTUNE_INITIALISED 30
#define DATA_AUTOTUNE_OFF 31
#define DATA_AUTOTUNE_RESTART 32
#define DATA_AUTOTUNE_COMPLETE 33
#define DATA_AUTOTUNE_ABANDONED 34
#define DATA_AUTOTUNE_SUCCESS 33
#define DATA_AUTOTUNE_FAILED 34
#define DATA_AUTOTUNE_REACHED_LIMIT 35
#define DATA_AUTOTUNE_TESTING 36
#define DATA_AUTOTUNE_PILOT_TESTING 36
#define DATA_AUTOTUNE_SAVEDGAINS 37
#define DATA_SAVE_TRIM 38
#define DATA_SAVEWP_ADD_WP 39
@ -383,6 +384,7 @@ enum FlipState {
#define ERROR_SUBSYSTEM_GPS 11
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
#define ERROR_SUBSYSTEM_FLIP 13
#define ERROR_SUBSYSTEM_AUTOTUNE 14
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1
@ -401,6 +403,8 @@ enum FlipState {
#define ERROR_CODE_CRASH_CHECK_CRASH 1
// subsystem specific error codes -- flip
#define ERROR_CODE_FLIP_ABANDONED 2
// subsystem specific error codes -- autotune
#define ERROR_CODE_AUTOTUNE_BAD_GAINS 2
// Arming Check Enable/Disable bits
#define ARMING_CHECK_NONE 0x00

4
ArduCopter/motors.pde
View File

@ -514,9 +514,9 @@ static void init_disarm_motors()
g.throttle_cruise.save();
#if AUTOTUNE == ENABLED
#if AUTOTUNE_ENABLED == ENABLED
// save auto tuned parameters
auto_tune_save_tuning_gains_and_reset();
autotune_save_tuning_gains();
#endif
// we are not in the air

21
ArduCopter/system.pde
View File

@ -450,6 +450,12 @@ static bool set_mode(uint8_t mode)
success = flip_init(ignore_checks);
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
success = autotune_init(ignore_checks);
break;
#endif
default:
success = false;
break;
@ -457,6 +463,8 @@ static bool set_mode(uint8_t mode)
// update flight mode
if (success) {
// perform any cleanup required by previous flight mode
exit_mode(control_mode);
control_mode = mode;
Log_Write_Mode(control_mode);
}else{
@ -468,6 +476,16 @@ static bool set_mode(uint8_t mode)
return success;
}
// exit_mode - high level call to organise cleanup as a flight mode is exited
static void exit_mode(uint8_t old_control_mode)
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_control_mode == AUTOTUNE) {
autotune_stop();
}
#endif
}
// update_auto_armed - update status of auto_armed flag
static void update_auto_armed()
{
@ -588,6 +606,9 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case FLIP:
port->print_P(PSTR("FLIP"));
break;
case AUTOTUNE:
port->print_P(PSTR("AUTOTUNE"));
break;
default:
port->printf_P(PSTR("Mode(%u)"), (unsigned)mode);
break;