diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 6cd54db8a0..f30f0d22ea 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -15,7 +15,6 @@ //#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 OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space //#define FRSKY_TELEM_ENABLED DISABLED // disable FRSky telemetry //#define PRECISION_LANDING DISABLED // disable precision landing using companion computer or IRLock sensor //#define BEACON_ENABLED DISABLED // disable beacon support diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index a530bd798f..07537db19d 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -113,7 +113,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(rc_loop, 100, 130, 3), SCHED_TASK(throttle_loop, 50, 75, 6), SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200, 9), -#if OPTFLOW == ENABLED +#if AP_OPTICALFLOW_ENABLED SCHED_TASK_CLASS(OpticalFlow, &copter.optflow, update, 200, 160, 12), #endif SCHED_TASK(update_batt_compass, 10, 120, 15), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 1a3c5a046c..12d959155a 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -124,9 +124,7 @@ #if AP_TERRAIN_AVAILABLE # include #endif -#if OPTFLOW == ENABLED # include -#endif #if RANGEFINDER_ENABLED == ENABLED # include #endif @@ -307,7 +305,7 @@ private: AP_Arming_Copter arming; // Optical flow sensor -#if OPTFLOW == ENABLED +#if AP_OPTICALFLOW_ENABLED OpticalFlow optflow; #endif @@ -975,7 +973,7 @@ private: #if MODE_SMARTRTL_ENABLED == ENABLED ModeSmartRTL mode_smartrtl; #endif -#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED +#if !HAL_MINIMIZE_FEATURES && AP_OPTICALFLOW_ENABLED ModeFlowHold mode_flowhold; #endif #if MODE_ZIGZAG_ENABLED == ENABLED diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 213144b3ad..db0737e89e 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -114,7 +114,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) } #endif -#if OPTFLOW == ENABLED +#if AP_OPTICALFLOW_ENABLED const OpticalFlow *optflow = AP::opticalflow(); if (optflow && optflow->enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 8c8cdbe8cd..b7ff31eeef 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -695,7 +695,7 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(terrain, "TERRAIN_", AP_Terrain), #endif -#if OPTFLOW == ENABLED +#if AP_OPTICALFLOW_ENABLED // @Group: FLOW // @Path: ../libraries/AP_OpticalFlow/AP_OpticalFlow.cpp GOBJECT(optflow, "FLOW", OpticalFlow), @@ -915,7 +915,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 1000), -#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED +#if !HAL_MINIMIZE_FEATURES && AP_OPTICALFLOW_ENABLED // @Group: FHLD // @Path: mode_flowhold.cpp AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, ModeFlowHold), @@ -1131,7 +1131,7 @@ ParametersG2::ParametersG2(void) #if MODE_SMARTRTL_ENABLED == ENABLED ,smart_rtl() #endif -#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED +#if !HAL_MINIMIZE_FEATURES && AP_OPTICALFLOW_ENABLED ,mode_flowhold_ptr(&copter.mode_flowhold) #endif #if MODE_FOLLOW_ENABLED == ENABLED diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index f158cd9bc1..a8813e5dbe 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -574,7 +574,7 @@ public: ToyMode toy_mode; #endif -#if OPTFLOW == ENABLED +#if AP_OPTICALFLOW_ENABLED // we need a pointer to the mode for the G2 table void *mode_flowhold_ptr; #endif diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 8949cb8134..e0f6bc46ff 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -496,7 +496,7 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi break; case AUX_FUNC::FLOWHOLD: -#if OPTFLOW == ENABLED +#if AP_OPTICALFLOW_ENABLED do_aux_function_change_mode(Mode::Number::FLOWHOLD, ch_flag); #endif break; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 107cb6b1e4..a404e4eac8 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -168,12 +168,6 @@ # define EKF_ORIGIN_MAX_ALT_KM 50 // EKF origin and home must be within 50km vertically #endif -////////////////////////////////////////////////////////////////////////////// -// OPTICAL_FLOW -#ifndef OPTFLOW - # define OPTFLOW ENABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // Auto Tuning #ifndef AUTOTUNE_ENABLED diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 41884b0261..3a516b2520 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -132,7 +132,7 @@ bool Copter::ekf_over_threshold() } bool optflow_healthy = false; -#if OPTFLOW == ENABLED +#if AP_OPTICALFLOW_ENABLED optflow_healthy = optflow.healthy(); #endif if (!optflow_healthy && (vel_variance >= (2.0f * g.fs_ekf_thresh))) { diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index acc71b7c4b..792e8d1bc4 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -139,7 +139,7 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode) break; #endif -#if OPTFLOW == ENABLED +#if AP_OPTICALFLOW_ENABLED case Mode::Number::FLOWHOLD: ret = (Mode *)g2.mode_flowhold_ptr; break; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 00692f8515..338494f307 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -770,7 +770,7 @@ private: }; -#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED +#if !HAL_MINIMIZE_FEATURES && AP_OPTICALFLOW_ENABLED /* class to support FLOWHOLD mode, which is a position hold mode using optical flow directly, avoiding the need for a rangefinder @@ -856,7 +856,7 @@ private: // last time there was significant stick input uint32_t last_stick_input_ms; }; -#endif // OPTFLOW +#endif // AP_OPTICALFLOW_ENABLED class ModeGuided : public Mode { diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 751a374f5f..8551a7d5ed 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -1,7 +1,7 @@ #include "Copter.h" #include -#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED +#if !HAL_MINIMIZE_FEATURES && AP_OPTICALFLOW_ENABLED /* implement FLOWHOLD mode, for position hold using optical flow @@ -509,4 +509,4 @@ void ModeFlowHold::update_height_estimate(void) last_ins_height = ins_height; } -#endif // OPTFLOW == ENABLED +#endif // AP_OPTICALFLOW_ENABLED diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 57096f6579..b35afe83d6 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -117,10 +117,10 @@ void Copter::init_ardupilot() attitude_control->parameter_sanity_check(); -#if OPTFLOW == ENABLED +#if AP_OPTICALFLOW_ENABLED // initialise optical flow sensor optflow.init(MASK_LOG_OPTFLOW); -#endif // OPTFLOW == ENABLED +#endif // AP_OPTICALFLOW_ENABLED #if HAL_MOUNT_ENABLED // initialise camera mount @@ -341,7 +341,7 @@ bool Copter::ekf_has_relative_position() const // return immediately if neither optflow nor visual odometry is enabled bool enabled = false; -#if OPTFLOW == ENABLED +#if AP_OPTICALFLOW_ENABLED if (optflow.enabled()) { enabled = true; }