AP_HAL_ChibiOS: correct timeout checking for dshot across timer wrap …

…boundaries
ArduPilot · Mar 23, 2023 · 51e4e22 · 51e4e22
1 parent c634118
commit 51e4e22
Showing 1 changed file with 10 additions and 12 deletions.
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -237,15 +237,12 @@ void RCOutput::dshot_collect_dma_locks(uint32_t cycle_start_us, uint32_t timeout
             uint32_t now = AP_HAL::micros();
             // if we have time left wait for the event
             eventmask_t mask = 0;
-            const uint32_t pulse_elapsed_us = now - group.last_dmar_send_us;
-            uint32_t wait_us = 0;
-            if (now < (cycle_start_us + timeout_period_us)) {
-                wait_us = (cycle_start_us + timeout_period_us) - now;
-            }
-            if (pulse_elapsed_us < group.dshot_pulse_send_time_us) {
-                // better to let the burst write in progress complete rather than cancelling mid way through
-                wait_us = MAX(wait_us, group.dshot_pulse_send_time_us - pulse_elapsed_us);
-            }
+            const uint32_t pulse_remaining_us
+                = AP_HAL::timeout_remaining(group.last_dmar_send_us, now, group.dshot_pulse_send_time_us);
+            const uint32_t timeout_remaining_us
+                = AP_HAL::timeout_remaining(cycle_start_us, now, timeout_period_us);
+            // better to let the burst write in progress complete rather than cancelling mid way through
+            uint32_t wait_us = MAX(pulse_remaining_us, timeout_remaining_us);
 
             // waiting for a very short period of time can cause a
             // timer wrap with ChibiOS timers. Use CH_CFG_ST_TIMEDELTA
@@ -1226,7 +1223,8 @@ void RCOutput::timer_tick(uint32_t cycle_start_us, uint32_t timeout_period_us)
     }
 
     // if we have enough time left send out LED data
-    if (serial_led_pending && ((cycle_start_us + timeout_period_us) > (AP_HAL::micros() + (_dshot_period_us >> 1)))) {
+    uint32_t now = AP_HAL::micros();
+    if (serial_led_pending && AP_HAL::timeout_remaining(cycle_start_us, now, timeout_period_us) > (_dshot_period_us >> 1)) {
         serial_led_pending = false;
         for (auto &group : pwm_group_list) {
             serial_led_pending |= !serial_led_send(group);
@@ -1240,7 +1238,7 @@ void RCOutput::timer_tick(uint32_t cycle_start_us, uint32_t timeout_period_us)
         return;
     }
 
-    uint32_t now = AP_HAL::micros();
+    now = AP_HAL::micros();
 
     if (now > min_pulse_trigger_us &&
         now - min_pulse_trigger_us > 4000) {
@@ -1393,7 +1391,7 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t cycle_start_us, uint32_t ti
 
     // if we are sharing UP channels then it might have taken a long time to get here,
     // if there's not enough time to actually send a pulse then cancel
-    if (AP_HAL::micros() + group.dshot_pulse_time_us > (cycle_start_us + timeout_period_us)) {
+    if (AP_HAL::timeout_remaining_us(cycle_start_us, timeout_period_us) < group.dshot_pulse_time_us) {
         group.dma_handle->unlock();
         return;
     }