Line data    Source code

       1             : #include "AP_Airspeed_DroneCAN.h"
       2             : 
       3             : #if AP_AIRSPEED_DRONECAN_ENABLED
       4             : 
       5             : #include <AP_CANManager/AP_CANManager.h>
       6             : #include <AP_DroneCAN/AP_DroneCAN.h>
       7             : #include <AP_BoardConfig/AP_BoardConfig.h>
       8             : 
       9             : extern const AP_HAL::HAL& hal;
      10             : 
      11             : AP_Airspeed_DroneCAN::DetectedModules AP_Airspeed_DroneCAN::_detected_modules[];
      12             : HAL_Semaphore AP_Airspeed_DroneCAN::_sem_registry;
      13             : 
      14           5 : bool AP_Airspeed_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
      15             : {
      16           5 :     const auto driver_index = ap_dronecan->get_driver_index();
      17             : 
      18           5 :     return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_airspeed, driver_index) != nullptr)
      19             : #if AP_AIRSPEED_HYGROMETER_ENABLE
      20           5 :         && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_hygrometer, driver_index) != nullptr)
      21             : #endif
      22             :     ;
      23             : }
      24             : 
      25           0 : AP_Airspeed_Backend* AP_Airspeed_DroneCAN::probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid)
      26             : {
      27           0 :     WITH_SEMAPHORE(_sem_registry);
      28             : 
      29           0 :     AP_Airspeed_DroneCAN* backend = nullptr;
      30             : 
      31           0 :     for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
      32           0 :         if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr) {
      33           0 :             const auto bus_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
      34           0 :                                                             _detected_modules[i].ap_dronecan->get_driver_index(),
      35           0 :                                                             _detected_modules[i].node_id, 0);
      36           0 :             if (previous_devid != 0 && previous_devid != bus_id) {
      37             :                 // match with previous ID only
      38           0 :                 continue;
      39             :             }
      40           0 :             backend = NEW_NOTHROW AP_Airspeed_DroneCAN(_frontend, _instance);
      41           0 :             if (backend != nullptr) {
      42           0 :                 _detected_modules[i].driver = backend;
      43           0 :                 backend->set_bus_id(bus_id);
      44             :             }
      45           0 :             break;
      46             :         }
      47             :     }
      48             : 
      49           0 :     return backend;
      50           0 : }
      51             : 
      52           0 : AP_Airspeed_DroneCAN* AP_Airspeed_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id)
      53             : {
      54           0 :     if (ap_dronecan == nullptr) {
      55           0 :         return nullptr;
      56             :     }
      57             : 
      58           0 :     for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
      59           0 :         if (_detected_modules[i].driver != nullptr &&
      60           0 :             _detected_modules[i].ap_dronecan == ap_dronecan &&
      61           0 :             _detected_modules[i].node_id == node_id ) {
      62           0 :             return _detected_modules[i].driver;
      63             :         }
      64             :     }
      65             : 
      66           0 :     bool detected = false;
      67           0 :     for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
      68           0 :         if (_detected_modules[i].ap_dronecan == ap_dronecan && _detected_modules[i].node_id == node_id) {
      69             :             // detected
      70           0 :             detected = true;
      71           0 :             break;
      72             :         }
      73             :     }
      74             : 
      75           0 :     if (!detected) {
      76           0 :         for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
      77           0 :             if (_detected_modules[i].ap_dronecan == nullptr) {
      78           0 :                 _detected_modules[i].ap_dronecan = ap_dronecan;
      79           0 :                 _detected_modules[i].node_id = node_id;
      80           0 :                 break;
      81             :             }
      82             :         }
      83             :     }
      84             : 
      85           0 :     return nullptr;
      86             : }
      87             : 
      88           0 : void AP_Airspeed_DroneCAN::handle_airspeed(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_RawAirData &msg)
      89             : {
      90           0 :     WITH_SEMAPHORE(_sem_registry);
      91             : 
      92           0 :     AP_Airspeed_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
      93             : 
      94           0 :     if (driver != nullptr) {
      95           0 :         WITH_SEMAPHORE(driver->_sem_airspeed);
      96           0 :         driver->_pressure = msg.differential_pressure;
      97           0 :         if (!isnan(msg.static_air_temperature) &&
      98           0 :             msg.static_air_temperature > 0) {
      99           0 :             driver->_temperature = KELVIN_TO_C(msg.static_air_temperature);
     100           0 :             driver->_have_temperature = true;
     101             :         }
     102           0 :         driver->_last_sample_time_ms = AP_HAL::millis();
     103           0 :     }
     104           0 : }
     105             : 
     106             : #if AP_AIRSPEED_HYGROMETER_ENABLE
     107           0 : void AP_Airspeed_DroneCAN::handle_hygrometer(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_hygrometer_Hygrometer &msg)
     108             : {
     109           0 :     WITH_SEMAPHORE(_sem_registry);
     110             : 
     111           0 :     AP_Airspeed_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
     112             : 
     113           0 :     if (driver != nullptr) {
     114           0 :         WITH_SEMAPHORE(driver->_sem_airspeed);
     115           0 :         driver->_hygrometer.temperature = KELVIN_TO_C(msg.temperature);
     116           0 :         driver->_hygrometer.humidity = msg.humidity;
     117           0 :         driver->_hygrometer.last_sample_ms = AP_HAL::millis();
     118           0 :     }
     119           0 : }
     120             : #endif // AP_AIRSPEED_HYGROMETER_ENABLE
     121             : 
     122           0 : bool AP_Airspeed_DroneCAN::init()
     123             : {
     124             :     // always returns true
     125           0 :     return true;
     126             : }
     127             : 
     128           0 : bool AP_Airspeed_DroneCAN::get_differential_pressure(float &pressure)
     129             : {
     130           0 :     WITH_SEMAPHORE(_sem_airspeed);
     131             : 
     132           0 :     if ((AP_HAL::millis() - _last_sample_time_ms) > 250) {
     133           0 :         return false;
     134             :     }
     135             : 
     136           0 :     pressure = _pressure;
     137             : 
     138           0 :     return true;
     139           0 : }
     140             : 
     141           0 : bool AP_Airspeed_DroneCAN::get_temperature(float &temperature)
     142             : {
     143           0 :     if (!_have_temperature) {
     144           0 :         return false;
     145             :     }
     146           0 :     WITH_SEMAPHORE(_sem_airspeed);
     147             : 
     148           0 :     if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
     149           0 :         return false;
     150             :     }
     151             : 
     152           0 :     temperature = _temperature;
     153             : 
     154           0 :     return true;
     155           0 : }
     156             : 
     157             : #if AP_AIRSPEED_HYGROMETER_ENABLE
     158             : /*
     159             :   return hygrometer data if available
     160             :  */
     161           0 : bool AP_Airspeed_DroneCAN::get_hygrometer(uint32_t &last_sample_ms, float &temperature, float &humidity)
     162             : {
     163           0 :     if (_hygrometer.last_sample_ms == 0) {
     164           0 :         return false;
     165             :     }
     166           0 :     WITH_SEMAPHORE(_sem_airspeed);
     167           0 :     last_sample_ms = _hygrometer.last_sample_ms;
     168           0 :     temperature = _hygrometer.temperature;
     169           0 :     humidity = _hygrometer.humidity;
     170           0 :     return true;
     171           0 : }
     172             : #endif // AP_AIRSPEED_HYGROMETER_ENABLE
     173             : #endif // AP_AIRSPEED_DRONECAN_ENABLED