zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: integrate control_autotune

mission-4.1.18
Randy Mackay 11 years ago committed by Andrew Tridgell
parent
823485f763
commit
aec67cd3df
7 changed files with 50 additions and 52 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 40
      ArduCopter/ArduCopter.pde
  2. 4
      ArduCopter/Log.pde
  3. 6
      ArduCopter/config.h
  4. 15
      ArduCopter/control_modes.pde
  5. 12
      ArduCopter/defines.h
  6. 4
      ArduCopter/motors.pde
  7. 21
      ArduCopter/system.pde

40
ArduCopter/ArduCopter.pde
Unescape View File

@ -1397,18 +1397,8 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) @@ -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) @@ -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() @@ -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) @@ -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
Unescape View File

@ -157,7 +157,7 @@ process_logs(uint8_t argc, const Menu::arg *argv) @@ -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) {} @@ -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
Unescape View File

@ -130,7 +130,7 @@ @@ -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 @@ @@ -411,8 +411,8 @@
//////////////////////////////////////////////////////////////////////////////
// Auto Tuning
#ifndef AUTOTUNE
# define AUTOTUNE ENABLED
#ifndef AUTOTUNE_ENABLED
# define AUTOTUNE_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////

15
ArduCopter/control_modes.pde
Unescape View File

@ -340,21 +340,22 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) @@ -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
Unescape View File

@ -128,7 +128,8 @@ @@ -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 { @@ -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 { @@ -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 { @@ -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
Unescape View File

@ -514,9 +514,9 @@ static void init_disarm_motors() @@ -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
Unescape View File

@ -450,6 +450,12 @@ static bool set_mode(uint8_t mode) @@ -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) @@ -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) @@ -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) @@ -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;

Write Preview
Loading…
Cancel
Save