diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 9b89e93abc..3049e75818 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -5,8 +5,6 @@ //#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space //#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash //#define RANGEFINDER_ENABLED DISABLED // disable rangefinder to save 1k of flash -//#define AC_AVOID_ENABLED DISABLED // disable stop-at-fence library -//#define AC_OAPATHPLANNER_ENABLED DISABLED // disable path planning around obstacles //#define PARACHUTE DISABLED // disable parachute release to save 1k of flash //#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands //#define MODE_ACRO_ENABLED DISABLED // disable acrobatic mode support diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 3a31aac761..cfc5ebc2d3 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -299,7 +299,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) bool AP_Arming_Copter::oa_checks(bool display_failure) { -#if AC_OAPATHPLANNER_ENABLED == ENABLED +#if AP_OAPATHPLANNER_ENABLED char failure_msg[50] = {}; if (copter.g2.oa.pre_arm_check(failure_msg, ARRAY_SIZE(failure_msg))) { return true; @@ -407,7 +407,7 @@ bool AP_Arming_Copter::proximity_checks(bool display_failure) const } // get closest object if we might use it for avoidance -#if AC_AVOID_ENABLED == ENABLED +#if AP_AVOIDANCE_ENABLED float angle_deg, distance; if (copter.avoid.proximity_avoidance_enabled() && copter.g2.proximity.get_closest_object(angle_deg, distance)) { // display error if something is within 60cm diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index fbf4c800b4..f05869c7a6 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -103,10 +103,10 @@ #include #endif -#if AC_AVOID_ENABLED == ENABLED +#if AP_AVOIDANCE_ENABLED #include #endif -#if AC_OAPATHPLANNER_ENABLED == ENABLED +#if AP_OAPATHPLANNER_ENABLED #include #include #endif @@ -155,11 +155,11 @@ #include // Custom control library #endif -#if AC_AVOID_ENABLED && !AP_FENCE_ENABLED +#if AP_AVOIDANCE_ENABLED && !AP_FENCE_ENABLED #error AC_Avoidance relies on AP_FENCE_ENABLED which is disabled #endif -#if AC_OAPATHPLANNER_ENABLED && !AP_FENCE_ENABLED +#if AP_OAPATHPLANNER_ENABLED && !AP_FENCE_ENABLED #error AP_OAPathPlanner relies on AP_FENCE_ENABLED which is disabled #endif @@ -513,7 +513,7 @@ private: AP_Mount camera_mount; #endif -#if AC_AVOID_ENABLED == ENABLED +#if AP_AVOIDANCE_ENABLED AC_Avoid avoid; #endif diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 431e4b9c22..9c7eeccf16 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -369,7 +369,7 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) CHECK_PAYLOAD_SIZE(ADSB_VEHICLE); copter.adsb.send_adsb_vehicle(chan); #endif -#if AC_OAPATHPLANNER_ENABLED == ENABLED +#if AP_OAPATHPLANNER_ENABLED AP_OADatabase *oadb = AP_OADatabase::get_singleton(); if (oadb != nullptr) { CHECK_PAYLOAD_SIZE(ADSB_VEHICLE); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index f8f1376c90..5eb636d938 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -595,7 +595,7 @@ const AP_Param::Info Copter::var_info[] = { // @Group: AVOID_ // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp -#if AC_AVOID_ENABLED == ENABLED +#if AP_AVOIDANCE_ENABLED GOBJECT(avoid, "AVOID_", AC_Avoid), #endif @@ -917,7 +917,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("TUNE_MAX", 32, ParametersG2, tuning_max, 0), -#if AC_OAPATHPLANNER_ENABLED == ENABLED +#if AP_OAPATHPLANNER_ENABLED // @Group: OA_ // @Path: ../libraries/AC_Avoidance/AP_OAPathPlanner.cpp AP_SUBGROUPINFO(oa, "OA_", 33, ParametersG2, AP_OAPathPlanner), diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 21bc10e78c..0e3d8920aa 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -595,7 +595,7 @@ public: AP_Float tuning_min; AP_Float tuning_max; -#if AC_OAPATHPLANNER_ENABLED == ENABLED +#if AP_OAPATHPLANNER_ENABLED // object avoidance path planning AP_OAPathPlanner oa; #endif diff --git a/ArduCopter/avoidance.cpp b/ArduCopter/avoidance.cpp index 76400a4b23..9fc85195f1 100644 --- a/ArduCopter/avoidance.cpp +++ b/ArduCopter/avoidance.cpp @@ -3,7 +3,7 @@ // check if proximity type Simple Avoidance should be enabled based on alt void Copter::low_alt_avoidance() { -#if AC_AVOID_ENABLED == ENABLED +#if AP_AVOIDANCE_ENABLED int32_t alt_cm; if (!get_rangefinder_height_interpolated_cm(alt_cm)) { // enable avoidance if we don't have a valid rangefinder reading diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 9b134c70e5..31bb285db5 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -29,7 +29,7 @@ #include "APM_Config.h" #include #include - +#include ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// @@ -557,16 +557,8 @@ // Fence, Rally and Terrain and AC_Avoidance defaults // -#ifndef AC_AVOID_ENABLED - #define AC_AVOID_ENABLED ENABLED -#endif - -#ifndef AC_OAPATHPLANNER_ENABLED - #define AC_OAPATHPLANNER_ENABLED ENABLED -#endif - -#if MODE_FOLLOW_ENABLED && !AC_AVOID_ENABLED - #error Follow Mode relies on AC_AVOID which is disabled +#if MODE_FOLLOW_ENABLED && !AP_AVOIDANCE_ENABLED + #error Follow Mode relies on AP_AVOIDANCE_ENABLED which is disabled #endif #if MODE_AUTO_ENABLED && !MODE_GUIDED_ENABLED diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 73b1384bc5..3c48cebdd6 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -947,7 +947,7 @@ float Mode::get_pilot_desired_throttle() const float Mode::get_avoidance_adjusted_climbrate(float target_rate) { -#if AC_AVOID_ENABLED == ENABLED +#if AP_AVOIDANCE_ENABLED AP::ac_avoid()->adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), target_rate, G_Dt); return target_rate; #else diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 36a344f87e..36c7a60143 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -79,7 +79,7 @@ void ModeAltHold::run() case AltHold_Flying: motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); -#if AC_AVOID_ENABLED == ENABLED +#if AP_AVOIDANCE_ENABLED // apply avoidance copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max); #endif @@ -100,4 +100,4 @@ void ModeAltHold::run() // run the vertical position controller and set output throttle pos_control->update_z_controller(); -} \ No newline at end of file +} diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index ddb55d3d6d..f5414e9dac 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -332,7 +332,7 @@ void ModeFlowHold::run() bf_angles.x = constrain_float(bf_angles.x, -angle_max, angle_max); bf_angles.y = constrain_float(bf_angles.y, -angle_max, angle_max); -#if AC_AVOID_ENABLED == ENABLED +#if AP_AVOIDANCE_ENABLED // apply avoidance copter.avoid.adjust_roll_pitch(bf_angles.x, bf_angles.y, copter.aparm.angle_max); #endif diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 0a76dc8cb8..9d9ef816a0 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -10,7 +10,7 @@ * TODO: "channel 7 option" to lock onto "pointed at" target * TODO: do better in terms of loitering around the moving point; may need a PID? Maybe use loiter controller somehow? * TODO: extrapolate target vehicle position using its velocity and acceleration - * TODO: ensure AC_AVOID_ENABLED is true because we rely on it velocity limiting functions + * TODO: ensure AP_AVOIDANCE_ENABLED is true because we rely on it velocity limiting functions */ // initialise follow mode diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index ba9a52f60e..52814dae44 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -800,7 +800,7 @@ void ModeGuided::velaccel_control_run() } bool do_avoid = false; -#if AC_AVOID_ENABLED +#if AP_AVOIDANCE_ENABLED // limit the velocity for obstacle/fence avoidance copter.avoid.adjust_velocity(guided_vel_target_cms, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt); do_avoid = copter.avoid.limits_active(); diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 8545d045cb..41f6e37c76 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -93,7 +93,7 @@ void Copter::init_ardupilot() airspeed.set_log_bit(MASK_LOG_IMU); #endif -#if AC_OAPATHPLANNER_ENABLED == ENABLED +#if AP_OAPATHPLANNER_ENABLED g2.oa.init(); #endif @@ -449,7 +449,7 @@ void Copter::allocate_motors(void) } AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info); -#if AC_OAPATHPLANNER_ENABLED == ENABLED +#if AP_OAPATHPLANNER_ENABLED wp_nav = new AC_WPNav_OA(inertial_nav, *ahrs_view, *pos_control, *attitude_control); #else wp_nav = new AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control);