Browse Source

Copter: make AutoTune a compile-time option

master
Randy Mackay 12 years ago
parent
commit
ba92c4891a
  1. 4
      ArduCopter/ArduCopter.pde
  2. 6
      ArduCopter/Log.pde
  3. 4
      ArduCopter/auto_tune.pde
  4. 6
      ArduCopter/config.h
  5. 2
      ArduCopter/control_modes.pde
  6. 2
      ArduCopter/motors.pde

4
ArduCopter/ArduCopter.pde

@ -1685,10 +1685,12 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) @@ -1685,10 +1685,12 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
}
break;
#if AUTOTUNE == ENABLED
case ROLL_PITCH_AUTOTUNE:
// indicate we can enter this mode successfully
roll_pitch_initialised = true;
break;
#endif
}
// if initialisation has been successful update the yaw mode
@ -1808,6 +1810,7 @@ void update_roll_pitch_mode(void) @@ -1808,6 +1810,7 @@ void update_roll_pitch_mode(void)
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_system.new_radio_frame) {
@ -1824,6 +1827,7 @@ void update_roll_pitch_mode(void) @@ -1824,6 +1827,7 @@ void update_roll_pitch_mode(void)
// copy user input for reporting purposes
get_autotune_roll_pitch_controller(g.rc_1.control_in, g.rc_2.control_in);
break;
#endif
}
#if FRAME_CONFIG != HELI_FRAME

6
ArduCopter/Log.pde

@ -160,6 +160,7 @@ process_logs(uint8_t argc, const Menu::arg *argv) @@ -160,6 +160,7 @@ process_logs(uint8_t argc, const Menu::arg *argv)
return 0;
}
#if AUTOTUNE == ENABLED
struct PACKED log_AutoTune {
LOG_PACKET_HEADER;
uint8_t axis; // roll or pitch
@ -203,6 +204,7 @@ static void Log_Write_AutoTuneDetails(int16_t angle_cd, float rate_cds) @@ -203,6 +204,7 @@ static void Log_Write_AutoTuneDetails(int16_t angle_cd, float rate_cds)
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
#endif
struct PACKED log_Current {
LOG_PACKET_HEADER;
@ -758,10 +760,12 @@ static void Log_Write_Error(uint8_t sub_system, uint8_t error_code) @@ -758,10 +760,12 @@ static void Log_Write_Error(uint8_t sub_system, uint8_t error_code)
static const struct LogStructure log_structure[] PROGMEM = {
LOG_COMMON_STRUCTURES,
#if AUTOTUNE == ENABLED
{ 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" },
#endif
{ LOG_CURRENT_MSG, sizeof(log_Current),
"CURR", "hIhhhf", "Thr,ThrInt,Volt,Curr,Vcc,CurrTot" },
@ -855,8 +859,10 @@ static void Log_Write_Cmd(uint8_t num, const struct Location *wp) {} @@ -855,8 +859,10 @@ 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
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
static void Log_Write_Current() {}
static void Log_Write_Compass() {}
static void Log_Write_Attitude() {}

4
ArduCopter/auto_tune.pde

@ -1,5 +1,6 @@ @@ -1,5 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if AUTOTUNE == ENABLED
/*
Auto tuning works in this way:
i) set up 3-position ch7 or ch8 switch to "AutoTune"
@ -563,4 +564,5 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch @@ -563,4 +564,5 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch
break;
}
}
}
}
#endif // AUTOTUNE == ENABLED

6
ArduCopter/config.h

@ -447,6 +447,12 @@ @@ -447,6 +447,12 @@
#define OPTFLOW_IMAX 100
#endif
//////////////////////////////////////////////////////////////////////////////
// Auto Tuning
#ifndef AUTOTUNE
# define AUTOTUNE DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Crop Sprayer
#ifndef SPRAYER

2
ArduCopter/control_modes.pde

@ -283,6 +283,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) @@ -283,6 +283,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
}
break;
#if AUTOTUNE == ENABLED
case AUX_SWITCH_AUTOTUNE:
// turn on auto tuner
switch(ch_flag) {
@ -300,6 +301,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) @@ -300,6 +301,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
break;
}
break;
#endif
}
}

2
ArduCopter/motors.pde

@ -398,8 +398,10 @@ static void init_disarm_motors() @@ -398,8 +398,10 @@ static void init_disarm_motors()
g.throttle_cruise.save();
#if AUTOTUNE == ENABLED
// save auto tuned parameters
auto_tune_save_tuning_gains();
#endif
// we are not in the air
set_takeoff_complete(false);

Loading…
Cancel
Save