Line data    Source code

       1             : #include "AP_Baro_DroneCAN.h"
       2             : 
       3             : #if AP_BARO_DRONECAN_ENABLED
       4             : 
       5             : #include <AP_CANManager/AP_CANManager.h>
       6             : #include <AP_BoardConfig/AP_BoardConfig.h>
       7             : #include "AP_Baro_SITL.h"
       8             : #include <AP_Vehicle/AP_Vehicle_Type.h>
       9             : 
      10             : extern const AP_HAL::HAL& hal;
      11             : 
      12             : AP_Baro_DroneCAN::DetectedModules AP_Baro_DroneCAN::_detected_modules[];
      13             : HAL_Semaphore AP_Baro_DroneCAN::_sem_registry;
      14             : 
      15             : /*
      16             :   constructor - registers instance at top Baro driver
      17             :  */
      18           0 : AP_Baro_DroneCAN::AP_Baro_DroneCAN(AP_Baro &baro) :
      19           0 :     AP_Baro_Backend(baro)
      20           0 : {}
      21             : 
      22           5 : bool AP_Baro_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
      23             : {
      24           5 :     const auto driver_index = ap_dronecan->get_driver_index();
      25             : 
      26           5 :     return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_pressure, driver_index) != nullptr)
      27           5 :         && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, driver_index) != nullptr)
      28             :     ;
      29             : }
      30             : 
      31        6282 : AP_Baro_Backend* AP_Baro_DroneCAN::probe(AP_Baro &baro)
      32             : {
      33        6282 :     WITH_SEMAPHORE(_sem_registry);
      34             : 
      35        6282 :     AP_Baro_DroneCAN* backend = nullptr;
      36       25128 :     for (auto &detected_module : _detected_modules) {
      37       18846 :         if (detected_module.driver == nullptr && detected_module.ap_dronecan != nullptr) {
      38           0 :             backend = NEW_NOTHROW AP_Baro_DroneCAN(baro);
      39           0 :             if (backend != nullptr) {
      40           0 :                 detected_module.driver = backend;
      41           0 :                 backend->_pressure = 0;
      42           0 :                 backend->_pressure_count = 0;
      43           0 :                 backend->_ap_dronecan = detected_module.ap_dronecan;
      44           0 :                 backend->_node_id = detected_module.node_id;
      45             : 
      46           0 :                 backend->_instance = backend->_frontend.register_sensor();
      47           0 :                 backend->set_bus_id(backend->_instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
      48           0 :                                                                                     detected_module.ap_dronecan->get_driver_index(),
      49           0 :                                                                                     backend->_node_id, 0));
      50             :             }
      51           0 :             break;
      52             :         }
      53             :     }
      54       12564 :     return backend;
      55        6282 : }
      56             : 
      57           0 : AP_Baro_DroneCAN* AP_Baro_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, bool create_new)
      58             : {
      59           0 :     if (ap_dronecan == nullptr) {
      60           0 :         return nullptr;
      61             :     }
      62           0 :     for (auto &detected_module : _detected_modules) {
      63           0 :         if (detected_module.driver != nullptr &&
      64           0 :             detected_module.ap_dronecan == ap_dronecan &&
      65           0 :             detected_module.node_id == node_id) {
      66           0 :             return detected_module.driver;
      67             :         }
      68             :     }
      69             :     
      70           0 :     if (create_new) {
      71           0 :         bool already_detected = false;
      72             :         //Check if there's an empty spot for possible registration
      73           0 :         for (auto &detected_module : _detected_modules) {
      74           0 :             if (detected_module.ap_dronecan == ap_dronecan && detected_module.node_id == node_id) {
      75             :                 //Already Detected
      76           0 :                 already_detected = true;
      77           0 :                 break;
      78             :             }
      79             :         }
      80           0 :         if (!already_detected) {
      81           0 :             for (auto &detected_module : _detected_modules) {
      82           0 :                 if (detected_module.ap_dronecan == nullptr) {
      83           0 :                     detected_module.ap_dronecan = ap_dronecan;
      84           0 :                     detected_module.node_id = node_id;
      85           0 :                     break;
      86             :                 }
      87             :             }
      88             :         }
      89             :     }
      90             : 
      91           0 :     return nullptr;
      92             : }
      93             : 
      94             : 
      95           0 : void AP_Baro_DroneCAN::_update_and_wrap_accumulator(float *accum, float val, uint8_t *count, const uint8_t max_count)
      96             : {
      97           0 :     *accum += val;
      98           0 :     *count += 1;
      99           0 :     if (*count == max_count) {
     100           0 :         *count = max_count / 2;
     101           0 :         *accum = *accum / 2;
     102             :     }
     103           0 : }
     104             : 
     105           0 : void AP_Baro_DroneCAN::handle_pressure(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticPressure &msg)
     106             : {
     107             :     AP_Baro_DroneCAN* driver;
     108             :     {
     109           0 :         WITH_SEMAPHORE(_sem_registry);
     110           0 :         driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, true);
     111           0 :         if (driver == nullptr) {
     112           0 :             return;
     113             :         }
     114           0 :     }
     115             :     {
     116           0 :         WITH_SEMAPHORE(driver->_sem_baro);
     117           0 :         _update_and_wrap_accumulator(&driver->_pressure, msg.static_pressure, &driver->_pressure_count, 32);
     118           0 :         driver->new_pressure = true;
     119           0 :     }
     120             : }
     121             : 
     122           0 : void AP_Baro_DroneCAN::handle_temperature(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticTemperature &msg)
     123             : {
     124             :     AP_Baro_DroneCAN* driver;
     125             :     {
     126           0 :         WITH_SEMAPHORE(_sem_registry);
     127           0 :         driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, false);
     128           0 :         if (driver == nullptr) {
     129           0 :             return;
     130             :         }
     131           0 :     }
     132             :     {
     133           0 :         WITH_SEMAPHORE(driver->_sem_baro);
     134           0 :         driver->_temperature = KELVIN_TO_C(msg.static_temperature);
     135           0 :     }
     136             : }
     137             : 
     138             : // Read the sensor
     139           0 : void AP_Baro_DroneCAN::update(void)
     140             : {
     141           0 :     float pressure = 0;
     142             : 
     143           0 :     WITH_SEMAPHORE(_sem_baro);
     144           0 :     if (new_pressure) {
     145           0 :         if (_pressure_count != 0) {
     146           0 :             pressure = _pressure / _pressure_count;
     147           0 :             _pressure_count = 0;
     148           0 :             _pressure = 0;
     149             :         }
     150           0 :         _copy_to_frontend(_instance, pressure, _temperature);
     151             : 
     152             : 
     153           0 :         _frontend.set_external_temperature(_temperature);
     154           0 :         new_pressure = false;
     155             :     }
     156           0 : }
     157             : 
     158             : #endif // AP_BARO_DRONECAN_ENABLED