diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 7e6ad3824d..634253fc5f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -41,6 +41,7 @@ #include "AP_RangeFinder_PWM.h" #include "AP_RangeFinder_GYUS42v2.h" #include "AP_RangeFinder_HC_SR04.h" +#include "AP_RangeFinder_Bebop.h" #include "AP_RangeFinder_BLPing.h" #include "AP_RangeFinder_UAVCAN.h" #include "AP_RangeFinder_Lanbao.h" @@ -347,14 +348,17 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) case Type::PLI2C: case Type::PLI2CV3: case Type::PLI2CV3HP: +#if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED FOREACH_I2C(i) { if (_add_backend(AP_RangeFinder_PulsedLightLRF::detect(i, state[instance], params[instance], _type), instance)) { break; } } +#endif break; case Type::MBI2C: { +#if AP_RANGEFINDER_MAXSONARI2CXL_ENABLED uint8_t addr = AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR; if (params[instance].address != 0) { addr = params[instance].address; @@ -367,8 +371,10 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } break; +#endif } case Type::LWI2C: +#if AP_RANGEFINDER_LWI2C_ENABLED if (params[instance].address) { // the LW20 needs a long time to boot up, so we delay 1.5s here if (!hal.util->was_watchdog_armed()) { @@ -388,8 +394,10 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } #endif } +#endif // AP_RANGEFINDER_LWI2C_ENABLED break; case Type::TRI2C: +#if AP_RANGEFINDER_TRI2C_ENABLED if (params[instance].address) { FOREACH_I2C(i) { if (_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance], @@ -399,15 +407,19 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } } +#endif break; case Type::VL53L0X: case Type::VL53L1X_Short: FOREACH_I2C(i) { +#if AP_RANGEFINDER_VL53L0X_ENABLED if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance], hal.i2c_mgr->get_device(i, params[instance].address)), instance)) { break; } +#endif +#if AP_RANGEFINDER_VL53L1X_ENABLED if (_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance], hal.i2c_mgr->get_device(i, params[instance].address), _type == Type::VL53L1X_Short ? AP_RangeFinder_VL53L1X::DistanceMode::Short : @@ -415,9 +427,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) instance)) { break; } +#endif } break; case Type::BenewakeTFminiPlus: { +#if AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED uint8_t addr = TFMINI_ADDR_DEFAULT; if (params[instance].address != 0) { addr = params[instance].address; @@ -430,62 +444,68 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } break; +#endif } case Type::PX4_PWM: -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -#ifndef HAL_BUILD_AP_PERIPH +#if AP_RANGEFINDER_PWM_ENABLED // to ease moving from PX4 to ChibiOS we'll lie a little about // the backend driver... if (AP_RangeFinder_PWM::detect()) { _add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance); } -#endif #endif break; case Type::BBB_PRU: -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI +#if AP_RANGEFINDER_BBB_PRU_ENABLED if (AP_RangeFinder_BBB_PRU::detect()) { _add_backend(new AP_RangeFinder_BBB_PRU(state[instance], params[instance]), instance); } #endif break; case Type::LWSER: +#if AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED if (AP_RangeFinder_LightWareSerial::detect(serial_instance)) { _add_backend(new AP_RangeFinder_LightWareSerial(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::LEDDARONE: +#if AP_RANGEFINDER_LEDDARONE_ENABLED if (AP_RangeFinder_LeddarOne::detect(serial_instance)) { _add_backend(new AP_RangeFinder_LeddarOne(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::USD1_Serial: +#if AP_RANGEFINDER_USD1_SERIAL_ENABLED if (AP_RangeFinder_USD1_Serial::detect(serial_instance)) { _add_backend(new AP_RangeFinder_USD1_Serial(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::BEBOP: -#if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO) && defined(HAVE_LIBIIO) +#if AP_RANGEFINDER_BEBOP_ENABLED if (AP_RangeFinder_Bebop::detect()) { _add_backend(new AP_RangeFinder_Bebop(state[instance], params[instance]), instance); } #endif break; case Type::MAVLink: -#ifndef HAL_BUILD_AP_PERIPH +#if AP_RANGEFINDER_MAVLINK_ENABLED if (AP_RangeFinder_MAVLink::detect()) { _add_backend(new AP_RangeFinder_MAVLink(state[instance], params[instance]), instance); } #endif break; case Type::MBSER: +#if AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_instance)) { _add_backend(new AP_RangeFinder_MaxsonarSerialLV(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::ANALOG: -#ifndef HAL_BUILD_AP_PERIPH +#if AP_RANGEFINDER_ANALOG_ENABLED // note that analog will always come back as present if the pin is valid if (AP_RangeFinder_analog::detect(params[instance])) { _add_backend(new AP_RangeFinder_analog(state[instance], params[instance]), instance); @@ -493,7 +513,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) #endif break; case Type::HC_SR04: -#ifndef HAL_BUILD_AP_PERIPH +#if AP_RANGEFINDER_HC_SR04_ENABLED // note that this will always come back as present if the pin is valid if (AP_RangeFinder_HC_SR04::detect(params[instance])) { _add_backend(new AP_RangeFinder_HC_SR04(state[instance], params[instance]), instance); @@ -501,55 +521,71 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) #endif break; case Type::NMEA: +#if AP_RANGEFINDER_NMEA_ENABLED if (AP_RangeFinder_NMEA::detect(serial_instance)) { _add_backend(new AP_RangeFinder_NMEA(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::WASP: +#if AP_RANGEFINDER_WASP_ENABLED if (AP_RangeFinder_Wasp::detect(serial_instance)) { _add_backend(new AP_RangeFinder_Wasp(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::BenewakeTF02: +#if AP_RANGEFINDER_BENEWAKE_TF02_ENABLED if (AP_RangeFinder_Benewake_TF02::detect(serial_instance)) { _add_backend(new AP_RangeFinder_Benewake_TF02(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::BenewakeTFmini: +#if AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED if (AP_RangeFinder_Benewake_TFMini::detect(serial_instance)) { _add_backend(new AP_RangeFinder_Benewake_TFMini(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::BenewakeTF03: +#if AP_RANGEFINDER_BENEWAKE_TF03_ENABLED if (AP_RangeFinder_Benewake_TF03::detect(serial_instance)) { _add_backend(new AP_RangeFinder_Benewake_TF03(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::PWM: -#ifndef HAL_BUILD_AP_PERIPH +#if AP_RANGEFINDER_PWM_ENABLED if (AP_RangeFinder_PWM::detect()) { _add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance); } #endif break; case Type::BLPing: +#if AP_RANGEFINDER_BLPING_ENABLED if (AP_RangeFinder_BLPing::detect(serial_instance)) { _add_backend(new AP_RangeFinder_BLPing(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::Lanbao: +#if AP_RANGEFINDER_LANBAO_ENABLED if (AP_RangeFinder_Lanbao::detect(serial_instance)) { _add_backend(new AP_RangeFinder_Lanbao(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::LeddarVu8_Serial: +#if AP_RANGEFINDER_LEDDARVU8_ENABLED if (AP_RangeFinder_LeddarVu8::detect(serial_instance)) { _add_backend(new AP_RangeFinder_LeddarVu8(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::UAVCAN: -#if HAL_CANMANAGER_ENABLED +#if AP_RANGEFINDER_UAVCAN_ENABLED /* the UAVCAN driver gets created when we first receive a measurement. We take the instance slot now, even if we don't @@ -560,13 +596,15 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) break; case Type::GYUS42v2: +#if AP_RANGEFINDER_GYUS42V2_ENABLED if (AP_RangeFinder_GYUS42v2::detect(serial_instance)) { _add_backend(new AP_RangeFinder_GYUS42v2(state[instance], params[instance]), instance, serial_instance++); } +#endif break; case Type::SIM: -#if AP_SIM_RANGEFINDER_ENABLED +#if AP_RANGEFINDER_SIM_ENABLED _add_backend(new AP_RangeFinder_SITL(state[instance], params[instance], instance), instance); #endif break; @@ -579,16 +617,17 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) #endif // HAL_MSP_RANGEFINDER_ENABLED break; -#if HAL_MAX_CAN_PROTOCOL_DRIVERS case Type::USD1_CAN: +#if AP_RANGEFINDER_USD1_CAN_ENABLED _add_backend(new AP_RangeFinder_USD1_CAN(state[instance], params[instance]), instance); +#endif break; case Type::Benewake_CAN: +#if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED _add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance); break; -#endif //HAL_MAX_CAN_PROTOCOL_DRIVERS +#endif case Type::NONE: - default: break; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 7d06daad07..9a85327fe6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -26,6 +26,10 @@ #define AP_RANGEFINDER_ENABLED 1 #endif +#ifndef AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#define AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED AP_RANGEFINDER_ENABLED +#endif + // Maximum number of range finder instances available on this platform #ifndef RANGEFINDER_MAX_INSTANCES #if AP_RANGEFINDER_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp index dca89c814d..d06b99b209 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp @@ -16,11 +16,10 @@ by Mirko Denecke */ -#include -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI - #include "AP_RangeFinder_BBB_PRU.h" +#if AP_RANGEFINDER_BBB_PRU_ENABLED + #include #include #include @@ -105,4 +104,4 @@ void AP_RangeFinder_BBB_PRU::update(void) state.distance_m = rangerpru->distance * 0.01f; state.last_reading_ms = AP_HAL::millis(); } -#endif // CONFIG_HAL_BOARD_SUBTYPE +#endif // AP_RANGEFINDER_BBB_PRU_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h index 0335928d02..ddff67dda2 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h @@ -3,6 +3,16 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend.h" +#include + +#ifndef AP_RANGEFINDER_BBB_PRU_ENABLED +#define AP_RANGEFINDER_BBB_PRU_ENABLED ( \ + AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI \ + ) +#endif + +#if AP_RANGEFINDER_BBB_PRU_ENABLED #define PRU0_CTRL_BASE 0x4a322000 @@ -43,3 +53,5 @@ protected: private: }; + +#endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp index bfc87f00d6..54036e2bba 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp @@ -13,10 +13,12 @@ along with this program. If not, see . */ -#include -#include #include "AP_RangeFinder_BLPing.h" +#if AP_RANGEFINDER_BLPING_ENABLED + +#include + void AP_RangeFinder_BLPing::update(void) { if (uart == nullptr) { @@ -226,3 +228,5 @@ PingProtocol::MessageId PingProtocol::parse_byte(uint8_t b) return msg.done ? get_message_id() : MessageId::INVALID; } + +#endif // AP_RANGEFINDER_BLPING_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h index 92d9fed435..9bd73113ef 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h @@ -1,6 +1,13 @@ #pragma once #include "AP_RangeFinder.h" + +#ifndef AP_RANGEFINDER_BLPING_ENABLED +#define AP_RANGEFINDER_BLPING_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_BLPING_ENABLED + #include "AP_RangeFinder_Backend_Serial.h" /** @@ -170,3 +177,5 @@ private: */ uint32_t last_init_ms; }; + +#endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp index 448e0b8e65..5d1dd73ff4 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp @@ -12,13 +12,14 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . */ + +#include "AP_RangeFinder_Bebop.h" + +#if AP_RANGEFINDER_BEBOP_ENABLED + #include #include -#if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO) && \ - defined(HAVE_LIBIIO) - #include #include #include @@ -32,7 +33,6 @@ #include #include #include -#include "AP_RangeFinder_Bebop.h" #include #include @@ -474,4 +474,5 @@ int AP_RangeFinder_Bebop::_update_mode(float altitude) } return _mode; } -#endif + +#endif // AP_RANGEFINDER_BEBOP_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h index cc7302183b..fa1c062203 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h @@ -16,6 +16,20 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend.h" + +#include +#include + +#ifndef AP_RANGEFINDER_BEBOP_ENABLED +#define AP_RANGEFINDER_BEBOP_ENABLED \ + AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && \ + (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO) && \ + defined(HAVE_LIBIIO) +#endif + +#if AP_RANGEFINDER_BEBOP_ENABLED + #include /* @@ -143,3 +157,5 @@ private: int16_t _last_min_distance_cm = 32; }; + +#endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index fed7778c48..6ffb9ef2cf 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -15,6 +15,8 @@ #include "AP_RangeFinder_Benewake.h" +#if AP_RANGEFINDER_BENEWAKE_ENABLED + #include #include @@ -132,3 +134,5 @@ bool AP_RangeFinder_Benewake::get_reading(float &reading_m) // no readings so return false return false; } + +#endif // AP_RANGEFINDER_BENEWAKE_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h index 4dbe430d07..a52f042f77 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h @@ -1,6 +1,13 @@ #pragma once #include "AP_RangeFinder.h" + +#ifndef AP_RANGEFINDER_BENEWAKE_ENABLED +#define AP_RANGEFINDER_BENEWAKE_ENABLED AP_RANGEFINDER_ENABLED +#endif + +#if AP_RANGEFINDER_BENEWAKE_ENABLED + #include "AP_RangeFinder_Backend_Serial.h" class AP_RangeFinder_Benewake : public AP_RangeFinder_Backend_Serial @@ -28,3 +35,5 @@ private: uint8_t linebuf[10]; uint8_t linebuf_len; }; + +#endif // AP_RANGEFINDER_BENEWAKE_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp index fa0c1032b5..b42b7a5320 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp @@ -2,7 +2,7 @@ #include #include "AP_RangeFinder_Benewake_CAN.h" -#if HAL_MAX_CAN_PROTOCOL_DRIVERS +#if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED const AP_Param::GroupInfo AP_RangeFinder_Benewake_CAN::var_info[] = { @@ -104,4 +104,4 @@ void Benewake_MultiCAN::handle_frame(AP_HAL::CANFrame &frame) } } -#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS +#endif // AP_RANGEFINDER_BENEWAKE_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h index b72aa30d55..2b1100608f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h @@ -1,9 +1,14 @@ #pragma once #include "AP_RangeFinder_Backend.h" + #include -#if HAL_MAX_CAN_PROTOCOL_DRIVERS +#ifndef AP_RANGEFINDER_BENEWAKE_CAN_ENABLED +#define AP_RANGEFINDER_BENEWAKE_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED class Benewake_MultiCAN; @@ -52,6 +57,6 @@ public: AP_RangeFinder_Benewake_CAN *drivers; }; -#endif //HAL_MAX_CAN_PROTOCOL_DRIVERS +#endif // AP_RANGEFINDER_BENEWAKE_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF02.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF02.h index 89af15632c..b188ec1a43 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF02.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF02.h @@ -2,6 +2,12 @@ #include "AP_RangeFinder_Benewake.h" +#ifndef AP_RANGEFINDER_BENEWAKE_TF02_ENABLED +#define AP_RANGEFINDER_BENEWAKE_TF02_ENABLED (AP_RANGEFINDER_BENEWAKE_ENABLED && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#if AP_RANGEFINDER_BENEWAKE_TF02_ENABLED + class AP_RangeFinder_Benewake_TF02 : public AP_RangeFinder_Benewake { public: @@ -11,3 +17,5 @@ protected: float model_dist_max_cm() const override { return 2200; } bool has_signal_byte() const override { return true; } }; + +#endif // AP_RANGEFINDER_BENEWAKE_TF02_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF03.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF03.h index 38f5746fac..3ba936cab9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF03.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF03.h @@ -2,6 +2,12 @@ #include "AP_RangeFinder_Benewake.h" +#ifndef AP_RANGEFINDER_BENEWAKE_TF03_ENABLED +#define AP_RANGEFINDER_BENEWAKE_TF03_ENABLED (AP_RANGEFINDER_BENEWAKE_ENABLED && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#if AP_RANGEFINDER_BENEWAKE_TF03_ENABLED + class AP_RangeFinder_Benewake_TF03 : public AP_RangeFinder_Benewake { public: @@ -10,3 +16,5 @@ public: protected: float model_dist_max_cm() const override { return 18000; } }; + +#endif // AP_RANGEFINDER_BENEWAKE_TF03_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMini.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMini.h index b2b39704cb..901d5f976a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMini.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMini.h @@ -2,6 +2,12 @@ #include "AP_RangeFinder_Benewake.h" +#ifndef AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED +#define AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED (AP_RANGEFINDER_BENEWAKE_ENABLED && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#if AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED + #define TFMINI_ADDR_DEFAULT 0x10 // TFMini default device id class AP_RangeFinder_Benewake_TFMini : public AP_RangeFinder_Benewake @@ -12,3 +18,5 @@ public: protected: float model_dist_max_cm() const override { return 1200; } }; + +#endif // AP_RANGEFINDER_BENEWAKE_TFMINI diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp index 75ca56277e..fc4e227c74 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp @@ -16,6 +16,8 @@ */ #include "AP_RangeFinder_Benewake_TFMiniPlus.h" +#if AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED + #include #include @@ -219,3 +221,5 @@ void AP_RangeFinder_Benewake_TFMiniPlus::timer() accum.count++; } } + +#endif // AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h index b19a44c087..3074504c92 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h @@ -19,6 +19,12 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend.h" +#ifndef AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED +#define AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED + #include #include @@ -60,3 +66,5 @@ private: uint32_t count; } accum; }; + +#endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp index 8132235bb9..36bac11572 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp @@ -13,11 +13,9 @@ along with this program. If not, see . */ -#include #include "AP_RangeFinder_GYUS42v2.h" -#include -extern const AP_HAL::HAL& hal; +#if AP_RANGEFINDER_GYUS42V2_ENABLED bool AP_RangeFinder_GYUS42v2::find_signature_in_buffer(uint8_t start) { @@ -70,3 +68,5 @@ bool AP_RangeFinder_GYUS42v2::get_reading(float &reading_m) return true; } + +#endif // AP_RANGEFINDER_GYUS42V2_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h b/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h index 3403b86d59..dc23122bc2 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h @@ -1,6 +1,13 @@ #pragma once #include "AP_RangeFinder.h" + +#ifndef AP_RANGEFINDER_GYUS42V2_ENABLED +#define AP_RANGEFINDER_GYUS42V2_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_GYUS42V2_ENABLED + #include "AP_RangeFinder_Backend_Serial.h" class AP_RangeFinder_GYUS42v2 : public AP_RangeFinder_Backend_Serial @@ -32,3 +39,5 @@ private: uint8_t buffer[7]; uint8_t buffer_used; }; + +#endif // AP_RANGEFINDER_GYUS42V2_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp index 877fba0d92..6e7cc156ec 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp @@ -25,10 +25,13 @@ * The second pin we use for triggering the ultransonic pulse */ +#include "AP_RangeFinder_HC_SR04.h" + +#if AP_RANGEFINDER_HC_SR04_ENABLED + #include #include "AP_RangeFinder.h" #include "AP_RangeFinder_Params.h" -#include "AP_RangeFinder_HC_SR04.h" #include @@ -140,3 +143,4 @@ void AP_RangeFinder_HC_SR04::update(void) } } +#endif // AP_RANGEFINDER_HC_SR04_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h b/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h index 5c8abe4589..5ef460f6bb 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h @@ -2,6 +2,13 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend.h" + +#ifndef AP_RANGEFINDER_HC_SR04_ENABLED +#define AP_RANGEFINDER_HC_SR04_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_HC_SR04_ENABLED + #include "AP_RangeFinder_Params.h" class AP_RangeFinder_HC_SR04 : public AP_RangeFinder_Backend @@ -37,3 +44,5 @@ private: uint32_t last_ping_ms; }; + +#endif // AP_RANGEFINDER_HC_SR04_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp index 6fcfc85116..d59b030e50 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp @@ -15,8 +15,11 @@ /* driver for Lanbao PSK-CM8JL65-CC5 Lidar */ -#include #include "AP_RangeFinder_Lanbao.h" + +#if AP_RANGEFINDER_LANBAO_ENABLED + +#include #include #include @@ -80,3 +83,5 @@ bool AP_RangeFinder_Lanbao::get_reading(float &reading_m) } return false; } + +#endif // AP_RANGEFINDER_LANBAO_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h index 86c85ff867..1f9a2fb8ee 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h @@ -3,6 +3,12 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend_Serial.h" +#ifndef AP_RANGEFINDER_LANBAO_ENABLED +#define AP_RANGEFINDER_LANBAO_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_LANBAO_ENABLED + class AP_RangeFinder_Lanbao : public AP_RangeFinder_Backend_Serial { @@ -28,3 +34,5 @@ private: uint8_t buf[6]; uint8_t buf_len = 0; }; + +#endif // AP_RANGEFINDER_LANBAO_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp index ce9ca93989..759c35e205 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp @@ -15,6 +15,8 @@ #include "AP_RangeFinder_LeddarOne.h" +#if AP_RANGEFINDER_LEDDARONE_ENABLED + #include #include @@ -183,3 +185,5 @@ LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detect return LEDDARONE_STATE_OK; } + +#endif // AP_RANGEFINDER_LEDDARONE_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h index 082a46f3f2..4dd68e930e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h @@ -3,6 +3,12 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend_Serial.h" +#ifndef AP_RANGEFINDER_LEDDARONE_ENABLED +#define AP_RANGEFINDER_LEDDARONE_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_LEDDARONE_ENABLED + // defines #define LEDDARONE_DEFAULT_ADDRESS 0x01 #define LEDDARONE_MODOBUS_FUNCTION_CODE 0x04 @@ -83,3 +89,5 @@ private: 0x09 // CRC Hi }; }; + +#endif // AP_RANGEFINDER_LEDDARONE_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp index b903a904f5..04e38624e0 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp @@ -205,4 +205,4 @@ bool AP_RangeFinder_LeddarVu8::parse_byte(uint8_t b, bool &valid_reading, uint16 return false; } -#endif +#endif // AP_RANGEFINDER_LEDDARVU8_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h index 78f99f5d4a..a0aaa55e90 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h @@ -4,7 +4,7 @@ #include "AP_RangeFinder_Backend_Serial.h" #ifndef AP_RANGEFINDER_LEDDARVU8_ENABLED -#define AP_RANGEFINDER_LEDDARVU8_ENABLED 1 +#define AP_RANGEFINDER_LEDDARVU8_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED #endif #if AP_RANGEFINDER_LEDDARVU8_ENABLED @@ -97,4 +97,4 @@ private: uint32_t last_distance_request_ms; // system time of last request to sensor to send distances }; -#endif +#endif // AP_RANGEFINDER_LEDDARVU8_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp index d8d5ac2acf..725b76f4f6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp @@ -13,6 +13,9 @@ along with this program. If not, see . */ #include "AP_RangeFinder_LightWareI2C.h" + +#if AP_RANGEFINDER_LWI2C_ENABLED + #include #include @@ -500,3 +503,5 @@ bool AP_RangeFinder_LightWareI2C::sf20_wait_on_reply(uint8_t *rx_two_byte) } return false; } + +#endif // AP_RANGEFINDER_LWI2C_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h index fd75e483c2..ebc5e760ce 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h @@ -2,6 +2,13 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend.h" + +#ifndef AP_RANGEFINDER_LWI2C_ENABLED +#define AP_RANGEFINDER_LWI2C_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_LWI2C_ENABLED + #include #define NUM_SF20_DATA_STREAMS 1 @@ -58,3 +65,5 @@ private: void data_log(uint16_t *val); AP_HAL::OwnPtr _dev; }; + +#endif // AP_RANGEFINDER_LWI2C_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp index 79a1e2055c..9636b47bf2 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp @@ -15,6 +15,8 @@ #include "AP_RangeFinder_LightWareSerial.h" +#if AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED + #include #include @@ -149,3 +151,5 @@ bool AP_RangeFinder_LightWareSerial::is_lost_signal_distance(int16_t distance_cm } return false; } + +#endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h index 590f1c55e7..7d9a164163 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h @@ -3,6 +3,12 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend_Serial.h" +#ifndef AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED +#define AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED + class AP_RangeFinder_LightWareSerial : public AP_RangeFinder_Backend_Serial { @@ -43,3 +49,5 @@ private: bool no_signal = false; }; + +#endif // AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp index a441e017b9..5e52a9a222 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp @@ -14,6 +14,9 @@ */ #include "AP_RangeFinder_MAVLink.h" + +#if AP_RANGEFINDER_MAVLINK_ENABLED + #include /* @@ -73,3 +76,5 @@ void AP_RangeFinder_MAVLink::update(void) update_status(); } } + +#endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h index ba39e1f5af..9b66fb6b3f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h @@ -3,6 +3,12 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend.h" +#ifndef AP_RANGEFINDER_MAVLINK_ENABLED +#define AP_RANGEFINDER_MAVLINK_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_MAVLINK_ENABLED + // Data timeout #define AP_RANGEFINDER_MAVLINK_TIMEOUT_MS 500 @@ -45,3 +51,5 @@ private: MAV_DISTANCE_SENSOR sensor_type = MAV_DISTANCE_SENSOR_UNKNOWN; }; + +#endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp index 020c0e6e71..ed6e1d722f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp @@ -23,6 +23,8 @@ */ #include "AP_RangeFinder_MaxsonarI2CXL.h" +#if AP_RANGEFINDER_MAXSONARI2CXL_ENABLED + #include #include @@ -151,3 +153,5 @@ void AP_RangeFinder_MaxsonarI2CXL::update(void) set_status(RangeFinder::Status::NoData); } } + +#endif // AP_RANGEFINDER_MAXSONARI2CXL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h index 48bbf07424..7cae78c326 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h @@ -2,6 +2,13 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend.h" + +#ifndef AP_RANGEFINDER_MAXSONARI2CXL_ENABLED +#define AP_RANGEFINDER_MAXSONARI2CXL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_MAXSONARI2CXL_ENABLED + #include #define AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR 0x70 @@ -41,3 +48,5 @@ private: bool get_reading(uint16_t &reading_cm); AP_HAL::OwnPtr _dev; }; + +#endif // AP_RANGEFINDER_MAXSONARI2CXL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp index e67ca9e7ce..7f0173947c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp @@ -17,6 +17,8 @@ #include "AP_RangeFinder_MaxsonarSerialLV.h" +#if AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED + #include #include @@ -68,3 +70,5 @@ bool AP_RangeFinder_MaxsonarSerialLV::get_reading(float &reading_m) return true; } + +#endif // AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h index ddb6c50251..7608aa76f6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h @@ -3,6 +3,12 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend_Serial.h" +#ifndef AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED +#define AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED + class AP_RangeFinder_MaxsonarSerialLV : public AP_RangeFinder_Backend_Serial { @@ -25,3 +31,5 @@ private: char linebuf[10]; uint8_t linebuf_len = 0; }; + +#endif // AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index 02f5f18c54..61945ca7c3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -15,6 +15,8 @@ #include "AP_RangeFinder_NMEA.h" +#if AP_RANGEFINDER_NMEA_ENABLED + #include #include @@ -187,3 +189,5 @@ bool AP_RangeFinder_NMEA::decode_latest_term() return false; } + +#endif // AP_RANGEFINDER_NMEA_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h index ccb1220320..45efb1f628 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h @@ -18,6 +18,12 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend_Serial.h" +#ifndef AP_RANGEFINDER_NMEA_ENABLED +#define AP_RANGEFINDER_NMEA_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_NMEA_ENABLED + class AP_RangeFinder_NMEA : public AP_RangeFinder_Backend_Serial { @@ -72,3 +78,5 @@ private: sentence_types _sentence_type; // the sentence type currently being processed bool _sentence_done; // true if this sentence has already been decoded }; + +#endif // AP_RANGEFINDER_NMEA_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp index f8aedca854..97352b2d4a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp @@ -15,6 +15,8 @@ #include "AP_RangeFinder_PWM.h" +#if AP_RANGEFINDER_PWM_ENABLED + #include #include @@ -130,3 +132,5 @@ void AP_RangeFinder_PWM::update(void) bool AP_RangeFinder_PWM::out_of_range(void) const { return params.powersave_range > 0 && estimated_terrain_height > params.powersave_range; } + +#endif // AP_RANGEFINDER_PWM_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h index 7e64a54ae3..597861d1d0 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.h @@ -17,6 +17,14 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend.h" +#ifndef AP_RANGEFINDER_PWM_ENABLED +#define AP_RANGEFINDER_PWM_ENABLED \ + (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && \ + AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#if AP_RANGEFINDER_PWM_ENABLED + class AP_RangeFinder_PWM : public AP_RangeFinder_Backend { public: @@ -58,3 +66,5 @@ private: bool was_out_of_range = -1; // this odd initialisation ensures we transition to new state }; + +#endif // AP_RANGEFINDER_PWM_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 89009f6775..608f2492ef 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -14,6 +14,8 @@ */ #include "AP_RangeFinder_PulsedLightLRF.h" +#if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED + #include #include @@ -218,3 +220,4 @@ failed: return false; } +#endif // AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h index 4a02a8a783..8f6d40e200 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h @@ -2,6 +2,13 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend.h" + +#ifndef AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED +#define AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED + #include /* Connection diagram @@ -57,3 +64,5 @@ private: enum { PHASE_MEASURE, PHASE_COLLECT } phase; }; + +#endif // AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp index 561b7b2f45..2e6e3613d8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp @@ -12,15 +12,12 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . */ - #include "AP_RangeFinder_SITL.h" -#if AP_SIM_RANGEFINDER_ENABLED +#if AP_RANGEFINDER_SIM_ENABLED #include -extern const AP_HAL::HAL& hal; - /* constructor - registers instance at top RangeFinder driver */ @@ -50,5 +47,4 @@ void AP_RangeFinder_SITL::update(void) update_status(); } -#endif // AP_SIM_RANGEFINDER_ENABLED - +#endif // AP_RANGEFINDER_SIM_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SITL.h b/libraries/AP_RangeFinder/AP_RangeFinder_SITL.h index 83c59706d0..e11763a26f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SITL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SITL.h @@ -16,13 +16,13 @@ #include -#ifndef AP_SIM_RANGEFINDER_ENABLED -#define AP_SIM_RANGEFINDER_ENABLED (AP_SIM_ENABLED && !defined(HAL_BUILD_AP_PERIPH)) +#include "AP_RangeFinder_Backend.h" + +#ifndef AP_RANGEFINDER_SIM_ENABLED +#define AP_RANGEFINDER_SIM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) #endif -#if AP_SIM_RANGEFINDER_ENABLED - -#include "AP_RangeFinder_Backend.h" +#if AP_RANGEFINDER_SIM_ENABLED #include @@ -47,4 +47,4 @@ private: }; -#endif // AP_SIM_RANGEFINDER_ENABLED +#endif // AP_RANGEFINDER_SIM_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp index 312e9fd97d..c1772bfbad 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp @@ -17,6 +17,8 @@ */ #include "AP_RangeFinder_TeraRangerI2C.h" +#if AP_RANGEFINDER_TRI2C_ENABLED + #include #include #include @@ -189,3 +191,5 @@ void AP_RangeFinder_TeraRangerI2C::update(void) set_status(RangeFinder::Status::NoData); } } + +#endif // AP_RANGEFINDER_TRI2C_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h index 2af3630ac4..09bdb5f0a7 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h @@ -2,6 +2,13 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend.h" + +#ifndef AP_RANGEFINDER_TRI2C_ENABLED +#define AP_RANGEFINDER_TRI2C_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_TRI2C_ENABLED + #include class AP_RangeFinder_TeraRangerI2C : public AP_RangeFinder_Backend @@ -40,3 +47,5 @@ private: uint32_t count; } accum; }; + +#endif // AP_RANGEFINDER_TRI2C_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp index 252e672058..bf6f4e857d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp @@ -1,9 +1,8 @@ -#include - -#if HAL_CANMANAGER_ENABLED - #include "AP_RangeFinder_UAVCAN.h" +#if AP_RANGEFINDER_UAVCAN_ENABLED + +#include #include #include @@ -173,5 +172,4 @@ void AP_RangeFinder_UAVCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t nod } } -#endif // HAL_CANMANAGER_ENABLED - +#endif // AP_RANGEFINDER_UAVCAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h index 2f48c260c8..5a92822744 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h @@ -2,7 +2,12 @@ #include "AP_RangeFinder_Backend.h" -#if HAL_CANMANAGER_ENABLED +#ifndef AP_RANGEFINDER_UAVCAN_ENABLED +#define AP_RANGEFINDER_UAVCAN_ENABLED (HAL_CANMANAGER_ENABLED && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#if AP_RANGEFINDER_UAVCAN_ENABLED + #include class MeasurementCb; @@ -34,4 +39,4 @@ private: bool new_data; MAV_DISTANCE_SENSOR _sensor_type; }; -#endif //HAL_CANMANAGER_ENABLED +#endif // AP_RANGEFINDER_UAVCAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp index dd74fda2ba..4928a93e99 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp @@ -1,7 +1,8 @@ -#include #include "AP_RangeFinder_USD1_CAN.h" -#if HAL_MAX_CAN_PROTOCOL_DRIVERS +#if AP_RANGEFINDER_USD1_CAN_ENABLED + +#include /* constructor @@ -39,4 +40,4 @@ void AP_RangeFinder_USD1_CAN::handle_frame(AP_HAL::CANFrame &frame) _distance_count++; } -#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS +#endif // AP_RANGEFINDER_USD1_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h index 761f72b8fd..874e44ebea 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h @@ -3,7 +3,11 @@ #include "AP_RangeFinder_Backend.h" #include -#if HAL_MAX_CAN_PROTOCOL_DRIVERS +#ifndef AP_RANGEFINDER_USD1_CAN_ENABLED +#define AP_RANGEFINDER_USD1_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#if AP_RANGEFINDER_USD1_CAN_ENABLED class AP_RangeFinder_USD1_CAN : public CANSensor, public AP_RangeFinder_Backend { public: @@ -22,5 +26,5 @@ private: float _distance_sum; uint32_t _distance_count; }; -#endif //HAL_MAX_CAN_PROTOCOL_DRIVERS +#endif // AP_RANGEFINDER_USD1_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp index 16dec00a9a..04d8c06329 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp @@ -13,15 +13,15 @@ along with this program. If not, see . */ -#include #include "AP_RangeFinder_USD1_Serial.h" + +#if AP_RANGEFINDER_USD1_SERIAL_ENABLED + #include #define USD1_HDR 254 // Header Byte from USD1_Serial (0xFE) #define USD1_HDR_V0 72 // Header Byte for beta V0 of USD1_Serial (0x48) -extern const AP_HAL::HAL& hal; - /* detect USD1_Serial Firmware Version */ @@ -173,3 +173,5 @@ bool AP_RangeFinder_USD1_Serial::get_reading(float &reading_m) return true; } + +#endif // AP_RANGEFINDER_USD1_SERIAL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.h index 042ffba3a0..faa8a5c9d6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.h @@ -3,6 +3,12 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend_Serial.h" +#ifndef AP_RANGEFINDER_USD1_SERIAL_ENABLED +#define AP_RANGEFINDER_USD1_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_USD1_SERIAL_ENABLED + class AP_RangeFinder_USD1_Serial : public AP_RangeFinder_Backend_Serial { @@ -37,3 +43,5 @@ private: uint8_t _header; uint8_t _version; }; + +#endif // AP_RANGEFINDER_USD1_SERIAL_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp index 05b748f2fd..51c10dec8c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp @@ -20,6 +20,8 @@ */ #include "AP_RangeFinder_VL53L0X.h" +#if AP_RANGEFINDER_VL53L0X_ENABLED + #include #include @@ -780,3 +782,5 @@ void AP_RangeFinder_VL53L0X::timer(void) counter++; } } + +#endif // AP_RANGEFINDER_VL53L0X_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h index 1f0933dae4..9ffa4f96e5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h @@ -4,6 +4,12 @@ #include "AP_RangeFinder_Backend.h" #include +#ifndef AP_RANGEFINDER_VL53L0X_ENABLED +#define AP_RANGEFINDER_VL53L0X_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_VL53L0X_ENABLED + class AP_RangeFinder_VL53L0X : public AP_RangeFinder_Backend { @@ -80,3 +86,5 @@ private: uint32_t sum_mm; uint32_t counter; }; + +#endif // AP_RANGEFINDER_VL53L0X_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp index c3aa6d4842..e0eba359ee 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp @@ -20,6 +20,8 @@ */ #include "AP_RangeFinder_VL53L1X.h" +#if AP_RANGEFINDER_VL53L1X_ENABLED + #include #include @@ -578,3 +580,5 @@ void AP_RangeFinder_VL53L1X::update(void) set_status(RangeFinder::Status::NoData); } } + +#endif // AP_RANGEFINDER_VL53L1X_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h index 5cd6efe58e..f073781d4c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h @@ -2,6 +2,13 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend.h" + +#ifndef AP_RANGEFINDER_VL53L1X_ENABLED +#define AP_RANGEFINDER_VL53L1X_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_VL53L1X_ENABLED + #include class AP_RangeFinder_VL53L1X : public AP_RangeFinder_Backend @@ -1293,3 +1300,5 @@ private: uint32_t calcMacroPeriod(uint8_t vcsel_period) const; bool setupManualCalibration(void); }; + +#endif // AP_RANGEFINDER_VL53L1X_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp index 36fed34a8a..cda423c79b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp @@ -15,6 +15,8 @@ #include "AP_RangeFinder_Wasp.h" +#if AP_RANGEFINDER_WASP_ENABLED + #include #include @@ -247,3 +249,4 @@ void AP_RangeFinder_Wasp::parse_response(void) { } } +#endif // AP_RANGEFINDER_WASP_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h index a58f779ff6..06154f255a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h @@ -3,6 +3,12 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend_Serial.h" +#ifndef AP_RANGEFINDER_WASP_ENABLED +#define AP_RANGEFINDER_WASP_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_WASP_ENABLED + // WASP 200 LRF // http://www.attolloengineering.com/wasp-200-lrf.html @@ -59,3 +65,5 @@ private: AP_Int16 thr; AP_Int8 baud; }; + +#endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp index 33cd9156a0..b9ffb1f06c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp @@ -18,12 +18,15 @@ * */ +#include "AP_RangeFinder_analog.h" + +#if AP_RANGEFINDER_ANALOG_ENABLED + #include #include #include #include "AP_RangeFinder.h" #include "AP_RangeFinder_Params.h" -#include "AP_RangeFinder_analog.h" extern const AP_HAL::HAL& hal; @@ -118,3 +121,4 @@ void AP_RangeFinder_analog::update(void) update_status(); } +#endif // AP_RANGEFINDER_ANALOG_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.h b/libraries/AP_RangeFinder/AP_RangeFinder_analog.h index b102a2d3fa..052782a0dd 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.h @@ -4,6 +4,12 @@ #include "AP_RangeFinder_Backend.h" #include "AP_RangeFinder_Params.h" +#ifndef AP_RANGEFINDER_ANALOG_ENABLED +#define AP_RANGEFINDER_ANALOG_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_ANALOG_ENABLED + class AP_RangeFinder_analog : public AP_RangeFinder_Backend { public: @@ -28,3 +34,5 @@ private: AP_HAL::AnalogSource *source; }; + +#endif