Plane: switch to an enum for flight modes

This commit is contained in:
Andrew Tridgell 2012-11-30 20:34:56 +11:00
parent f7a05f577e
commit 4864d162d0
3 changed files with 14 additions and 39 deletions

View File

@ -97,7 +97,8 @@ static void stabilize()
// Mix Stick input to allow users to override control surfaces
// -----------------------------------------------------------
if (stick_mixing_enabled()) {
if (control_mode < FLY_BY_WIRE_A || control_mode > FLY_BY_WIRE_C) {
if (control_mode != FLY_BY_WIRE_A &&
control_mode != FLY_BY_WIRE_B) {
// do stick mixing on aileron/elevator if not in a fly by
// wire mode
ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;

View File

@ -43,7 +43,6 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
case STABILIZE:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case FLY_BY_WIRE_C:
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case AUTO:
@ -163,13 +162,6 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
control_sensors_enabled |= (1<<15); // motor control
break;
case FLY_BY_WIRE_C:
control_sensors_enabled |= (1<<10); // 3D angular rate control
control_sensors_enabled |= (1<<11); // attitude stabilisation
control_sensors_enabled |= (1<<13); // altitude control
control_sensors_enabled |= (1<<15); // motor control
break;
case AUTO:
case RTL:
case LOITER:
@ -1153,7 +1145,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case STABILIZE:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
case FLY_BY_WIRE_C:
case AUTO:
case RTL:
case LOITER:

View File

@ -59,35 +59,18 @@
#define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 2
// Auto Pilot modes
// ----------------
#define MANUAL 0
#define CIRCLE 1 // When flying sans GPS, and we loose
// the radio, just circle
#define STABILIZE 2
#define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal =>
// desired roll angle, left stick vertical =>
// desired pitch angle, right stick vertical =
// manual throttle
#define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal =>
// desired roll angle, left stick vertical =>
// desired pitch angle, right stick vertical
// => desired airspeed
#define FLY_BY_WIRE_C 7 // Fly By Wire C has left stick horizontal =>
// desired roll angle, left stick vertical =>
// desired climb rate, right stick vertical =>
// desired airspeed
// Fly By Wire B and Fly By Wire C require airspeed sensor
#define AUTO 10
#define RTL 11
#define LOITER 12
//#define TAKEOFF 13 // This is not used by APM. It appears here
// for consistency with ACM
//#define LAND 14 // This is not used by APM. It appears here for
// consistency with ACM
#define GUIDED 15
#define INITIALISING 16 // in startup routines
enum FlightMode {
MANUAL = 0,
CIRCLE = 1,
STABILIZE = 2,
FLY_BY_WIRE_A = 5,
FLY_BY_WIRE_B = 6,
AUTO = 10,
RTL = 11,
LOITER = 12,
GUIDED = 15,
INITIALISING = 16
};
// Commands - Note that APM now uses a subset of the MAVLink protocol