From 26e6a11ba39ce724c72dc7471f2bb3f1ec7b100c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Sep 2023 19:53:57 +1000 Subject: [PATCH] AP_RangeFinder: remove more code based on defines --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 160 +++++++++--------- libraries/AP_RangeFinder/AP_RangeFinder.h | 76 +++++++++ .../AP_RangeFinder_USD1_Serial.h | 10 +- .../AP_RangeFinder/AP_RangeFinder_config.h | 4 + 4 files changed, 168 insertions(+), 82 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 425ae116d7562..5636656604b35 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -26,6 +26,8 @@ defined(HAVE_LIBIIO) #include "AP_RangeFinder_Bebop.h" #endif +#include "AP_RangeFinder_Backend.h" +#include "AP_RangeFinder_Backend_Serial.h" #include "AP_RangeFinder_MAVLink.h" #include "AP_RangeFinder_LeddarOne.h" #include "AP_RangeFinder_USD1_Serial.h" @@ -271,20 +273,20 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) const Type _type = (Type)params[instance].type.get(); switch (_type) { +#if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED 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: { +#endif #if AP_RANGEFINDER_MAXSONARI2CXL_ENABLED + case Type::MBI2C: { uint8_t addr = AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR; if (params[instance].address != 0) { addr = params[instance].address; @@ -297,10 +299,10 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } break; -#endif } - case Type::LWI2C: +#endif #if AP_RANGEFINDER_LWI2C_ENABLED + case Type::LWI2C: if (params[instance].address) { // the LW20 needs a long time to boot up, so we delay 1.5s here #ifndef HAL_BUILD_AP_PERIPH @@ -322,10 +324,10 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } #endif } -#endif // AP_RANGEFINDER_LWI2C_ENABLED break; - case Type::TRI2C: +#endif // AP_RANGEFINDER_LWI2C_ENABLED #if AP_RANGEFINDER_TRI2C_ENABLED + case Type::TRI2C: if (params[instance].address) { FOREACH_I2C(i) { if (_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance], @@ -335,8 +337,8 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } } -#endif break; +#endif case Type::VL53L0X: case Type::VL53L1X_Short: FOREACH_I2C(i) { @@ -358,8 +360,8 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) #endif } break; - case Type::BenewakeTFminiPlus: { #if AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED + case Type::BenewakeTFminiPlus: { uint8_t addr = TFMINIPLUS_ADDR_DEFAULT; if (params[instance].address != 0) { addr = params[instance].address; @@ -372,193 +374,193 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } break; -#endif } - case Type::PX4_PWM: +#endif #if AP_RANGEFINDER_PWM_ENABLED + case Type::PX4_PWM: // 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 break; - case Type::BBB_PRU: +#endif #if AP_RANGEFINDER_BBB_PRU_ENABLED + case Type::BBB_PRU: if (AP_RangeFinder_BBB_PRU::detect()) { _add_backend(new AP_RangeFinder_BBB_PRU(state[instance], params[instance]), instance); } -#endif break; - case Type::LWSER: +#endif #if AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED + case Type::LWSER: serial_create_fn = AP_RangeFinder_LightWareSerial::create; -#endif break; - case Type::LEDDARONE: +#endif #if AP_RANGEFINDER_LEDDARONE_ENABLED + case Type::LEDDARONE: serial_create_fn = AP_RangeFinder_LeddarOne::create; -#endif break; - case Type::USD1_Serial: +#endif #if AP_RANGEFINDER_USD1_SERIAL_ENABLED + case Type::USD1_Serial: serial_create_fn = AP_RangeFinder_USD1_Serial::create; -#endif break; - case Type::BEBOP: +#endif #if AP_RANGEFINDER_BEBOP_ENABLED + case Type::BEBOP: if (AP_RangeFinder_Bebop::detect()) { _add_backend(new AP_RangeFinder_Bebop(state[instance], params[instance]), instance); } -#endif break; - case Type::MAVLink: +#endif #if AP_RANGEFINDER_MAVLINK_ENABLED + case Type::MAVLink: if (AP_RangeFinder_MAVLink::detect()) { _add_backend(new AP_RangeFinder_MAVLink(state[instance], params[instance]), instance); } -#endif break; - case Type::MBSER: +#endif #if AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED + case Type::MBSER: serial_create_fn = AP_RangeFinder_MaxsonarSerialLV::create; -#endif break; - case Type::ANALOG: +#endif #if AP_RANGEFINDER_ANALOG_ENABLED + case Type::ANALOG: // 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); } -#endif break; - case Type::HC_SR04: +#endif #if AP_RANGEFINDER_HC_SR04_ENABLED + case Type::HC_SR04: // 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); } -#endif break; - case Type::NMEA: +#endif #if AP_RANGEFINDER_NMEA_ENABLED + case Type::NMEA: serial_create_fn = AP_RangeFinder_NMEA::create; -#endif break; - case Type::WASP: +#endif #if AP_RANGEFINDER_WASP_ENABLED + case Type::WASP: serial_create_fn = AP_RangeFinder_Wasp::create; -#endif break; - case Type::BenewakeTF02: +#endif #if AP_RANGEFINDER_BENEWAKE_TF02_ENABLED + case Type::BenewakeTF02: serial_create_fn = AP_RangeFinder_Benewake_TF02::create; -#endif break; - case Type::BenewakeTFmini: +#endif #if AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED + case Type::BenewakeTFmini: serial_create_fn = AP_RangeFinder_Benewake_TFMini::create; -#endif break; - case Type::BenewakeTF03: +#endif #if AP_RANGEFINDER_BENEWAKE_TF03_ENABLED + case Type::BenewakeTF03: serial_create_fn = AP_RangeFinder_Benewake_TF03::create; -#endif break; - case Type::TeraRanger_Serial: +#endif #if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED + case Type::TeraRanger_Serial: serial_create_fn = AP_RangeFinder_TeraRanger_Serial::create; -#endif break; - case Type::PWM: +#endif #if AP_RANGEFINDER_PWM_ENABLED + case Type::PWM: if (AP_RangeFinder_PWM::detect()) { _add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance); } -#endif break; - case Type::BLPing: +#endif #if AP_RANGEFINDER_BLPING_ENABLED + case Type::BLPing: serial_create_fn = AP_RangeFinder_BLPing::create; -#endif break; - case Type::Lanbao: +#endif #if AP_RANGEFINDER_LANBAO_ENABLED + case Type::Lanbao: serial_create_fn = AP_RangeFinder_Lanbao::create; -#endif break; - case Type::LeddarVu8_Serial: +#endif #if AP_RANGEFINDER_LEDDARVU8_ENABLED + case Type::LeddarVu8_Serial: serial_create_fn = AP_RangeFinder_LeddarVu8::create; -#endif break; +#endif - case Type::UAVCAN: #if AP_RANGEFINDER_DRONECAN_ENABLED + case Type::UAVCAN: /* the UAVCAN driver gets created when we first receive a measurement. We take the instance slot now, even if we don't yet have the driver */ num_instances = MAX(num_instances, instance+1); -#endif break; +#endif - case Type::GYUS42v2: #if AP_RANGEFINDER_GYUS42V2_ENABLED + case Type::GYUS42v2: serial_create_fn = AP_RangeFinder_GYUS42v2::create; -#endif break; +#endif - case Type::SIM: #if AP_RANGEFINDER_SIM_ENABLED + case Type::SIM: _add_backend(new AP_RangeFinder_SITL(state[instance], params[instance], instance), instance); -#endif break; +#endif - case Type::MSP: #if HAL_MSP_RANGEFINDER_ENABLED + case Type::MSP: if (AP_RangeFinder_MSP::detect()) { _add_backend(new AP_RangeFinder_MSP(state[instance], params[instance]), instance); } -#endif // HAL_MSP_RANGEFINDER_ENABLED break; +#endif // HAL_MSP_RANGEFINDER_ENABLED - case Type::USD1_CAN: #if AP_RANGEFINDER_USD1_CAN_ENABLED + case Type::USD1_CAN: _add_backend(new AP_RangeFinder_USD1_CAN(state[instance], params[instance]), instance); -#endif break; - case Type::Benewake_CAN: +#endif #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED + case Type::Benewake_CAN: _add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance); -#endif break; +#endif - case Type::Lua_Scripting: #if AP_RANGEFINDER_LUA_ENABLED + case Type::Lua_Scripting: _add_backend(new AP_RangeFinder_Lua(state[instance], params[instance]), instance); -#endif break; +#endif - case Type::NoopLoop_P: #if AP_RANGEFINDER_NOOPLOOP_ENABLED + case Type::NoopLoop_P: serial_create_fn = AP_RangeFinder_NoopLoop::create; -#endif break; +#endif - case Type::TOFSenseP_CAN: #if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED + case Type::TOFSenseP_CAN: _add_backend(new AP_RangeFinder_TOFSenseP_CAN(state[instance], params[instance]), instance); -#endif break; - case Type::NRA24_CAN: +#endif #if AP_RANGEFINDER_NRA24_CAN_ENABLED + case Type::NRA24_CAN: _add_backend(new AP_RangeFinder_NRA24_CAN(state[instance], params[instance]), instance); -#endif break; - case Type::TOFSenseF_I2C: { +#endif #if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED + case Type::TOFSenseF_I2C: { uint8_t addr = TOFSENSEP_I2C_DEFAULT_ADDR; if (params[instance].address != 0) { addr = params[instance].address; @@ -571,9 +573,8 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } break; -#endif } - +#endif case Type::NONE: break; } @@ -816,19 +817,26 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le // backend-specific checks. This might end up drivers[i]->arming_checks(...). switch (drivers[i]->allocated_type()) { -#if AP_RANGEFINDER_PWM_ENABLED || AP_RANGEFINDER_ANALOG_ENABLED +#if AP_RANGEFINDER_ANALOG_ENABLED || AP_RANGEFINDER_PWM_ENABLED +#if AP_RANGEFINDER_ANALOG_ENABLED case Type::ANALOG: +#endif +#if AP_RANGEFINDER_PWM_ENABLED case Type::PX4_PWM: - case Type::PWM: { + case Type::PWM: +#endif + { // ensure pin is configured if (params[i].pin == -1) { hal.util->snprintf(failure_msg, failure_msg_len, "RNGFND%u_PIN not set", unsigned(i + 1)); return false; } +#if AP_RANGEFINDER_ANALOG_ENABLED if (drivers[i]->allocated_type() == Type::ANALOG) { // Analog backend does not use GPIO pin break; } +#endif // ensure that the pin we're configured to use is available if (!hal.gpio->valid_pin(params[i].pin)) { @@ -842,7 +850,7 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le } break; } -#endif +#endif // AP_RANGEFINDER_ANALOG_ENABLED || AP_RANGEFINDER_PWM_ENABLED #if AP_RANGEFINDER_NRA24_CAN_ENABLED case Type::NRA24_CAN: { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 6ffda40ff9f13..1f3353656708d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -57,47 +57,123 @@ class RangeFinder // RangeFinder driver types enum class Type { NONE = 0, +#if AP_RANGEFINDER_ANALOG_ENABLED ANALOG = 1, +#endif +#if AP_RANGEFINDER_MAXSONARI2CXL_ENABLED MBI2C = 2, +#endif +#if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED PLI2C = 3, +#endif // PX4 = 4, // no longer used, but may be in some user's parameters +#if AP_RANGEFINDER_PWM_ENABLED PX4_PWM= 5, +#endif +#if AP_RANGEFINDER_BBB_PRU_ENABLED BBB_PRU= 6, +#endif +#if AP_RANGEFINDER_LWI2C_ENABLED LWI2C = 7, +#endif +#if AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED LWSER = 8, +#endif +#if AP_RANGEFINDER_BEBOP_ENABLED BEBOP = 9, +#endif +#if AP_RANGEFINDER_MAVLINK_ENABLED MAVLink = 10, +#endif +#if AP_RANGEFINDER_USD1_SERIAL_ENABLED USD1_Serial = 11, +#endif +#if AP_RANGEFINDER_LEDDARONE_ENABLED LEDDARONE = 12, +#endif +#if AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED MBSER = 13, +#endif +#if AP_RANGEFINDER_TRI2C_ENABLED TRI2C = 14, +#endif +#if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED PLI2CV3= 15, +#endif VL53L0X = 16, +#if AP_RANGEFINDER_NMEA_ENABLED NMEA = 17, +#endif +#if AP_RANGEFINDER_WASP_ENABLED WASP = 18, +#endif +#if AP_RANGEFINDER_BENEWAKE_TF02_ENABLED BenewakeTF02 = 19, +#endif +#if AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED BenewakeTFmini = 20, +#endif +#if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED PLI2CV3HP = 21, +#endif +#if AP_RANGEFINDER_PWM_ENABLED PWM = 22, +#endif +#if AP_RANGEFINDER_BLPING_ENABLED BLPing = 23, +#endif +#if AP_RANGEFINDER_DRONECAN_ENABLED UAVCAN = 24, +#endif +#if AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED BenewakeTFminiPlus = 25, +#endif +#if AP_RANGEFINDER_LANBAO_ENABLED Lanbao = 26, +#endif +#if AP_RANGEFINDER_BENEWAKE_TF03_ENABLED BenewakeTF03 = 27, +#endif VL53L1X_Short = 28, +#if AP_RANGEFINDER_LEDDARVU8_ENABLED LeddarVu8_Serial = 29, +#endif +#if AP_RANGEFINDER_HC_SR04_ENABLED HC_SR04 = 30, +#endif +#if AP_RANGEFINDER_GYUS42V2_ENABLED GYUS42v2 = 31, +#endif +#if HAL_MSP_RANGEFINDER_ENABLED MSP = 32, +#endif +#if AP_RANGEFINDER_USD1_CAN_ENABLED USD1_CAN = 33, +#endif +#if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED Benewake_CAN = 34, +#endif +#if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED TeraRanger_Serial = 35, +#endif +#if AP_RANGEFINDER_LUA_ENABLED Lua_Scripting = 36, +#endif +#if AP_RANGEFINDER_NOOPLOOP_ENABLED NoopLoop_P = 37, +#endif +#if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED TOFSenseP_CAN = 38, +#endif +#if AP_RANGEFINDER_NRA24_CAN_ENABLED NRA24_CAN = 39, +#endif +#if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED TOFSenseF_I2C = 40, +#endif +#if AP_RANGEFINDER_SIM_ENABLED SIM = 100, +#endif }; enum class Function { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.h index 7fa2d2642c015..3c48c38628f4b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.h @@ -1,14 +1,12 @@ #pragma once -#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 +#include "AP_RangeFinder_config.h" #if AP_RANGEFINDER_USD1_SERIAL_ENABLED +#include "AP_RangeFinder.h" +#include "AP_RangeFinder_Backend_Serial.h" + class AP_RangeFinder_USD1_Serial : public AP_RangeFinder_Backend_Serial { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/libraries/AP_RangeFinder/AP_RangeFinder_config.h index d3dee401d720e..0800cadd3eae8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_config.h @@ -157,6 +157,10 @@ #define AP_RANGEFINDER_USD1_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) #endif +#ifndef AP_RANGEFINDER_USD1_SERIAL_ENABLED +#define AP_RANGEFINDER_USD1_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + #ifndef AP_RANGEFINDER_VL53L0X_ENABLED #define AP_RANGEFINDER_VL53L0X_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED #endif