Line data    Source code

       1             : #include "AP_RangeFinder_DroneCAN.h"
       2             : 
       3             : #if AP_RANGEFINDER_DRONECAN_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : #include <AP_CANManager/AP_CANManager.h>
       7             : #include <AP_DroneCAN/AP_DroneCAN.h>
       8             : #include <GCS_MAVLink/GCS.h>
       9             : #include <AP_BoardConfig/AP_BoardConfig.h>
      10             : 
      11             : extern const AP_HAL::HAL& hal;
      12             : 
      13             : #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)
      14             : 
      15             : //links the rangefinder uavcan message to this backend
      16           5 : bool AP_RangeFinder_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
      17             : {
      18           5 :     const auto driver_index = ap_dronecan->get_driver_index();
      19             : 
      20           5 :     return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, driver_index) != nullptr);
      21             : }
      22             : 
      23             : //Method to find the backend relating to the node id
      24           0 : AP_RangeFinder_DroneCAN* AP_RangeFinder_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new)
      25             : {
      26           0 :     if (ap_dronecan == nullptr) {
      27           0 :         return nullptr;
      28             :     }
      29           0 :     AP_RangeFinder_DroneCAN* driver = nullptr;
      30           0 :     RangeFinder &frontend = *AP::rangefinder();
      31             :     //Scan through the Rangefinder params to find UAVCAN RFND with matching address.
      32           0 :     for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
      33           0 :         if ((RangeFinder::Type)frontend.params[i].type.get() == RangeFinder::Type::UAVCAN &&
      34           0 :             frontend.params[i].address == address) {
      35           0 :             driver = (AP_RangeFinder_DroneCAN*)frontend.drivers[i];
      36             :         }
      37             :         //Double check if the driver was initialised as UAVCAN Type
      38           0 :         if (driver != nullptr && (driver->_backend_type == RangeFinder::Type::UAVCAN)) {
      39           0 :             if (driver->_ap_dronecan == ap_dronecan && 
      40           0 :                 driver->_node_id == node_id) {
      41           0 :                 return driver;
      42             :             } else {
      43             :                 //we found a possible duplicate addressed sensor
      44             :                 //we return nothing in such scenario
      45           0 :                 return nullptr;
      46             :             }
      47             :         }
      48             :     }
      49             :     
      50           0 :     if (create_new) {
      51           0 :         for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
      52           0 :             if ((RangeFinder::Type)frontend.params[i].type.get() == RangeFinder::Type::UAVCAN &&
      53           0 :                 frontend.params[i].address == address) {
      54           0 :                 WITH_SEMAPHORE(frontend.detect_sem);
      55           0 :                 if (frontend.drivers[i] != nullptr) {
      56             :                     //we probably initialised this driver as something else, reboot is required for setting
      57             :                     //it up as UAVCAN type
      58           0 :                     return nullptr;
      59             :                 }
      60           0 :                 frontend.drivers[i] = NEW_NOTHROW AP_RangeFinder_DroneCAN(frontend.state[i], frontend.params[i]);
      61           0 :                 driver = (AP_RangeFinder_DroneCAN*)frontend.drivers[i];
      62           0 :                 if (driver == nullptr) {
      63           0 :                     break;
      64             :                 }
      65           0 :                 GCS_SEND_TEXT(MAV_SEVERITY_INFO, "RangeFinder[%u]: added DroneCAN node %u addr %u",
      66             :                                 unsigned(i), unsigned(node_id), unsigned(address));
      67             :                 //Assign node id and respective uavcan driver, for identification
      68           0 :                 if (driver->_ap_dronecan == nullptr) {
      69           0 :                     driver->_ap_dronecan = ap_dronecan;
      70           0 :                     driver->_node_id = node_id;
      71           0 :                     break;
      72             :                 }
      73           0 :             }
      74             :         }
      75             :     }
      76             : 
      77           0 :     return driver;
      78             : }
      79             : 
      80             : //Called from frontend to update with the readings received by handler
      81           0 : void AP_RangeFinder_DroneCAN::update()
      82             : {
      83           0 :     WITH_SEMAPHORE(_sem);
      84           0 :     if ((AP_HAL::millis() - _last_reading_ms) > 500U) {
      85             :         // if last read was more than 500ms, report NoData
      86           0 :         set_status(RangeFinder::Status::NoData);
      87           0 :         return;
      88             :     }
      89             : 
      90           0 :     if (!new_data) {
      91           0 :         return;
      92             :     }
      93             : 
      94           0 :     state.distance_m = _distance_m;
      95           0 :     state.last_reading_ms = _last_reading_ms;
      96           0 :     new_data = false;
      97             : 
      98           0 :     if (_status == RangeFinder::Status::Good) {
      99             :         // copy over states
     100           0 :         update_status();
     101             :     } else {
     102             :         // handle additional states received by measurement handler
     103           0 :         set_status(_status);
     104             :     }
     105             : 
     106           0 : }
     107             : 
     108             : //RangeFinder message handler
     109           0 : void AP_RangeFinder_DroneCAN::handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_range_sensor_Measurement &msg)
     110             : {
     111             :     //fetch the matching uavcan driver, node id and sensor id backend instance
     112           0 :     AP_RangeFinder_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, msg.sensor_id, true);
     113           0 :     if (driver == nullptr) {
     114           0 :         return;
     115             :     }
     116           0 :     WITH_SEMAPHORE(driver->_sem);
     117           0 :     switch (msg.reading_type) {
     118           0 :         case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE:
     119             :         {
     120             :             //update the states in backend instance
     121           0 :             driver->_distance_m = msg.range;
     122           0 :             driver->_last_reading_ms = AP_HAL::millis();
     123           0 :             driver->_status = RangeFinder::Status::Good;
     124           0 :             driver->new_data = true;
     125           0 :             break;
     126             :         }
     127             :         //Additional states supported by RFND message
     128           0 :         case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE:
     129             :         {
     130           0 :             driver->_distance_m = msg.range;
     131           0 :             driver->_last_reading_ms = AP_HAL::millis();
     132           0 :             driver->_status = RangeFinder::Status::OutOfRangeLow;
     133           0 :             driver->new_data = true;
     134           0 :             break;
     135             :         }
     136           0 :         case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR:
     137             :         {
     138           0 :             driver->_distance_m = msg.range;
     139           0 :             driver->_last_reading_ms = AP_HAL::millis();
     140           0 :             driver->_status = RangeFinder::Status::OutOfRangeHigh;
     141           0 :             driver->new_data = true;
     142           0 :             break;
     143             :         }
     144           0 :         default:
     145             :         {
     146           0 :             break;
     147             :         }
     148             :     }
     149             :     //copy over the sensor type of Rangefinder 
     150           0 :     switch (msg.sensor_type) {
     151           0 :         case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR:
     152             :         {
     153           0 :             driver->_sensor_type = MAV_DISTANCE_SENSOR_ULTRASOUND;
     154           0 :             break;
     155             :         }
     156           0 :         case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR:
     157             :         {
     158           0 :             driver->_sensor_type = MAV_DISTANCE_SENSOR_LASER;
     159           0 :             break;
     160             :         }
     161           0 :         case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR:
     162             :         {
     163           0 :             driver->_sensor_type = MAV_DISTANCE_SENSOR_RADAR;
     164           0 :             break;
     165             :         }
     166           0 :         default:
     167             :         {
     168           0 :             driver->_sensor_type = MAV_DISTANCE_SENSOR_UNKNOWN;
     169           0 :             break;
     170             :         }
     171             :     }
     172           0 : }
     173             : 
     174             : #endif  // AP_RANGEFINDER_DRONECAN_ENABLED