Copter: add option to disable GUIDED flight mode

Saves about 6kB of flash
This commit is contained in:
Peter Barker 2018-02-23 15:09:07 +11:00 committed by Randy Mackay
parent 0ddeb56a05
commit b8c432b1a1
6 changed files with 19 additions and 0 deletions

View File

@ -25,6 +25,7 @@
//#define WINCH_ENABLED DISABLED // disable winch support
//#define MODE_AUTO_ENABLED DISABLED // disable auto mode support
//#define MODE_DRIFT_ENABLED DISABLED // disable drift mode support
//#define MODE_GUIDED_ENABLED DISABLED // disable guided mode support
//#define MODE_LOITER_ENABLED DISABLED // disable loiter mode support
//#define MODE_POSHOLD_ENABLED DISABLED // disable poshold mode support
//#define MODE_SMARTRTL_ENABLED DISABLED // disable smartrtl mode support

View File

@ -968,7 +968,9 @@ private:
ModeDrift mode_drift;
#endif
ModeFlip mode_flip;
#if MODE_GUIDED_ENABLED == ENABLED
ModeGuided mode_guided;
#endif
ModeLand mode_land;
#if MODE_LOITER_ENABLED == ENABLED
ModeLoiter mode_loiter;

View File

@ -1341,6 +1341,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
break;
}
#if MODE_GUIDED_ENABLED == ENABLED
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82
{
// decode packet
@ -1581,6 +1582,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
break;
}
#endif
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
{

View File

@ -279,6 +279,12 @@
# define MODE_DRIFT_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Guided mode - control vehicle's position or angles from GCS
#ifndef MODE_GUIDED_ENABLED
# define MODE_GUIDED_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Loiter mode - allows vehicle to hold global position
#ifndef MODE_LOITER_ENABLED
@ -616,6 +622,10 @@
#error AC_Avoidance relies on AC_FENCE which is disabled
#endif
#if MODE_AUTO_ENABLED && !MODE_GUIDED_ENABLED
#error ModeAuto requires ModeGuided which is disabled
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//

View File

@ -66,9 +66,11 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode)
break;
#endif
#if MODE_GUIDED_ENABLED == ENABLED
case GUIDED:
ret = &mode_guided;
break;
#endif
case LAND:
ret = &mode_land;

View File

@ -36,12 +36,14 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
#endif
switch(control_mode) {
#if MODE_GUIDED_ENABLED == ENABLED
case GUIDED:
if (mode_guided.takeoff_start(takeoff_alt_cm)) {
set_auto_armed(true);
return true;
}
return false;
#endif
case LOITER:
case POSHOLD:
case ALT_HOLD: