AP_DroneCAN: add support for detecting downed link

bugobliterator · Oct 18, 2023 · fd8e09c · fd8e09c
1 parent cc799d3
commit fd8e09c
Showing 1 changed file with 23 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,18 @@ void CanardInterface::processTx(bool raw_commands_only = false) {
         if (txq == nullptr) {
             return;
         }
+        const auto *stats = ifaces[iface]->get_statistics();
+        bool iface_down = true;
+        if (stats == nullptr || (AP_HAL::micros64() - stats->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 a successful transmit in the
+            last 500 milliseconds
+            */
+            iface_down = false;
+        } 
         // scan through list of pending transfers
         while (true) {
             auto txf = &txq->frame;
@@ -240,15 +252,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