aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: added initial implementation of ACRO mode

Browse Source 
rate based control with attitude locking
This commit is contained in:
Andrew Tridgell 2013-07-10 23:25:38 +10:00
parent f2316747f5
commit 608007361b
7 changed files with 103 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduPlane/ArduPlane.pde
View File

@ -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:

63
ArduPlane/Attitude.pde
View File

@ -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

6
ArduPlane/GCS_Mavlink.pde
View File

@ -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:

1
ArduPlane/defines.h
View File

@ -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,

2
ArduPlane/events.pde
View File

@ -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:

1
ArduPlane/setup.pde
View File

@ -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 &&

4
ArduPlane/system.pde
View File

@ -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;