Line data    Source code

       1             : #include "AP_RCProtocol_config.h"
       2             : 
       3             : #if AP_RCPROTOCOL_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             : #include "AP_RCProtocol_DroneCAN.h"
       9             : 
      10             : AP_RCProtocol_DroneCAN::Registry AP_RCProtocol_DroneCAN::registry;
      11             : AP_RCProtocol_DroneCAN *AP_RCProtocol_DroneCAN::_singleton;
      12             : 
      13           5 : bool AP_RCProtocol_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
      14             : {
      15           5 :     const auto driver_index = ap_dronecan->get_driver_index();
      16             : 
      17           5 :     return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_rcinput, driver_index) != nullptr);
      18             : }
      19             : 
      20           0 : AP_RCProtocol_DroneCAN* AP_RCProtocol_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id)
      21             : {
      22           0 :     if (_singleton == nullptr) {
      23           0 :         return nullptr;
      24             :     }
      25             : 
      26           0 :     if (ap_dronecan == nullptr) {
      27           0 :         return nullptr;
      28             :     }
      29             : 
      30           0 :     for (auto &device : registry.detected_devices) {
      31           0 :         if (device.driver == nullptr) {
      32           0 :             continue;
      33             :         }
      34           0 :         if (device.ap_dronecan != ap_dronecan) {
      35           0 :             continue;
      36             :         }
      37           0 :         if (device.node_id != node_id ) {
      38           0 :             continue;
      39             :         }
      40           0 :         return device.driver;
      41             :     }
      42             : 
      43             :     // not found in registry; add it if possible.
      44           0 :     for (auto &device : registry.detected_devices) {
      45           0 :         if (device.ap_dronecan == nullptr) {
      46           0 :             device.ap_dronecan = ap_dronecan;
      47           0 :             device.node_id = node_id;
      48           0 :             device.driver = _singleton;
      49           0 :             return device.driver;
      50             :         }
      51             :     }
      52             : 
      53           0 :     return nullptr;
      54             : }
      55             : 
      56           0 : void AP_RCProtocol_DroneCAN::handle_rcinput(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_rc_RCInput &msg)
      57             : {
      58           0 :     AP_RCProtocol_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
      59           0 :     if (driver == nullptr) {
      60           0 :         return;
      61             :     }
      62             : 
      63           0 :     auto &rcin = driver->rcin;
      64           0 :     WITH_SEMAPHORE(rcin.sem);
      65           0 :     rcin.quality = msg.quality;
      66           0 :     rcin.status = msg.status;
      67           0 :     rcin.num_channels = MIN(msg.rcin.len, ARRAY_SIZE(rcin.channels));
      68           0 :     for (auto i=0; i<rcin.num_channels; i++) {
      69           0 :         rcin.channels[i] = msg.rcin.data[i];
      70             :     }
      71             : 
      72           0 :     rcin.last_sample_time_ms = AP_HAL::millis();
      73           0 : }
      74             : 
      75    36177494 : void AP_RCProtocol_DroneCAN::update()
      76             : {
      77             :     {
      78    36177494 :         WITH_SEMAPHORE(rcin.sem);
      79    36177494 :         if (rcin.last_sample_time_ms == last_receive_ms) {
      80             :             // no new data
      81    36177494 :             return;
      82             :         }
      83           0 :         last_receive_ms = rcin.last_sample_time_ms;
      84             : 
      85           0 :         if (rcin.bits.QUALITY_VALID) {
      86           0 :             switch (rcin.bits.QUALITY_TYPE) {
      87           0 :                 case QualityType::RSSI:
      88             :                     // as this is 0, it also provides backwards compatibility for systems not setting these bits
      89           0 :                     _rssi = rcin.quality;
      90           0 :                     break;
      91           0 :                 case QualityType::LQ_ACTIVE_ANTENNA:
      92             :                     // highest bit carries active antenna data
      93           0 :                     frontend._rc_link_status.link_quality = (rcin.quality & 0x7F);
      94           0 :                     frontend._rc_link_status.active_antenna = (rcin.quality & 0x80) ? 1 : 0;
      95           0 :                     break;
      96           0 :                 case QualityType::RSSI_DBM:
      97           0 :                     frontend._rc_link_status.rssi_dbm = rcin.quality;
      98             :                     // also set the rssi field, this avoids having to waste a slot for sending both
      99             :                     // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
     100           0 :                     if (rcin.quality < 50) {
     101           0 :                         _rssi = 255;
     102           0 :                     } else if (rcin.quality > 120) {
     103           0 :                         _rssi = 0;
     104             :                     } else {
     105           0 :                         _rssi = int16_t(roundf((120.0f - rcin.quality) * (255.0f / 70.0f)));
     106             :                     }
     107           0 :                     break;
     108           0 :                 case QualityType::SNR:
     109             :                     // SNR is shifted by 128 to support negative values
     110           0 :                     frontend._rc_link_status.snr = (int8_t)rcin.quality - 128;
     111           0 :                     break;
     112           0 :                 case QualityType::TX_POWER:
     113             :                     // carries tx power in units of 5 mW, thus can't support higher than 255*5 = 1275 mW
     114           0 :                     frontend._rc_link_status.tx_power = (int16_t)rcin.quality * 5;
     115           0 :                     break;
     116             :             }
     117             :         } else {
     118           0 :             _rssi = -1;
     119           0 :             frontend._rc_link_status.link_quality = -1;
     120           0 :             frontend._rc_link_status.rssi_dbm = -1;
     121           0 :             frontend._rc_link_status.snr = INT8_MIN;
     122           0 :             frontend._rc_link_status.tx_power = -1;
     123           0 :             frontend._rc_link_status.active_antenna = -1;
     124             :         }
     125             : 
     126           0 :         add_input(
     127           0 :             rcin.num_channels,
     128           0 :             rcin.channels,
     129           0 :             rcin.bits.FAILSAFE,
     130           0 :             _rssi,
     131           0 :             frontend._rc_link_status.link_quality
     132             :             );
     133    36177494 :     }
     134             : }
     135             : 
     136             : #endif // AP_RCPROTOCOL_DRONECAN_ENABLED