mirror of https://github.com/ArduPilot/ardupilot
Sub: remove ENABLE/DISABLE defines, use 0/1 instead
same as Plane, Rover, Sub, Tracker
This commit is contained in:
parent
11e05b1436
commit
1f0c31bcef
|
@ -492,7 +492,7 @@ const AP_Param::Info Sub::var_info[] = {
|
||||||
// @Path: ../libraries/AC_WPNav/AC_Loiter.cpp
|
// @Path: ../libraries/AC_WPNav/AC_Loiter.cpp
|
||||||
GOBJECT(loiter_nav, "LOIT_", AC_Loiter),
|
GOBJECT(loiter_nav, "LOIT_", AC_Loiter),
|
||||||
|
|
||||||
#if CIRCLE_NAV_ENABLED == ENABLED
|
#if CIRCLE_NAV_ENABLED
|
||||||
// @Group: CIRCLE_
|
// @Group: CIRCLE_
|
||||||
// @Path: ../libraries/AC_WPNav/AC_Circle.cpp
|
// @Path: ../libraries/AC_WPNav/AC_Circle.cpp
|
||||||
GOBJECT(circle_nav, "CIRCLE_", AC_Circle),
|
GOBJECT(circle_nav, "CIRCLE_", AC_Circle),
|
||||||
|
@ -598,7 +598,7 @@ const AP_Param::Info Sub::var_info[] = {
|
||||||
// @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
|
// @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
|
||||||
GOBJECT(scheduler, "SCHED_", AP_Scheduler),
|
GOBJECT(scheduler, "SCHED_", AP_Scheduler),
|
||||||
|
|
||||||
#if AVOIDANCE_ENABLED == ENABLED
|
#if AVOIDANCE_ENABLED
|
||||||
// @Group: AVOID_
|
// @Group: AVOID_
|
||||||
// @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
|
// @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
|
||||||
GOBJECT(avoid, "AVOID_", AC_Avoid),
|
GOBJECT(avoid, "AVOID_", AC_Avoid),
|
||||||
|
@ -614,7 +614,7 @@ const AP_Param::Info Sub::var_info[] = {
|
||||||
// @Path: ../libraries/AP_Motors/AP_Motors6DOF.cpp,../libraries/AP_Motors/AP_MotorsMulticopter.cpp
|
// @Path: ../libraries/AP_Motors/AP_Motors6DOF.cpp,../libraries/AP_Motors/AP_MotorsMulticopter.cpp
|
||||||
GOBJECT(motors, "MOT_", AP_Motors6DOF),
|
GOBJECT(motors, "MOT_", AP_Motors6DOF),
|
||||||
|
|
||||||
#if RCMAP_ENABLED == ENABLED
|
#if RCMAP_ENABLED
|
||||||
// @Group: RCMAP_
|
// @Group: RCMAP_
|
||||||
// @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
|
// @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
|
||||||
GOBJECT(rcmap, "RCMAP_", RCMapper),
|
GOBJECT(rcmap, "RCMAP_", RCMapper),
|
||||||
|
|
|
@ -78,7 +78,7 @@
|
||||||
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
|
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
|
||||||
|
|
||||||
// libraries which are dependent on #defines in defines.h and/or config.h
|
// libraries which are dependent on #defines in defines.h and/or config.h
|
||||||
#if RCMAP_ENABLED == ENABLED
|
#if RCMAP_ENABLED
|
||||||
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
|
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -88,7 +88,7 @@
|
||||||
#include <AP_RPM/AP_RPM.h>
|
#include <AP_RPM/AP_RPM.h>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if AVOIDANCE_ENABLED == ENABLED
|
#if AVOIDANCE_ENABLED
|
||||||
#include <AC_Avoidance/AC_Avoid.h> // Stop at fence library
|
#include <AC_Avoidance/AC_Avoid.h> // Stop at fence library
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -206,7 +206,7 @@ private:
|
||||||
|
|
||||||
Mode::Number prev_control_mode;
|
Mode::Number prev_control_mode;
|
||||||
|
|
||||||
#if RCMAP_ENABLED == ENABLED
|
#if RCMAP_ENABLED
|
||||||
RCMapper rcmap;
|
RCMapper rcmap;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -355,7 +355,7 @@ private:
|
||||||
AP_Mount camera_mount;
|
AP_Mount camera_mount;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if AVOIDANCE_ENABLED == ENABLED
|
#if AVOIDANCE_ENABLED
|
||||||
AC_Avoid avoid;
|
AC_Avoid avoid;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -514,7 +514,7 @@ private:
|
||||||
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
|
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
|
||||||
void do_circle(const AP_Mission::Mission_Command& cmd);
|
void do_circle(const AP_Mission::Mission_Command& cmd);
|
||||||
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
|
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED
|
||||||
void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
||||||
void do_guided_limits(const AP_Mission::Mission_Command& cmd);
|
void do_guided_limits(const AP_Mission::Mission_Command& cmd);
|
||||||
#endif
|
#endif
|
||||||
|
@ -531,7 +531,7 @@ private:
|
||||||
bool verify_surface(const AP_Mission::Mission_Command& cmd);
|
bool verify_surface(const AP_Mission::Mission_Command& cmd);
|
||||||
bool verify_RTL(void);
|
bool verify_RTL(void);
|
||||||
bool verify_circle(const AP_Mission::Mission_Command& cmd);
|
bool verify_circle(const AP_Mission::Mission_Command& cmd);
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED
|
||||||
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
||||||
#endif
|
#endif
|
||||||
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
|
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
|
||||||
|
|
|
@ -61,7 +61,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
|
||||||
do_loiter_time(cmd);
|
do_loiter_time(cmd);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED
|
||||||
case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer
|
case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer
|
||||||
do_nav_guided_enable(cmd);
|
do_nav_guided_enable(cmd);
|
||||||
break;
|
break;
|
||||||
|
@ -107,7 +107,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
|
||||||
do_mount_control(cmd);
|
do_mount_control(cmd);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED
|
||||||
case MAV_CMD_DO_GUIDED_LIMITS: // 222 accept guided mode limits
|
case MAV_CMD_DO_GUIDED_LIMITS: // 222 accept guided mode limits
|
||||||
do_guided_limits(cmd);
|
do_guided_limits(cmd);
|
||||||
break;
|
break;
|
||||||
|
@ -170,7 +170,7 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
|
||||||
case MAV_CMD_NAV_LOITER_TIME:
|
case MAV_CMD_NAV_LOITER_TIME:
|
||||||
return verify_loiter_time();
|
return verify_loiter_time();
|
||||||
|
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED
|
||||||
case MAV_CMD_NAV_GUIDED_ENABLE:
|
case MAV_CMD_NAV_GUIDED_ENABLE:
|
||||||
return verify_nav_guided_enable(cmd);
|
return verify_nav_guided_enable(cmd);
|
||||||
#endif
|
#endif
|
||||||
|
@ -384,7 +384,7 @@ void Sub::do_loiter_time(const AP_Mission::Mission_Command& cmd)
|
||||||
loiter_time_max = cmd.p1; // units are (seconds)
|
loiter_time_max = cmd.p1; // units are (seconds)
|
||||||
}
|
}
|
||||||
|
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED
|
||||||
// do_nav_guided_enable - initiate accepting commands from external nav computer
|
// do_nav_guided_enable - initiate accepting commands from external nav computer
|
||||||
void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
||||||
{
|
{
|
||||||
|
@ -417,7 +417,7 @@ void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd)
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000));
|
gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000));
|
||||||
}
|
}
|
||||||
|
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED
|
||||||
// do_guided_limits - pass guided limits to guided controller
|
// do_guided_limits - pass guided limits to guided controller
|
||||||
void Sub::do_guided_limits(const AP_Mission::Mission_Command& cmd)
|
void Sub::do_guided_limits(const AP_Mission::Mission_Command& cmd)
|
||||||
{
|
{
|
||||||
|
@ -550,7 +550,7 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
|
||||||
return fabsf(sub.circle_nav.get_angle_total()/M_2PI) >= turns;
|
return fabsf(sub.circle_nav.get_angle_total()/M_2PI) >= turns;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED
|
||||||
// verify_nav_guided - check if we have breached any limits
|
// verify_nav_guided - check if we have breached any limits
|
||||||
bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
||||||
{
|
{
|
||||||
|
|
|
@ -31,7 +31,7 @@
|
||||||
//
|
//
|
||||||
|
|
||||||
#ifndef CIRCLE_NAV_ENABLED
|
#ifndef CIRCLE_NAV_ENABLED
|
||||||
# define CIRCLE_NAV_ENABLED ENABLED
|
# define CIRCLE_NAV_ENABLED 1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
@ -39,7 +39,7 @@
|
||||||
//
|
//
|
||||||
|
|
||||||
#ifndef RCMAP_ENABLED
|
#ifndef RCMAP_ENABLED
|
||||||
# define RCMAP_ENABLED DISABLED
|
# define RCMAP_ENABLED 0
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
@ -59,7 +59,7 @@
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF
|
#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF
|
||||||
# define RANGEFINDER_TILT_CORRECTION DISABLED
|
# define RANGEFINDER_TILT_CORRECTION 0
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef RANGEFINDER_SIGNAL_MIN_DEFAULT
|
#ifndef RANGEFINDER_SIGNAL_MIN_DEFAULT
|
||||||
|
@ -72,11 +72,11 @@
|
||||||
|
|
||||||
// Avoidance (relies on Proximity and Fence)
|
// Avoidance (relies on Proximity and Fence)
|
||||||
#ifndef AVOIDANCE_ENABLED
|
#ifndef AVOIDANCE_ENABLED
|
||||||
# define AVOIDANCE_ENABLED DISABLED
|
# define AVOIDANCE_ENABLED 0
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if AVOIDANCE_ENABLED == ENABLED // Avoidance Library relies on Fence
|
#if AVOIDANCE_ENABLED // Avoidance Library relies on Fence
|
||||||
# define FENCE_ENABLED ENABLED
|
# define FENCE_ENABLED 1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef MAV_SYSTEM_ID
|
#ifndef MAV_SYSTEM_ID
|
||||||
|
@ -90,7 +90,7 @@
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// Nav-Guided - allows external nav computer to control vehicle
|
// Nav-Guided - allows external nav computer to control vehicle
|
||||||
#ifndef NAV_GUIDED
|
#ifndef NAV_GUIDED
|
||||||
# define NAV_GUIDED ENABLED
|
# define NAV_GUIDED 1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
@ -156,7 +156,7 @@
|
||||||
// PosHold parameter defaults
|
// PosHold parameter defaults
|
||||||
//
|
//
|
||||||
#ifndef POSHOLD_ENABLED
|
#ifndef POSHOLD_ENABLED
|
||||||
# define POSHOLD_ENABLED ENABLED // PosHold flight mode enabled by default
|
# define POSHOLD_ENABLED 1 // PosHold flight mode enabled by default
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
|
|
@ -2,14 +2,6 @@
|
||||||
|
|
||||||
#include <AP_HAL/AP_HAL_Boards.h>
|
#include <AP_HAL/AP_HAL_Boards.h>
|
||||||
|
|
||||||
// Just so that it's completely clear...
|
|
||||||
#define ENABLED 1
|
|
||||||
#define DISABLED 0
|
|
||||||
|
|
||||||
// this avoids a very common config error
|
|
||||||
#define ENABLE ENABLED
|
|
||||||
#define DISABLE DISABLED
|
|
||||||
|
|
||||||
#define BOTTOM_DETECTOR_TRIGGER_SEC 1.0
|
#define BOTTOM_DETECTOR_TRIGGER_SEC 1.0
|
||||||
#define SURFACE_DETECTOR_TRIGGER_SEC 1.0
|
#define SURFACE_DETECTOR_TRIGGER_SEC 1.0
|
||||||
|
|
||||||
|
@ -77,7 +69,7 @@ enum LoggingParameters {
|
||||||
|
|
||||||
// GCS failsafe
|
// GCS failsafe
|
||||||
#ifndef FS_GCS
|
#ifndef FS_GCS
|
||||||
# define FS_GCS DISABLED
|
# define FS_GCS 0
|
||||||
#endif
|
#endif
|
||||||
#ifndef FS_GCS_TIMEOUT_S
|
#ifndef FS_GCS_TIMEOUT_S
|
||||||
# define FS_GCS_TIMEOUT_S 5.0 // gcs failsafe triggers after this number of seconds with no GCS heartbeat
|
# define FS_GCS_TIMEOUT_S 5.0 // gcs failsafe triggers after this number of seconds with no GCS heartbeat
|
||||||
|
|
|
@ -50,7 +50,7 @@ void ModeAuto::run()
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case Auto_NavGuided:
|
case Auto_NavGuided:
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED
|
||||||
auto_nav_guided_run();
|
auto_nav_guided_run();
|
||||||
#endif
|
#endif
|
||||||
break;
|
break;
|
||||||
|
@ -252,7 +252,7 @@ void ModeAuto::auto_circle_run()
|
||||||
attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw(), true);
|
attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw(), true);
|
||||||
}
|
}
|
||||||
|
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED
|
||||||
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
|
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
|
||||||
void ModeAuto::auto_nav_guided_start()
|
void ModeAuto::auto_nav_guided_start()
|
||||||
{
|
{
|
||||||
|
|
|
@ -4,7 +4,7 @@
|
||||||
|
|
||||||
#include "Sub.h"
|
#include "Sub.h"
|
||||||
|
|
||||||
#if POSHOLD_ENABLED == ENABLED
|
#if POSHOLD_ENABLED
|
||||||
|
|
||||||
// poshold_init - initialise PosHold controller
|
// poshold_init - initialise PosHold controller
|
||||||
bool ModePoshold::init(bool ignore_checks)
|
bool ModePoshold::init(bool ignore_checks)
|
||||||
|
@ -111,4 +111,4 @@ void ModePoshold::run()
|
||||||
// Update z axis //
|
// Update z axis //
|
||||||
control_depth();
|
control_depth();
|
||||||
}
|
}
|
||||||
#endif // POSHOLD_ENABLED == ENABLED
|
#endif // POSHOLD_ENABLED
|
||||||
|
|
|
@ -41,7 +41,7 @@ void Sub::read_rangefinder()
|
||||||
|
|
||||||
int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
|
int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
|
||||||
|
|
||||||
#if RANGEFINDER_TILT_CORRECTION == ENABLED
|
#if RANGEFINDER_TILT_CORRECTION
|
||||||
// correct alt for angle of the rangefinder
|
// correct alt for angle of the rangefinder
|
||||||
temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
|
temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue