Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: added AUTOTUNE flight mode 

this is just like FBWA, but with automatic roll/pitch tuning
This commit is contained in:
Andrew Tridgell 2014-04-12 14:12:14 +10:00
parent 50fc75917e
commit 1bce4239a3
9 changed files with 63 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.pde
View File

@ -1230,6 +1230,7 @@ static void update_flight_mode(void)
break;
}
case AUTOTUNE:
case FLY_BY_WIRE_A: {
// set nav_roll and nav_pitch using sticks
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
@ -1342,6 +1343,7 @@ static void update_navigation()
case INITIALISING:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CIRCLE:
// nothing to do

5
ArduPlane/Attitude.pde
View File

@ -130,6 +130,7 @@ static void stabilize_stick_mixing_direct()
if (!stick_mixing_enabled() ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == TRAINING) {
@ -148,6 +149,7 @@ static void stabilize_stick_mixing_fbw()
if (!stick_mixing_enabled() ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == TRAINING ||
@ -842,7 +844,8 @@ static void set_servos(void)
(control_mode == STABILIZE ||
control_mode == TRAINING ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A)) {
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE)) {
// manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set
channel_throttle->radio_out = channel_throttle->radio_in;

3
ArduPlane/GCS_Mavlink.pde
View File

@ -51,6 +51,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
break;
case STABILIZE:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
@ -174,6 +175,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
case STABILIZE:
case FLY_BY_WIRE_A:
case AUTOTUNE:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
break;
@ -1339,6 +1341,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case TRAINING:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
case AUTO:

12
ArduPlane/Parameters.pde
View File

@ -471,7 +471,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: FLTMODE1
// @DisplayName: FlightMode1
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
// @Description: Flight mode for switch position 1 (910 to 1230 and above 2049)
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
@ -479,35 +479,35 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: FLTMODE2
// @DisplayName: FlightMode2
// @Description: Flight mode for switch position 2 (1231 to 1360)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
// @Param: FLTMODE3
// @DisplayName: FlightMode3
// @Description: Flight mode for switch position 3 (1361 to 1490)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
// @Param: FLTMODE4
// @DisplayName: FlightMode4
// @Description: Flight mode for switch position 4 (1491 to 1620)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
// @Param: FLTMODE5
// @DisplayName: FlightMode5
// @Description: Flight mode for switch position 5 (1621 to 1749)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
// @Param: FLTMODE6
// @DisplayName: FlightMode6
// @Description: Flight mode for switch position 6 (1750 to 2049)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
// @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),

17
ArduPlane/control_modes.pde
View File

@ -79,3 +79,20 @@ static void reset_control_switch()
read_control_switch();
}
/*
called when entering autotune
*/
static void autotune_start(void)
{
rollController.autotune_start();
pitchController.autotune_start();
}
/*
called when exiting autotune
*/
static void autotune_restore(void)
{
rollController.autotune_restore();
pitchController.autotune_restore();
}

1
ArduPlane/defines.h
View File

@ -62,6 +62,7 @@ enum FlightMode {
FLY_BY_WIRE_A = 5,
FLY_BY_WIRE_B = 6,
CRUISE = 7,
AUTOTUNE = 8,
AUTO = 10,
RTL = 11,
LOITER = 12,

2
ArduPlane/events.pde
View File

@ -13,6 +13,7 @@ static void failsafe_short_on_event(enum failsafe_state fstype)
case STABILIZE:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
case TRAINING:
@ -60,6 +61,7 @@ static void failsafe_long_on_event(enum failsafe_state fstype)
case STABILIZE:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
case TRAINING:

1
ArduPlane/setup.pde
View File

@ -310,6 +310,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
mode != TRAINING &&
mode != ACRO &&
mode != FLY_BY_WIRE_A &&
mode != AUTOTUNE &&
mode != FLY_BY_WIRE_B &&
mode != CRUISE &&
mode != AUTO &&

41
ArduPlane/system.pde
View File

@ -292,36 +292,55 @@ static void set_mode(enum FlightMode mode)
previous_mode = control_mode;
control_mode = mode;
if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
// restore last gains
autotune_restore();
}
switch(control_mode)
{
case INITIALISING:
auto_throttle_mode = true;
break;
case MANUAL:
case STABILIZE:
case TRAINING:
case FLY_BY_WIRE_A:
auto_throttle_mode = false;
break;
case AUTOTUNE:
auto_throttle_mode = false;
autotune_start();
break;
case ACRO:
auto_throttle_mode = false;
acro_state.locked_roll = false;
acro_state.locked_pitch = false;
break;
case CRUISE:
auto_throttle_mode = true;
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
target_altitude_cm = current_loc.alt;
break;
case FLY_BY_WIRE_B:
auto_throttle_mode = true;
target_altitude_cm = current_loc.alt;
break;
case CIRCLE:
// the altitude to circle at is taken from the current altitude
auto_throttle_mode = true;
next_WP_loc.alt = current_loc.alt;
break;
case AUTO:
auto_throttle_mode = true;
prev_WP_loc = current_loc;
// start the mission. Note that we use resume(), not start(),
// as the correct behaviour for plane when entering auto is to
@ -332,34 +351,25 @@ static void set_mode(enum FlightMode mode)
break;
case RTL:
auto_throttle_mode = true;
prev_WP_loc = current_loc;
do_RTL();
break;
case LOITER:
auto_throttle_mode = true;
do_loiter_at_location();
break;
case GUIDED:
auto_throttle_mode = true;
guided_throttle_passthru = false;
set_guided_WP();
break;
default:
prev_WP_loc = current_loc;
do_RTL();
break;
}
// if in an auto-throttle mode, start with throttle suppressed for
// safety. suppress_throttle() will unsupress it when appropriate
if (control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) {
auto_throttle_mode = true;
throttle_suppressed = true;
} else {
auto_throttle_mode = false;
throttle_suppressed = false;
}
// start with throttle suppressed in auto_throttle modes
throttle_suppressed = auto_throttle_mode;
if (should_log(MASK_LOG_MODE))
Log_Write_Mode(control_mode);
@ -565,6 +575,9 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case FLY_BY_WIRE_A:
port->print_P(PSTR("FBW_A"));
break;
case AUTOTUNE:
port->print_P(PSTR("AUTOTUNE"));
break;
case FLY_BY_WIRE_B:
port->print_P(PSTR("FBW_B"));
break;