From 608007361b12ddd87a29f1877df1399f6f24dcbe Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 10 Jul 2013 23:25:38 +1000 Subject: [PATCH] Plane: added initial implementation of ACRO mode rate based control with attitude locking --- ArduPlane/ArduPlane.pde | 26 ++++++++++++++++ ArduPlane/Attitude.pde | 63 +++++++++++++++++++++++++++++++++++++++ ArduPlane/GCS_Mavlink.pde | 6 ++++ ArduPlane/defines.h | 1 + ArduPlane/events.pde | 2 ++ ArduPlane/setup.pde | 1 + ArduPlane/system.pde | 4 +++ 7 files changed, 103 insertions(+) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index da9728ef16..67fcbec587 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -454,6 +454,16 @@ static struct { //////////////////////////////////////////////////////////////////////////////// AP_Airspeed airspeed; +//////////////////////////////////////////////////////////////////////////////// +// ACRO controller state +//////////////////////////////////////////////////////////////////////////////// +static struct { + bool locked_roll; + bool locked_pitch; + int32_t locked_roll_cd; + int32_t locked_pitch_cd; +} acro_state; + //////////////////////////////////////////////////////////////////////////////// // flight mode specific //////////////////////////////////////////////////////////////////////////////// @@ -1111,6 +1121,21 @@ static void update_flight_mode(void) break; } + case ACRO: { + // handle locked/unlocked control + if (acro_state.locked_roll) { + nav_roll_cd = acro_state.locked_roll_cd; + } else { + nav_roll_cd = ahrs.roll_sensor; + } + if (acro_state.locked_pitch) { + nav_pitch_cd = acro_state.locked_pitch_cd; + } else { + nav_pitch_cd = ahrs.pitch_sensor; + } + break; + } + case FLY_BY_WIRE_A: { // set nav_roll and nav_pitch using sticks nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd; @@ -1220,6 +1245,7 @@ static void update_navigation() case STABILIZE: case TRAINING: case INITIALISING: + case ACRO: case FLY_BY_WIRE_A: case FLY_BY_WIRE_B: case CIRCLE: diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index d92669bfff..818371f1f1 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -99,6 +99,7 @@ static void stabilize_pitch(float speed_scaler) static void stabilize_stick_mixing_direct() { if (!stick_mixing_enabled() || + control_mode == ACRO || control_mode == FLY_BY_WIRE_A || control_mode == FLY_BY_WIRE_B || control_mode == TRAINING) { @@ -136,6 +137,7 @@ static void stabilize_stick_mixing_direct() static void stabilize_stick_mixing_fbw() { if (!stick_mixing_enabled() || + control_mode == ACRO || control_mode == FLY_BY_WIRE_A || control_mode == FLY_BY_WIRE_B || control_mode == TRAINING) { @@ -228,6 +230,64 @@ static void stabilize_training(float speed_scaler) } +/* + this is the ACRO mode stabilization function. It does rate + stabilization on roll and pitch axes + */ +static void stabilize_acro(float speed_scaler) +{ + float roll_rate = channel_roll->norm_input() * 180; + float pitch_rate = channel_pitch->norm_input() * 180; + + if (roll_rate == 0 && + acro_state.locked_roll && + (ahrs.pitch_sensor > 7000 || + ahrs.pitch_sensor < -7000)) { + // when near the poles do rate holding, but don't unlock the + // desired roll + channel_roll->servo_out = g.rollController.get_rate_out(roll_rate, speed_scaler); + } else if (roll_rate == 0) { + if (!acro_state.locked_roll) { + acro_state.locked_roll = true; + acro_state.locked_roll_cd = ahrs.roll_sensor; + } + // try to cope with wrap while looping. + int32_t roll_error_cd = ahrs.roll_sensor - acro_state.locked_roll_cd; + if (roll_error_cd > 13500 && roll_error_cd < 21500) { + acro_state.locked_roll_cd += 18000; + } + if (roll_error_cd < -13500 && roll_error_cd > -21500) { + acro_state.locked_roll_cd -= 18000; + } + acro_state.locked_roll_cd = wrap_180_cd(acro_state.locked_roll_cd); + nav_roll_cd = acro_state.locked_roll_cd; + roll_error_cd = ahrs.roll_sensor - nav_roll_cd; + if (roll_error_cd > 31500) { + nav_roll_cd += 36000; + } else if (roll_error_cd < -31500) { + nav_roll_cd -= 36000; + } + stabilize_roll(speed_scaler); + } else { + acro_state.locked_roll = false; + channel_roll->servo_out = g.rollController.get_rate_out(roll_rate, speed_scaler); + } + + if (pitch_rate == 0) { + if (!acro_state.locked_pitch) { + acro_state.locked_pitch = true; + acro_state.locked_pitch_cd = ahrs.pitch_sensor; + } + nav_pitch_cd = acro_state.locked_pitch_cd; + stabilize_pitch(speed_scaler); + } else { + acro_state.locked_pitch = false; + channel_pitch->servo_out = g.pitchController.get_rate_out(pitch_rate, speed_scaler); + } + + stabilize_yaw(speed_scaler); +} + /* main stabilization function for all 3 axes */ @@ -241,6 +301,8 @@ static void stabilize() if (control_mode == TRAINING) { stabilize_training(speed_scaler); + } else if (control_mode == ACRO) { + stabilize_acro(speed_scaler); } else { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { stabilize_stick_mixing_fbw(); @@ -705,6 +767,7 @@ static void set_servos(void) } else if (g.throttle_passthru_stabilize && (control_mode == STABILIZE || control_mode == TRAINING || + control_mode == ACRO || control_mode == FLY_BY_WIRE_A)) { // manual pass through of throttle while in FBWA or // STABILIZE mode with THR_PASS_STAB set diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 70f6f92bd4..a21b58c112 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -43,6 +43,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) switch (control_mode) { case MANUAL: case TRAINING: + case ACRO: base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case STABILIZE: @@ -159,6 +160,10 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack case MANUAL: break; + case ACRO: + control_sensors_enabled |= (1<<10); // 3D angular rate control + break; + case STABILIZE: case FLY_BY_WIRE_A: control_sensors_enabled |= (1<<10); // 3D angular rate control @@ -1264,6 +1269,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case CIRCLE: case STABILIZE: case TRAINING: + case ACRO: case FLY_BY_WIRE_A: case FLY_BY_WIRE_B: case AUTO: diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 0ad21b94a6..e175aa1e98 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -60,6 +60,7 @@ enum FlightMode { CIRCLE = 1, STABILIZE = 2, TRAINING = 3, + ACRO = 4, FLY_BY_WIRE_A = 5, FLY_BY_WIRE_B = 6, AUTO = 10, diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index 4bc15dbf99..082c53261b 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -11,6 +11,7 @@ static void failsafe_short_on_event(int16_t fstype) { case MANUAL: case STABILIZE: + case ACRO: case FLY_BY_WIRE_A: case FLY_BY_WIRE_B: case TRAINING: @@ -44,6 +45,7 @@ static void failsafe_long_on_event(int16_t fstype) { case MANUAL: case STABILIZE: + case ACRO: case FLY_BY_WIRE_A: case FLY_BY_WIRE_B: case TRAINING: diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 11d63f9ab4..c57dfd9ac0 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -310,6 +310,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) mode != CIRCLE && mode != STABILIZE && mode != TRAINING && + mode != ACRO && mode != FLY_BY_WIRE_A && mode != FLY_BY_WIRE_B && mode != AUTO && diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 49c83b3514..d103511b78 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -340,6 +340,7 @@ static void set_mode(enum FlightMode mode) case MANUAL: case STABILIZE: case TRAINING: + case ACRO: case FLY_BY_WIRE_A: break; @@ -620,6 +621,9 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case TRAINING: port->print_P(PSTR("Training")); break; + case ACRO: + port->print_P(PSTR("ACRO")); + break; case FLY_BY_WIRE_A: port->print_P(PSTR("FBW_A")); break;