AP_HAL_ChibiOS: GPIO: retry pins after ISR flood and add arming check

libraries/AP_HAL_ChibiOS/GPIO.cpp

@@ -45,10 +45,12 @@ static struct gpio_entry {
     uint8_t pwm_num;
     ioline_t pal_line;
     AP_HAL::GPIO::irq_handler_fn_t fn; // callback for GPIO interface
+    thread_reference_t thd_wait;
     bool is_input;
     uint8_t mode;
-    thread_reference_t thd_wait;
     uint16_t isr_quota;
+    uint8_t isr_disabled_ticks;
+    AP_HAL::GPIO::INTERRUPT_TRIGGER_TYPE isr_mode;
 } _gpio_tab[] = HAL_GPIO_PINS;
 
 /*
@@ -292,6 +294,8 @@ bool GPIO::attach_interrupt(uint8_t pin,
     if (!g) {
         return false;
     }
+    g->isr_disabled_ticks = 0;
+    g->isr_quota = 0;
     if (!_attach_interrupt(g->pal_line,
                            palcallback_t(fn?pal_interrupt_cb_functor:nullptr),
                            g,
@@ -299,6 +303,7 @@ bool GPIO::attach_interrupt(uint8_t pin,
         return false;
     }
     g->fn = fn;
+    g->isr_mode = mode;
     return true;
 }
 
@@ -317,6 +322,9 @@ bool GPIO::attach_interrupt(uint8_t pin,
     if (!g) {
         return false;
     }
+    g->isr_disabled_ticks = 0;
+    g->isr_quota = 0;
+    g->isr_mode = mode;
     return _attach_interrupt(g->pal_line, proc, mode);
 }
 
@@ -575,16 +583,62 @@ void GPIO::timer_tick()
     // 10k per 100ms call to timer_tick()
     const uint16_t quota = 10000U;
     for (uint8_t i=0; i<ARRAY_SIZE(_gpio_tab); i++) {
-        if (_gpio_tab[i].isr_quota == 1) {
+        if (_gpio_tab[i].isr_quota != 1) {
+            // Reset quota for next tick
+            _gpio_tab[i].isr_quota = quota;
+            continue;
+        }
         // we ran out of ISR quota for this pin since the last
         // check. This is not really an internal error, but we use
         // INTERNAL_ERROR() to get the reporting mechanism
+
+        if (_gpio_tab[i].isr_disabled_ticks == 0) {
 #ifndef HAL_NO_UARTDRIVER
             GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"ISR flood on pin %u", _gpio_tab[i].pin_num);
 #endif
+            // Only trigger internal error if armed
+            if (hal.util->get_soft_armed()) {
                 INTERNAL_ERROR(AP_InternalError::error_t::gpio_isr);
             }
-        _gpio_tab[i].isr_quota = quota;
         }
+        if (hal.util->get_soft_armed()) {
+            // Don't start counting until disarmed
+            _gpio_tab[i].isr_disabled_ticks = 1;
+            continue;
+        }
+
+        // Increment disabled ticks, don't wrap
+        if (_gpio_tab[i].isr_disabled_ticks < UINT8_MAX) {
+            _gpio_tab[i].isr_disabled_ticks++;
+        }
+
+        // 100 * 100ms = 10 seconds
+        const uint8_t ISR_retry_ticks = 100U;
+        if ((_gpio_tab[i].isr_disabled_ticks > ISR_retry_ticks) && (_gpio_tab[i].fn != nullptr)) {
+            // Try re-enabling
+#ifndef HAL_NO_UARTDRIVER
+            GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Retrying pin %d after ISR flood", _gpio_tab[i].pin_num);
+#endif
+            if (attach_interrupt(_gpio_tab[i].pin_num, _gpio_tab[i].fn, _gpio_tab[i].isr_mode)) {
+                // Success, reset quota
+                _gpio_tab[i].isr_quota = quota;
+            } else {
+                // Failed, reset disabled count to try again later
+                _gpio_tab[i].isr_disabled_ticks = 1;
+            }
+        }
+    }
+}
+
+// Check for ISR floods
+bool GPIO::arming_checks(size_t buflen, char *buffer) const
+{
+    for (uint8_t i=0; i<ARRAY_SIZE(_gpio_tab); i++) {
+        if (_gpio_tab[i].isr_disabled_ticks != 0) {
+            hal.util->snprintf(buffer, buflen, "Pin %u disabled (ISR flood)", _gpio_tab[i].pin_num);
+            return false;
+        }
+    }
+    return true;
 }
 #endif // IOMCU_FW

libraries/AP_HAL_ChibiOS/GPIO.h

@@ -77,6 +77,9 @@ public:
 #ifndef IOMCU_FW
     // timer tick
     void timer_tick(void) override;
+
+    // Check for ISR floods
+    bool arming_checks(size_t buflen, char *buffer) const override;
 #endif
 
     // check if a pin number is valid