Line data    Source code

       1             : /*
       2             :   map local serial ports to remote DroneCAN serial ports using the
       3             :   TUNNEL_TARGETTED message
       4             :  */
       5             : 
       6             : #include "AP_DroneCAN.h"
       7             : 
       8             : #if HAL_ENABLE_DRONECAN_DRIVERS && AP_DRONECAN_SERIAL_ENABLED
       9             : 
      10             : #include <AP_Math/AP_Math.h>
      11             : #include <AP_BoardConfig/AP_BoardConfig.h>
      12             : 
      13             : AP_DroneCAN_Serial *AP_DroneCAN_Serial::serial[HAL_MAX_CAN_PROTOCOL_DRIVERS];
      14             : 
      15             : #ifndef AP_DRONECAN_SERIAL_MIN_TXSIZE
      16             : #define AP_DRONECAN_SERIAL_MIN_TXSIZE 2048
      17             : #endif
      18             : 
      19             : #ifndef AP_DRONECAN_SERIAL_MIN_RXSIZE
      20             : #define AP_DRONECAN_SERIAL_MIN_RXSIZE 2048
      21             : #endif
      22             : 
      23             : /*
      24             :   initialise DroneCAN serial aports
      25             : */
      26           5 : void AP_DroneCAN_Serial::init(AP_DroneCAN *_dronecan)
      27             : {
      28           5 :     if (enable == 0) {
      29           4 :         return;
      30             :     }
      31           1 :     const uint8_t driver_index = _dronecan->get_driver_index();
      32           1 :     if (driver_index >= ARRAY_SIZE(serial)) {
      33           0 :         return;
      34             :     }
      35           1 :     serial[driver_index] = this;
      36           1 :     dronecan = _dronecan;
      37             : 
      38           1 :     const uint8_t base_port = driver_index == 0? AP_SERIALMANAGER_CAN_D1_PORT_1 : AP_SERIALMANAGER_CAN_D2_PORT_1;
      39           1 :     bool need_subscriber = false;
      40             : 
      41           4 :     for (uint8_t i=0; i<ARRAY_SIZE(ports); i++) {
      42           3 :         auto &p = ports[i];
      43           3 :         p.state.idx = base_port + i;
      44           3 :         if (p.node > 0 && p.idx >= 0) {
      45           2 :             p.init();
      46           2 :             AP::serialmanager().register_port(&p);
      47           2 :             need_subscriber = true;
      48             :         }
      49             :     }
      50             : 
      51           1 :     if (need_subscriber) {
      52           1 :         if (Canard::allocate_sub_arg_callback(dronecan, &handle_tunnel_targetted, dronecan->get_driver_index()) == nullptr) {
      53           0 :             AP_BoardConfig::allocation_error("serial_tunnel_sub");
      54             :         }
      55           1 :         targetted = NEW_NOTHROW Canard::Publisher<uavcan_tunnel_Targetted>(dronecan->get_canard_iface());
      56           1 :         if (targetted == nullptr) {
      57           0 :             AP_BoardConfig::allocation_error("serial_tunnel_pub");
      58             :         }
      59           1 :         targetted->set_timeout_ms(20);
      60           1 :         targetted->set_priority(CANARD_TRANSFER_PRIORITY_MEDIUM);
      61             :     }
      62             : }
      63             : 
      64             : /*
      65             :   update DroneCAN serial ports
      66             : */
      67       21794 : void AP_DroneCAN_Serial::update(void)
      68             : {
      69       21794 :     const uint32_t now_ms = AP_HAL::millis();
      70       87176 :     for (auto &p : ports) {
      71       65382 :         if (p.baudrate == 0) {
      72       65232 :             continue;
      73             :         }
      74        6058 :         if (p.writebuffer == nullptr || p.node <= 0 || p.idx < 0) {
      75         108 :             continue;
      76             :         }
      77        5950 :         uavcan_tunnel_Targetted pkt {};
      78        5950 :         uint32_t n = 0;
      79             :         {
      80        5950 :             WITH_SEMAPHORE(p.sem);
      81             :             uint32_t avail;
      82        5950 :             const bool send_keepalive = now_ms - p.last_send_ms > 500;
      83        5950 :             const auto *ptr = p.writebuffer->readptr(avail);
      84        5950 :             if (!send_keepalive && (ptr == nullptr || avail <= 0)) {
      85        5800 :                 continue;
      86             :             }
      87         150 :             n = MIN(avail, sizeof(pkt.buffer.data));
      88         150 :             pkt.target_node = p.node;
      89         150 :             switch (p.state.protocol) {
      90           0 :                 case AP_SerialManager::SerialProtocol_MAVLink:
      91           0 :                     pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_MAVLINK;
      92           0 :                     break;
      93         150 :                 case AP_SerialManager::SerialProtocol_MAVLink2:
      94         150 :                     pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_MAVLINK2;
      95         150 :                     break;
      96           0 :                 case AP_SerialManager::SerialProtocol_GPS:
      97             :                 case AP_SerialManager::SerialProtocol_GPS2: // is not in SERIAL1_PROTOCOL option list, but could be entered by user
      98           0 :                     pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_GPS_GENERIC;
      99           0 :                     break;
     100           0 :                 default:
     101           0 :                     pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_UNDEFINED;
     102             :             }
     103         150 :             pkt.buffer.len = n;
     104         150 :             pkt.baudrate = p.baudrate;
     105         150 :             pkt.serial_id = p.idx;
     106         150 :             pkt.options = UAVCAN_TUNNEL_TARGETTED_OPTION_LOCK_PORT;
     107         150 :             if (ptr != nullptr) {
     108          80 :                 memcpy(pkt.buffer.data, ptr, n);
     109             :             }
     110        5950 :         }
     111         150 :         if (targetted->broadcast(pkt)) {
     112         150 :             WITH_SEMAPHORE(p.sem);
     113         150 :             p.writebuffer->advance(n);
     114         150 :             p.tx_stats_bytes += n;
     115         150 :             p.last_send_ms = now_ms;
     116         150 :         }
     117             :     }
     118       21794 : }
     119             : 
     120             : /*
     121             :   handle incoming tunnel serial packet
     122             :  */
     123           0 : void AP_DroneCAN_Serial::handle_tunnel_targetted(AP_DroneCAN *dronecan,
     124             :                                                  const CanardRxTransfer& transfer,
     125             :                                                  const uavcan_tunnel_Targetted &msg)
     126             : {
     127           0 :     uint8_t driver_index = dronecan->get_driver_index();
     128           0 :     if (driver_index >= ARRAY_SIZE(serial) || serial[driver_index] == nullptr) {
     129           0 :         return;
     130             :     }
     131           0 :     auto &s = *serial[driver_index];
     132           0 :     for (auto &p : s.ports) {
     133           0 :         if (p.idx == msg.serial_id && transfer.source_node_id == p.node) {
     134           0 :             WITH_SEMAPHORE(p.sem);
     135           0 :             if (p.readbuffer != nullptr) {
     136           0 :                 const uint32_t written = p.readbuffer->write(msg.buffer.data, msg.buffer.len);
     137           0 :                 p.rx_stats_bytes += msg.buffer.len;
     138           0 :                 p.rx_stats_dropped_bytes += msg.buffer.len - written;
     139           0 :                 p.last_recv_us = AP_HAL::micros64();
     140             :             }
     141           0 :             break;
     142           0 :         }
     143             :     }
     144             : }
     145             : 
     146             : /*
     147             :   initialise port
     148             :  */
     149           2 : void AP_DroneCAN_Serial::Port::init(void)
     150             : {
     151           2 :     baudrate = AP_SerialManager::map_baudrate(state.baud);
     152           2 :     begin(baudrate, 0, 0);
     153           2 : }
     154             : 
     155             : /*
     156             :   available space in outgoing buffer
     157             :  */
     158         921 : uint32_t AP_DroneCAN_Serial::Port::txspace(void)
     159             : {
     160         921 :     WITH_SEMAPHORE(sem);
     161         921 :     return writebuffer != nullptr ? writebuffer->space() : 0;
     162         921 : }
     163             : 
     164           6 : void AP_DroneCAN_Serial::Port::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
     165             : {
     166           6 :     rxS = MAX(rxS, AP_DRONECAN_SERIAL_MIN_RXSIZE);
     167           6 :     txS = MAX(txS, AP_DRONECAN_SERIAL_MIN_TXSIZE);
     168           6 :     init_buffers(rxS, txS);
     169           6 :     if (b != 0) {
     170           6 :         baudrate = b;
     171             :     }
     172           6 : }
     173             : 
     174         276 : size_t AP_DroneCAN_Serial::Port::_write(const uint8_t *buffer, size_t size)
     175             : {
     176         276 :     WITH_SEMAPHORE(sem);
     177         276 :     return writebuffer != nullptr ? writebuffer->write(buffer, size) : 0;
     178         276 : }
     179             : 
     180           0 : ssize_t AP_DroneCAN_Serial::Port::_read(uint8_t *buffer, uint16_t count)
     181             : {
     182           0 :     WITH_SEMAPHORE(sem);
     183           0 :     return readbuffer != nullptr ? readbuffer->read(buffer, count) : -1;
     184           0 : }
     185             : 
     186       26642 : uint32_t AP_DroneCAN_Serial::Port::_available()
     187             : {
     188       26642 :     WITH_SEMAPHORE(sem);
     189       26642 :     return readbuffer != nullptr ? readbuffer->available() : 0;
     190       26642 : }
     191             : 
     192             : 
     193           0 : bool AP_DroneCAN_Serial::Port::_discard_input()
     194             : {
     195           0 :     WITH_SEMAPHORE(sem);
     196           0 :     if (readbuffer != nullptr) {
     197           0 :         rx_stats_dropped_bytes += readbuffer->available();
     198           0 :         readbuffer->clear();
     199             :     }
     200           0 :     return true;
     201           0 : }
     202             : 
     203             : /*
     204             :   initialise read/write buffers
     205             :  */
     206           6 : bool AP_DroneCAN_Serial::Port::init_buffers(const uint32_t size_rx, const uint32_t size_tx)
     207             : {
     208           6 :     if (size_tx == last_size_tx &&
     209           4 :         size_rx == last_size_rx) {
     210           4 :         return true;
     211             :     }
     212           2 :     WITH_SEMAPHORE(sem);
     213           2 :     if (readbuffer == nullptr) {
     214           2 :         readbuffer = NEW_NOTHROW ByteBuffer(size_rx);
     215             :     } else {
     216           0 :         readbuffer->set_size_best(size_rx);
     217             :     }
     218           2 :     if (writebuffer == nullptr) {
     219           2 :         writebuffer = NEW_NOTHROW ByteBuffer(size_tx);
     220             :     } else {
     221           0 :         writebuffer->set_size_best(size_tx);
     222             :     }
     223           2 :     last_size_rx = size_rx;
     224           2 :     last_size_tx = size_tx;
     225           2 :     return readbuffer != nullptr && writebuffer != nullptr;
     226           2 : }
     227             : 
     228             : /*
     229             :   return timestamp estimate in microseconds for when the start of
     230             :   a nbytes packet arrived on the uart.
     231             : */
     232           0 : uint64_t AP_DroneCAN_Serial::Port::receive_time_constraint_us(uint16_t nbytes)
     233             : {
     234           0 :     WITH_SEMAPHORE(sem);
     235           0 :     uint64_t last_receive_us = last_recv_us;
     236           0 :     if (baudrate > 0) {
     237             :         // assume 10 bits per byte. 
     238           0 :         uint32_t transport_time_us = (1000000UL * 10UL / baudrate) * (nbytes+available());
     239           0 :         last_receive_us -= transport_time_us;
     240             :     }
     241           0 :     return last_receive_us;
     242           0 : }
     243             : 
     244             : #endif // HAL_ENABLE_DRONECAN_DRIVERS && AP_DRONECAN_SERIAL_ENABLED