From 1f0c31bcef7c9b4dd85690a57f5cab9c9cd37d98 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 13 Aug 2024 21:42:38 +1000 Subject: [PATCH] Sub: remove ENABLE/DISABLE defines, use 0/1 instead same as Plane, Rover, Sub, Tracker --- ArduSub/Parameters.cpp | 6 +++--- ArduSub/Sub.h | 12 ++++++------ ArduSub/commands_logic.cpp | 12 ++++++------ ArduSub/config.h | 16 ++++++++-------- ArduSub/defines.h | 10 +--------- ArduSub/mode_auto.cpp | 4 ++-- ArduSub/mode_poshold.cpp | 4 ++-- ArduSub/sensors.cpp | 2 +- 8 files changed, 29 insertions(+), 37 deletions(-) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 9444643a0c..0314f28ced 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -492,7 +492,7 @@ const AP_Param::Info Sub::var_info[] = { // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp GOBJECT(loiter_nav, "LOIT_", AC_Loiter), -#if CIRCLE_NAV_ENABLED == ENABLED +#if CIRCLE_NAV_ENABLED // @Group: CIRCLE_ // @Path: ../libraries/AC_WPNav/AC_Circle.cpp GOBJECT(circle_nav, "CIRCLE_", AC_Circle), @@ -598,7 +598,7 @@ const AP_Param::Info Sub::var_info[] = { // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp GOBJECT(scheduler, "SCHED_", AP_Scheduler), -#if AVOIDANCE_ENABLED == ENABLED +#if AVOIDANCE_ENABLED // @Group: AVOID_ // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp 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 GOBJECT(motors, "MOT_", AP_Motors6DOF), -#if RCMAP_ENABLED == ENABLED +#if RCMAP_ENABLED // @Group: RCMAP_ // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp GOBJECT(rcmap, "RCMAP_", RCMapper), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 909a28c517..52cd205175 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -78,7 +78,7 @@ #include // Optical Flow library // libraries which are dependent on #defines in defines.h and/or config.h -#if RCMAP_ENABLED == ENABLED +#if RCMAP_ENABLED #include // RC input mapping library #endif @@ -88,7 +88,7 @@ #include #endif -#if AVOIDANCE_ENABLED == ENABLED +#if AVOIDANCE_ENABLED #include // Stop at fence library #endif @@ -206,7 +206,7 @@ private: Mode::Number prev_control_mode; -#if RCMAP_ENABLED == ENABLED +#if RCMAP_ENABLED RCMapper rcmap; #endif @@ -355,7 +355,7 @@ private: AP_Mount camera_mount; #endif -#if AVOIDANCE_ENABLED == ENABLED +#if AVOIDANCE_ENABLED AC_Avoid avoid; #endif @@ -514,7 +514,7 @@ private: void do_loiter_unlimited(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); -#if NAV_GUIDED == ENABLED +#if NAV_GUIDED void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd); void do_guided_limits(const AP_Mission::Mission_Command& cmd); #endif @@ -531,7 +531,7 @@ private: bool verify_surface(const AP_Mission::Mission_Command& cmd); bool verify_RTL(void); 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); #endif bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index a77f4111e7..60e802c45f 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -61,7 +61,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd) do_loiter_time(cmd); break; -#if NAV_GUIDED == ENABLED +#if NAV_GUIDED case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer do_nav_guided_enable(cmd); break; @@ -107,7 +107,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd) do_mount_control(cmd); break; -#if NAV_GUIDED == ENABLED +#if NAV_GUIDED case MAV_CMD_DO_GUIDED_LIMITS: // 222 accept guided mode limits do_guided_limits(cmd); break; @@ -170,7 +170,7 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_LOITER_TIME: return verify_loiter_time(); -#if NAV_GUIDED == ENABLED +#if NAV_GUIDED case MAV_CMD_NAV_GUIDED_ENABLE: return verify_nav_guided_enable(cmd); #endif @@ -384,7 +384,7 @@ void Sub::do_loiter_time(const AP_Mission::Mission_Command& cmd) 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 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)); } -#if NAV_GUIDED == ENABLED +#if NAV_GUIDED // do_guided_limits - pass guided limits to guided controller 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; } -#if NAV_GUIDED == ENABLED +#if NAV_GUIDED // verify_nav_guided - check if we have breached any limits bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { diff --git a/ArduSub/config.h b/ArduSub/config.h index 3fab9bafaa..7e2f568626 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -31,7 +31,7 @@ // #ifndef CIRCLE_NAV_ENABLED -# define CIRCLE_NAV_ENABLED ENABLED +# define CIRCLE_NAV_ENABLED 1 #endif ////////////////////////////////////////////////////////////////////////////// @@ -39,7 +39,7 @@ // #ifndef RCMAP_ENABLED -# define RCMAP_ENABLED DISABLED +# define RCMAP_ENABLED 0 #endif ////////////////////////////////////////////////////////////////////////////// @@ -59,7 +59,7 @@ #endif #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 #ifndef RANGEFINDER_SIGNAL_MIN_DEFAULT @@ -72,11 +72,11 @@ // Avoidance (relies on Proximity and Fence) #ifndef AVOIDANCE_ENABLED -# define AVOIDANCE_ENABLED DISABLED +# define AVOIDANCE_ENABLED 0 #endif -#if AVOIDANCE_ENABLED == ENABLED // Avoidance Library relies on Fence -# define FENCE_ENABLED ENABLED +#if AVOIDANCE_ENABLED // Avoidance Library relies on Fence +# define FENCE_ENABLED 1 #endif #ifndef MAV_SYSTEM_ID @@ -90,7 +90,7 @@ ////////////////////////////////////////////////////////////////////////////// // Nav-Guided - allows external nav computer to control vehicle #ifndef NAV_GUIDED -# define NAV_GUIDED ENABLED +# define NAV_GUIDED 1 #endif ////////////////////////////////////////////////////////////////////////////// @@ -156,7 +156,7 @@ // PosHold parameter defaults // #ifndef POSHOLD_ENABLED -# define POSHOLD_ENABLED ENABLED // PosHold flight mode enabled by default +# define POSHOLD_ENABLED 1 // PosHold flight mode enabled by default #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 49dcde32fd..af49e7ccd1 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -2,14 +2,6 @@ #include -// 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 SURFACE_DETECTOR_TRIGGER_SEC 1.0 @@ -77,7 +69,7 @@ enum LoggingParameters { // GCS failsafe #ifndef FS_GCS -# define FS_GCS DISABLED +# define FS_GCS 0 #endif #ifndef FS_GCS_TIMEOUT_S # define FS_GCS_TIMEOUT_S 5.0 // gcs failsafe triggers after this number of seconds with no GCS heartbeat diff --git a/ArduSub/mode_auto.cpp b/ArduSub/mode_auto.cpp index 39aabc69eb..d1fdc42f2a 100644 --- a/ArduSub/mode_auto.cpp +++ b/ArduSub/mode_auto.cpp @@ -50,7 +50,7 @@ void ModeAuto::run() break; case Auto_NavGuided: -#if NAV_GUIDED == ENABLED +#if NAV_GUIDED auto_nav_guided_run(); #endif 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); } -#if NAV_GUIDED == ENABLED +#if NAV_GUIDED // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode void ModeAuto::auto_nav_guided_start() { diff --git a/ArduSub/mode_poshold.cpp b/ArduSub/mode_poshold.cpp index a5cb3d9bfd..479cdcb543 100644 --- a/ArduSub/mode_poshold.cpp +++ b/ArduSub/mode_poshold.cpp @@ -4,7 +4,7 @@ #include "Sub.h" -#if POSHOLD_ENABLED == ENABLED +#if POSHOLD_ENABLED // poshold_init - initialise PosHold controller bool ModePoshold::init(bool ignore_checks) @@ -111,4 +111,4 @@ void ModePoshold::run() // Update z axis // control_depth(); } -#endif // POSHOLD_ENABLED == ENABLED +#endif // POSHOLD_ENABLED diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index d9a491de3f..f918e81a71 100644 --- a/ArduSub/sensors.cpp +++ b/ArduSub/sensors.cpp @@ -41,7 +41,7 @@ void Sub::read_rangefinder() 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 temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z); #endif