From 1bce4239a33bc8f00d512e9951161bfffed04f18 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 12 Apr 2014 14:12:14 +1000 Subject: [PATCH] Plane: added AUTOTUNE flight mode this is just like FBWA, but with automatic roll/pitch tuning --- ArduPlane/ArduPlane.pde | 2 ++ ArduPlane/Attitude.pde | 5 ++++- ArduPlane/GCS_Mavlink.pde | 3 +++ ArduPlane/Parameters.pde | 12 +++++------ ArduPlane/control_modes.pde | 17 +++++++++++++++ ArduPlane/defines.h | 1 + ArduPlane/events.pde | 2 ++ ArduPlane/setup.pde | 1 + ArduPlane/system.pde | 41 ++++++++++++++++++++++++------------- 9 files changed, 63 insertions(+), 21 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index d8535a16c0..686324a44a 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -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 diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 8b91de823b..0233a19e72 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -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; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 3e1491c4d2..4681347146 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -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: diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 96d78fb72b..3c8231851e 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -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), diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde index 61251761b9..d26a544bc9 100644 --- a/ArduPlane/control_modes.pde +++ b/ArduPlane/control_modes.pde @@ -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(); +} diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index c3619885fb..ca9ebd2144 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -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, diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index 2027fa9433..2179fa04a7 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -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: diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index f6f1242ecc..c3453a2a9c 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -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 && diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index f0c635fa54..12a771c17d 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -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;