From ea26e94f661848896cc2bbe5b13024ea59457628 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 1 Nov 2019 14:03:14 +1100 Subject: [PATCH] AP_RangeFinder: use enum-class for RangeFinder type --- .../AP_RangeFinder_PulsedLightLRF.cpp | 8 +-- .../AP_RangeFinder_PulsedLightLRF.h | 6 +- .../AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp | 6 +- libraries/AP_RangeFinder/RangeFinder.cpp | 60 +++++++++---------- libraries/AP_RangeFinder/RangeFinder.h | 58 +++++++++--------- .../AP_RangeFinder/RangeFinder_Backend.cpp | 6 +- .../AP_RangeFinder/RangeFinder_Backend.h | 4 +- .../examples/RFIND_test/RFIND_test.cpp | 2 +- 8 files changed, 75 insertions(+), 75 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 0377ae6919..c2a08e62af 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -48,7 +48,7 @@ extern const AP_HAL::HAL& hal; AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, - RangeFinder::RangeFinder_Type _rftype) + RangeFinder::Type _rftype) : AP_RangeFinder_Backend(_state, _params) , _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR)) , rftype(_rftype) @@ -62,7 +62,7 @@ AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus, AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(uint8_t bus, RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, - RangeFinder::RangeFinder_Type rftype) + RangeFinder::Type rftype) { AP_RangeFinder_PulsedLightLRF *sensor = new AP_RangeFinder_PulsedLightLRF(bus, _state, _params, rftype); @@ -167,9 +167,9 @@ bool AP_RangeFinder_PulsedLightLRF::init(void) // LidarLite needs split transfers _dev->set_split_transfers(true); - if (rftype == RangeFinder::RangeFinder_TYPE_PLI2CV3) { + if (rftype == RangeFinder::Type::PLI2CV3) { v2_hardware = true; - } else if (rftype == RangeFinder::RangeFinder_TYPE_PLI2CV3HP) { + } else if (rftype == RangeFinder::Type::PLI2CV3HP) { v3hp_hardware = true; } else { // auto-detect v1 vs v2 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h index 53e51601e0..28d9dd2921 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h @@ -22,7 +22,7 @@ public: static AP_RangeFinder_Backend *detect(uint8_t bus, RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, - RangeFinder::RangeFinder_Type rftype); + RangeFinder::Type rftype); // update state void update(void) override {} @@ -38,7 +38,7 @@ private: AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, - RangeFinder::RangeFinder_Type rftype); + RangeFinder::Type rftype); // start a reading bool init(void); @@ -53,7 +53,7 @@ private: bool v2_hardware; bool v3hp_hardware; uint16_t last_distance_cm; - RangeFinder::RangeFinder_Type rftype; + RangeFinder::Type rftype; enum { PHASE_MEASURE, PHASE_COLLECT } phase; }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp index e05adf9359..52b9a636c9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp @@ -51,12 +51,12 @@ AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u AP_RangeFinder_UAVCAN* driver = nullptr; //Scan through the Rangefinder params to find UAVCAN RFND with matching address. for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) { - if (AP::rangefinder()->params[i].type == RangeFinder::RangeFinder_TYPE_UAVCAN && + if ((RangeFinder::Type)AP::rangefinder()->params[i].type.get() == RangeFinder::Type::UAVCAN && AP::rangefinder()->params[i].address == address) { driver = (AP_RangeFinder_UAVCAN*)AP::rangefinder()->drivers[i]; } //Double check if the driver was initialised as UAVCAN Type - if (driver != nullptr && (driver->_backend_type == RangeFinder::RangeFinder_TYPE_UAVCAN)) { + if (driver != nullptr && (driver->_backend_type == RangeFinder::Type::UAVCAN)) { if (driver->_ap_uavcan == ap_uavcan && driver->_node_id == node_id) { return driver; @@ -70,7 +70,7 @@ AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u if (create_new) { for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) { - if (AP::rangefinder()->params[i].type == RangeFinder::RangeFinder_TYPE_UAVCAN && + if ((RangeFinder::Type)AP::rangefinder()->params[i].type.get() == RangeFinder::Type::UAVCAN && AP::rangefinder()->params[i].address == address) { if (AP::rangefinder()->drivers[i] != nullptr) { //we probably initialised this driver as something else, reboot is required for setting diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 542a9a982d..f4d7d22577 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -329,7 +329,7 @@ void RangeFinder::update(void) { for (uint8_t i=0; iget_device(i, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)))) { @@ -380,7 +380,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } break; - case RangeFinder_TYPE_LWI2C: + case Type::LWI2C: 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()) { @@ -399,7 +399,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) #endif } break; - case RangeFinder_TYPE_TRI2C: + case Type::TRI2C: if (params[instance].address) { FOREACH_I2C(i) { if (_add_backend(AP_RangeFinder_TeraRangerI2C::detect(state[instance], params[instance], @@ -409,7 +409,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } break; - case RangeFinder_TYPE_VL53L0X: + case Type::VL53L0X: FOREACH_I2C(i) { if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance], hal.i2c_mgr->get_device(i, params[instance].address)))) { @@ -421,7 +421,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } } break; - case RangeFinder_TYPE_BenewakeTFminiPlus: + case Type::BenewakeTFminiPlus: FOREACH_I2C(i) { if (_add_backend(AP_RangeFinder_Benewake_TFMiniPlus::detect(state[instance], params[instance], hal.i2c_mgr->get_device(i, params[instance].address)))) { @@ -430,7 +430,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } break; #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - case RangeFinder_TYPE_PX4_PWM: + case Type::PX4_PWM: #ifndef HAL_BUILD_AP_PERIPH // to ease moving from PX4 to ChibiOS we'll lie a little about // the backend driver... @@ -441,48 +441,48 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) break; #endif #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI - case RangeFinder_TYPE_BBB_PRU: + case Type::BBB_PRU: if (AP_RangeFinder_BBB_PRU::detect()) { drivers[instance] = new AP_RangeFinder_BBB_PRU(state[instance], params[instance]); } break; #endif - case RangeFinder_TYPE_LWSER: + case Type::LWSER: if (AP_RangeFinder_LightWareSerial::detect(serial_instance)) { drivers[instance] = new AP_RangeFinder_LightWareSerial(state[instance], params[instance], serial_instance++); } break; - case RangeFinder_TYPE_LEDDARONE: + case Type::LEDDARONE: if (AP_RangeFinder_LeddarOne::detect(serial_instance)) { drivers[instance] = new AP_RangeFinder_LeddarOne(state[instance], params[instance], serial_instance++); } break; - case RangeFinder_TYPE_ULANDING: + case Type::ULANDING: if (AP_RangeFinder_uLanding::detect(serial_instance)) { drivers[instance] = new AP_RangeFinder_uLanding(state[instance], params[instance], serial_instance++); } break; #if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO) && defined(HAVE_LIBIIO) - case RangeFinder_TYPE_BEBOP: + case Type::BEBOP: if (AP_RangeFinder_Bebop::detect()) { drivers[instance] = new AP_RangeFinder_Bebop(state[instance], params[instance]); } break; #endif - case RangeFinder_TYPE_MAVLink: + case Type::MAVLink: #ifndef HAL_BUILD_AP_PERIPH if (AP_RangeFinder_MAVLink::detect()) { drivers[instance] = new AP_RangeFinder_MAVLink(state[instance], params[instance]); } #endif break; - case RangeFinder_TYPE_MBSER: + case Type::MBSER: if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_instance)) { drivers[instance] = new AP_RangeFinder_MaxsonarSerialLV(state[instance], params[instance], serial_instance++); } break; - case RangeFinder_TYPE_ANALOG: + case Type::ANALOG: #ifndef HAL_BUILD_AP_PERIPH // note that analog will always come back as present if the pin is valid if (AP_RangeFinder_analog::detect(params[instance])) { @@ -490,44 +490,44 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) } #endif break; - case RangeFinder_TYPE_NMEA: + case Type::NMEA: if (AP_RangeFinder_NMEA::detect(serial_instance)) { drivers[instance] = new AP_RangeFinder_NMEA(state[instance], params[instance], serial_instance++); } break; - case RangeFinder_TYPE_WASP: + case Type::WASP: if (AP_RangeFinder_Wasp::detect(serial_instance)) { drivers[instance] = new AP_RangeFinder_Wasp(state[instance], params[instance], serial_instance++); } break; - case RangeFinder_TYPE_BenewakeTF02: + case Type::BenewakeTF02: if (AP_RangeFinder_Benewake::detect(serial_instance)) { drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF02); } break; - case RangeFinder_TYPE_BenewakeTFmini: + case Type::BenewakeTFmini: if (AP_RangeFinder_Benewake::detect(serial_instance)) { drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini); } break; - case RangeFinder_TYPE_BenewakeTF03: + case Type::BenewakeTF03: if (AP_RangeFinder_Benewake::detect(serial_instance)) { drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF03); } break; - case RangeFinder_TYPE_PWM: + case Type::PWM: #ifndef HAL_BUILD_AP_PERIPH if (AP_RangeFinder_PWM::detect()) { drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height); } #endif break; - case RangeFinder_TYPE_BLPing: + case Type::BLPing: if (AP_RangeFinder_BLPing::detect(serial_instance)) { drivers[instance] = new AP_RangeFinder_BLPing(state[instance], params[instance], serial_instance++); } break; - case RangeFinder_TYPE_Lanbao: + case Type::Lanbao: if (AP_RangeFinder_Lanbao::detect(serial_instance)) { drivers[instance] = new AP_RangeFinder_Lanbao(state[instance], params[instance], serial_instance++); } @@ -548,7 +548,7 @@ AP_RangeFinder_Backend *RangeFinder::get_backend(uint8_t id) const { return nullptr; } if (drivers[id] != nullptr) { - if (drivers[id]->type() == RangeFinder_TYPE_NONE) { + if (drivers[id]->type() == Type::NONE) { // pretend it isn't here; disabled at runtime? return nullptr; } @@ -569,7 +569,7 @@ void RangeFinder::handle_msg(const mavlink_message_t &msg) { uint8_t i; for (i=0; ihandle_msg(msg); } } @@ -727,7 +727,7 @@ void RangeFinder::Log_RFND() bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const { for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) { - if ((params[i].type != RangeFinder_TYPE_NONE) && (drivers[i] == nullptr)) { + if (((Type)params[i].type.get() != Type::NONE) && (drivers[i] == nullptr)) { hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %d was not detected", i + 1); return false; } diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index e117e27bb2..fbec5656b5 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -48,35 +48,35 @@ public: RangeFinder &operator=(const RangeFinder&) = delete; // RangeFinder driver types - enum RangeFinder_Type { - RangeFinder_TYPE_NONE = 0, - RangeFinder_TYPE_ANALOG = 1, - RangeFinder_TYPE_MBI2C = 2, - RangeFinder_TYPE_PLI2C = 3, - RangeFinder_TYPE_PX4 = 4, - RangeFinder_TYPE_PX4_PWM= 5, - RangeFinder_TYPE_BBB_PRU= 6, - RangeFinder_TYPE_LWI2C = 7, - RangeFinder_TYPE_LWSER = 8, - RangeFinder_TYPE_BEBOP = 9, - RangeFinder_TYPE_MAVLink = 10, - RangeFinder_TYPE_ULANDING= 11, - RangeFinder_TYPE_LEDDARONE = 12, - RangeFinder_TYPE_MBSER = 13, - RangeFinder_TYPE_TRI2C = 14, - RangeFinder_TYPE_PLI2CV3= 15, - RangeFinder_TYPE_VL53L0X = 16, - RangeFinder_TYPE_NMEA = 17, - RangeFinder_TYPE_WASP = 18, - RangeFinder_TYPE_BenewakeTF02 = 19, - RangeFinder_TYPE_BenewakeTFmini = 20, - RangeFinder_TYPE_PLI2CV3HP = 21, - RangeFinder_TYPE_PWM = 22, - RangeFinder_TYPE_BLPing = 23, - RangeFinder_TYPE_UAVCAN = 24, - RangeFinder_TYPE_BenewakeTFminiPlus = 25, - RangeFinder_TYPE_Lanbao = 26, - RangeFinder_TYPE_BenewakeTF03 = 27, + enum class Type { + NONE = 0, + ANALOG = 1, + MBI2C = 2, + PLI2C = 3, + PX4 = 4, + PX4_PWM= 5, + BBB_PRU= 6, + LWI2C = 7, + LWSER = 8, + BEBOP = 9, + MAVLink = 10, + ULANDING= 11, + LEDDARONE = 12, + MBSER = 13, + TRI2C = 14, + PLI2CV3= 15, + VL53L0X = 16, + NMEA = 17, + WASP = 18, + BenewakeTF02 = 19, + BenewakeTFmini = 20, + PLI2CV3HP = 21, + PWM = 22, + BLPing = 23, + UAVCAN = 24, + BenewakeTFminiPlus = 25, + Lanbao = 26, + BenewakeTF03 = 27, }; enum RangeFinder_Function { diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp index b777616cc8..195f4939c3 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp @@ -28,18 +28,18 @@ AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_ state(_state), params(_params) { - _backend_type = (RangeFinder::RangeFinder_Type)params.type.get(); + _backend_type = type(); } MAV_DISTANCE_SENSOR AP_RangeFinder_Backend::get_mav_distance_sensor_type() const { - if (params.type == RangeFinder::RangeFinder_TYPE_NONE) { + if (type() == RangeFinder::Type::NONE) { return MAV_DISTANCE_SENSOR_UNKNOWN; } return _get_mav_distance_sensor_type(); } RangeFinder::RangeFinder_Status AP_RangeFinder_Backend::status() const { - if (params.type == RangeFinder::RangeFinder_TYPE_NONE) { + if (type() == RangeFinder::Type::NONE) { // turned off at runtime? return RangeFinder::RangeFinder_NotConnected; } diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.h b/libraries/AP_RangeFinder/RangeFinder_Backend.h index ce9a41d463..0c91d48aa5 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.h @@ -41,7 +41,7 @@ public: int16_t ground_clearance_cm() const { return params.ground_clearance_cm; } MAV_DISTANCE_SENSOR get_mav_distance_sensor_type() const; RangeFinder::RangeFinder_Status status() const; - RangeFinder::RangeFinder_Type type() const { return (RangeFinder::RangeFinder_Type)params.type.get(); } + RangeFinder::Type type() const { return (RangeFinder::Type)params.type.get(); } // true if sensor is returning data bool has_data() const; @@ -71,7 +71,7 @@ protected: HAL_Semaphore _sem; //Type Backend initialised with - RangeFinder::RangeFinder_Type _backend_type; + RangeFinder::Type _backend_type; virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const = 0; }; diff --git a/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp b/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp index 5880af058f..cf784c0710 100644 --- a/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp +++ b/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp @@ -19,7 +19,7 @@ void setup() hal.console->printf("Range Finder library test\n"); // setup for analog pin 13 - AP_Param::set_object_value(&sonar, sonar.var_info, "_TYPE", RangeFinder::RangeFinder_TYPE_PLI2C); + AP_Param::set_object_value(&sonar, sonar.var_info, "_TYPE", (uint8_t)RangeFinder::Type::PLI2C); AP_Param::set_object_value(&sonar, sonar.var_info, "_PIN", -1.0f); AP_Param::set_object_value(&sonar, sonar.var_info, "_SCALING", 1.0f);