AP_DroneCAN: add support for detecting downed link

ArduPilot · Nov 1, 2023 · 7f044d6 · 7f044d6
1 parent cc799d3
commit 7f044d6
Showing 1 changed file with 25 additions and 4 deletions.
diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp
@@ -214,6 +214,20 @@ void CanardInterface::processTx(bool raw_commands_only = false) {
         if (txq == nullptr) {
             return;
         }
+        // volatile as the value can change at any time during can interrupt
+        // we need to ensure that this is not optimized
+        volatile const auto *stats = ifaces[iface]->get_statistics();
+        uint64_t last_transmit_us = stats->last_transmit_us;
+        bool iface_down = true;
+        if (stats == nullptr || (AP_HAL::micros64() - last_transmit_us) < 200000UL) {
+            /*
+            We were not able to queue the frame for
+            sending. Only mark the send as failing if the
+            interface is active. We consider an interface as
+            active if it has had successful transmits for some time.
+            */
+            iface_down = false;
+        } 
         // scan through list of pending transfers
         while (true) {
             auto txf = &txq->frame;
@@ -240,15 +254,22 @@ void CanardInterface::processTx(bool raw_commands_only = false) {
             if (!write) {
                 // if there is no space then we need to start from the
                 // top of the queue, so wait for the next loop
-                break;
-            }
-            if ((txf->iface_mask & (1U<<iface)) && (AP_HAL::micros64() < txf->deadline_usec)) {
+                if (!iface_down) {
+                    break;
+                } else {
+                    txf->iface_mask &= ~(1U<<iface);
+                }
+            } else if ((txf->iface_mask & (1U<<iface)) && (AP_HAL::micros64() < txf->deadline_usec)) {
                 // try sending to interfaces, clearing the mask if we succeed
                 if (ifaces[iface]->send(txmsg, txf->deadline_usec, 0) > 0) {
                     txf->iface_mask &= ~(1U<<iface);
                 } else {
                     // if we fail to send then we try sending on next interface
-                    break;
+                    if (!iface_down) {
+                        break;
+                    } else {
+                        txf->iface_mask &= ~(1U<<iface);
+                    }
                 }
             }
             // look at next transfer