From 479c238177b2ea527b427c7d563977d059f121b1 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 8 Apr 2019 15:12:46 +0530 Subject: [PATCH] RangeFinder: add support for rangefinder sensor over can --- .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 4 +- .../AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp | 181 ++++++++++++++++++ .../AP_RangeFinder/AP_RangeFinder_UAVCAN.h | 37 ++++ libraries/AP_RangeFinder/RangeFinder.cpp | 1 + libraries/AP_RangeFinder/RangeFinder.h | 4 +- .../AP_RangeFinder/RangeFinder_Backend.cpp | 1 + .../AP_RangeFinder/RangeFinder_Backend.h | 3 + 7 files changed, 228 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 5f5bbf4078..5d0250532d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -6,7 +6,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @Param: TYPE // @DisplayName: Rangefinder type // @Description: What type of rangefinder device that is connected - // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing + // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN // @User: Standard AP_GROUPINFO("TYPE", 1, AP_RangeFinder_Params, type, 0), @@ -97,7 +97,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @Param: ADDR // @DisplayName: Bus address of sensor - // @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor. + // @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor and UAVCAN Range Sensors to allow for multiple sensors on different addresses. A value of 0 disables the sensor. // @Range: 0 127 // @Increment: 1 // @User: Standard diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp new file mode 100644 index 0000000000..e05adf9359 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp @@ -0,0 +1,181 @@ +#include + +#if HAL_WITH_UAVCAN + +#include "AP_RangeFinder_UAVCAN.h" + +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +#define debug_range_finder_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { hal.console->printf(fmt, ##args); }} while (0) + +//UAVCAN Frontend Registry Binder +UC_REGISTRY_BINDER(MeasurementCb, uavcan::equipment::range_sensor::Measurement); + +/* + constructor - registers instance at top RangeFinder driver + */ +AP_RangeFinder_UAVCAN::AP_RangeFinder_UAVCAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend(_state, _params) +{} + +//links the rangefinder uavcan message to this backend +void AP_RangeFinder_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) +{ + if (ap_uavcan == nullptr) { + return; + } + + auto* node = ap_uavcan->get_node(); + + uavcan::Subscriber *measurement_listener; + measurement_listener = new uavcan::Subscriber(*node); + // Register method to handle incoming RangeFinder measurement + const int measurement_listener_res = measurement_listener->start(MeasurementCb(ap_uavcan, &handle_measurement)); + if (measurement_listener_res < 0) { + AP_HAL::panic("UAVCAN RangeFinder subscriber start problem\n\r"); + return; + } +} + +//Method to find the backend relating to the node id +AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t address, bool create_new) +{ + if (ap_uavcan == nullptr) { + return nullptr; + } + 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 && + 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->_ap_uavcan == ap_uavcan && + driver->_node_id == node_id) { + return driver; + } else { + //we found a possible duplicate addressed sensor + //we return nothing in such scenario + return nullptr; + } + } + } + + if (create_new) { + for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) { + if (AP::rangefinder()->params[i].type == RangeFinder::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 + //it up as UAVCAN type + return nullptr; + } + AP::rangefinder()->drivers[i] = new AP_RangeFinder_UAVCAN(AP::rangefinder()->state[i], AP::rangefinder()->params[i]); + driver = (AP_RangeFinder_UAVCAN*)AP::rangefinder()->drivers[i]; + if (driver == nullptr) { + break; + } + AP::rangefinder()->num_instances = MAX(i+1, AP::rangefinder()->num_instances); + //Assign node id and respective uavcan driver, for identification + if (driver->_ap_uavcan == nullptr) { + driver->_ap_uavcan = ap_uavcan; + driver->_node_id = node_id; + break; + } + } + } + } + + return driver; +} + +//Called from frontend to update with the readings received by handler +void AP_RangeFinder_UAVCAN::update() +{ + WITH_SEMAPHORE(_sem); + if ((AP_HAL::millis() - _last_reading_ms) > 500) { + //if data is older than 500ms, report NoData + set_status(RangeFinder::RangeFinder_NoData); + } else if (_status == RangeFinder::RangeFinder_Good && new_data) { + //copy over states + state.distance_cm = _distance_cm; + state.last_reading_ms = _last_reading_ms; + update_status(); + new_data = false; + } else if (_status != RangeFinder::RangeFinder_Good) { + //handle additional states received by measurement handler + set_status(_status); + } +} + +//RangeFinder message handler +void AP_RangeFinder_UAVCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb) +{ + //fetch the matching uavcan driver, node id and sensor id backend instance + AP_RangeFinder_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, cb.msg->sensor_id, true); + if (driver == nullptr) { + return; + } + WITH_SEMAPHORE(driver->_sem); + switch (cb.msg->reading_type) { + case uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE: + { + //update the states in backend instance + driver->_distance_cm = cb.msg->range*100.0f; + driver->_last_reading_ms = AP_HAL::millis(); + driver->_status = RangeFinder::RangeFinder_Good; + driver->new_data = true; + break; + } + //Additional states supported by RFND message + case uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE: + { + driver->_last_reading_ms = AP_HAL::millis(); + driver->_status = RangeFinder::RangeFinder_OutOfRangeLow; + break; + } + case uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR: + { + driver->_last_reading_ms = AP_HAL::millis(); + driver->_status = RangeFinder::RangeFinder_OutOfRangeHigh; + break; + } + default: + { + break; + } + } + //copy over the sensor type of Rangefinder + switch (cb.msg->sensor_type) { + case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR: + { + driver->_sensor_type = MAV_DISTANCE_SENSOR_ULTRASOUND; + break; + } + case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR: + { + driver->_sensor_type = MAV_DISTANCE_SENSOR_LASER; + break; + } + case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR: + { + driver->_sensor_type = MAV_DISTANCE_SENSOR_RADAR; + break; + } + default: + { + driver->_sensor_type = MAV_DISTANCE_SENSOR_UNKNOWN; + break; + } + } +} + +#endif // HAL_WITH_UAVCAN + diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h new file mode 100644 index 0000000000..4de1946cce --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h @@ -0,0 +1,37 @@ +#pragma once + +#include "RangeFinder_Backend.h" + +#if HAL_WITH_UAVCAN +#include +#include + +class MeasurementCb; + +class AP_RangeFinder_UAVCAN : public AP_RangeFinder_Backend { +public: + AP_RangeFinder_UAVCAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + + void update() override; + + static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + static AP_RangeFinder_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t address, bool create_new); + static AP_RangeFinder_Backend* detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + + static void handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb); + +protected: + virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return _sensor_type; + } +private: + uint8_t _instance; + RangeFinder::RangeFinder_Status _status; + uint16_t _distance_cm; + uint32_t _last_reading_ms; + AP_UAVCAN* _ap_uavcan; + uint8_t _node_id; + bool new_data; + MAV_DISTANCE_SENSOR _sensor_type; +}; +#endif //HAL_WITH_UAVCAN diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 696dae1eec..687d029045 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -37,6 +37,7 @@ #include "AP_RangeFinder_Benewake.h" #include "AP_RangeFinder_PWM.h" #include "AP_RangeFinder_BLPing.h" +#include "AP_RangeFinder_UAVCAN.h" #include #include diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 93b18386d1..cd732b06d2 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -35,7 +35,8 @@ class AP_RangeFinder_Backend; class RangeFinder { friend class AP_RangeFinder_Backend; - + //UAVCAN drivers are initialised in the Backend, hence list of drivers is needed there. + friend class AP_RangeFinder_UAVCAN; public: RangeFinder(AP_SerialManager &_serial_manager); @@ -69,6 +70,7 @@ public: RangeFinder_TYPE_PLI2CV3HP = 21, RangeFinder_TYPE_PWM = 22, RangeFinder_TYPE_BLPing = 23, + RangeFinder_TYPE_UAVCAN = 24 }; enum RangeFinder_Function { diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp index 04f7c6f4cf..b777616cc8 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp @@ -28,6 +28,7 @@ AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_ state(_state), params(_params) { + _backend_type = (RangeFinder::RangeFinder_Type)params.type.get(); } MAV_DISTANCE_SENSOR AP_RangeFinder_Backend::get_mav_distance_sensor_type() const { diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.h b/libraries/AP_RangeFinder/RangeFinder_Backend.h index 01fdaa452f..b3386ef8fc 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.h @@ -70,5 +70,8 @@ protected: // semaphore for access to shared frontend data HAL_Semaphore _sem; + //Type Backend initialised with + RangeFinder::RangeFinder_Type _backend_type; + virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const = 0; };