Safe wrap timeouts in dshot

libraries/AP_HAL/system.h

@@ -22,4 +22,32 @@ uint64_t millis64();
 void dump_stack_trace();
 void dump_core_file();
 
+// Function to reliably determine whether a timeout has expired using any unsigned time type and interval
+// The template makes sure that callers do not mix up different types for storing time or assessing the
+// current time.
+// The comparison is guaranteed to work even if the present and the past cross a wrap boundary
+template <typename T, typename S, typename R>
+inline bool timeout_expired(const T past_time, const S now, const R timeout)
+{
+    static_assert(std::is_same<T, S>::value, "timeout_expired() must compare values of the same unsigned type");
+    static_assert(std::is_unsigned<T>::value, "timeout_expired() must use unsigned times");
+    static_assert(std::is_unsigned<R>::value, "timeout_expired() must use unsigned timeouts");
+    const T dt = now - past_time;
+    return (dt >= timeout);
+}
+
+// Function to reliably determine whether how much of a timeout is left using any unsigned time type and interval
+// The template makes sure that callers do not mix up different types for storing time or assessing the
+// current time.
+// The comparison is guaranteed to work even if the present and the past cross a wrap boundary
+template <typename T, typename S, typename R>
+inline T timeout_remaining(const T past_time, const S now, const R timeout)
+{
+    static_assert(std::is_same<T, S>::value, "timeout_remaining() must compare values of the same unsigned type");
+    static_assert(std::is_unsigned<T>::value, "timeout_remaining() must use unsigned times");
+    static_assert(std::is_unsigned<R>::value, "timeout_remaining() must use unsigned timeouts");
+    const T dt = now - past_time;
+    return (dt >= timeout) ? T(0) : (timeout - dt);
+}
+
 } // namespace AP_HAL

libraries/AP_HAL/tests/test_timeouts.cpp

@@ -0,0 +1,72 @@
+#include <AP_gtest.h>
+#include <AP_HAL/HAL.h>
+#include <AP_HAL/Util.h>
+#include <AP_Math/AP_Math.h>
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+
+template <typename T>
+static void timeout_test(T timenow, T trigger)
+{
+    T ticks = 2000U;
+    T time_past = timenow;
+    bool timeout_triggered = false;
+
+    while (ticks-- != 0) {
+        timenow++;
+        if (AP_HAL::timeout_expired(time_past, timenow, 1000U)) {
+            if (!timeout_triggered) {
+                ASSERT_EQ(timenow, trigger);
+                timeout_triggered = true;
+            }
+        }
+    }
+
+    ASSERT_TRUE(timeout_triggered);
+}
+
+TEST(timeout_wrap_Test, Basic)
+{
+    timeout_test(0xFFFFFFFFU - 499U, 500U);
+}
+
+TEST(timeout_Test, Basic)
+{
+    timeout_test(500U, 1500U);
+}
+
+template <typename T>
+static void timeout_remaining(T timenow, T incr)
+{
+    T ticks = 2000U;
+    T time_past = timenow;
+    T last_remaining = 1000;
+
+    while (ticks-- != 0) {
+        timenow+=incr;
+        const uint32_t new_remaining = AP_HAL::timeout_remaining(time_past, timenow, 1000U);
+        ASSERT_LT(new_remaining, MAX(last_remaining, 1U));   // time remaining must be going down
+        last_remaining = new_remaining;
+        ASSERT_LT(new_remaining, 1001U);
+    }
+
+    ASSERT_EQ(AP_HAL::timeout_remaining(time_past, timenow, 1000U), 0U);
+}
+
+TEST(timeout_remaining_wrap_Test, Basic)
+{
+    timeout_remaining(0xFFFFFFFFU - 500U, 1U);
+}
+
+TEST(timeout_remaining_Test, Basic)
+{
+    timeout_remaining(500U, 1U);
+}
+
+TEST(timeout_remaining_wrap_Test_5us, Basic)
+{
+    timeout_remaining(0xFFFFFFFF - 500U, 5U);
+}
+
+AP_GTEST_MAIN()

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -214,7 +214,7 @@ void RCOutput::led_thread()
         // actually sending out data - thus we need to work out how much time we have left to collect the locks
 
         // process any pending LED output requests
-        led_timer_tick(LED_OUTPUT_PERIOD_US + AP_HAL::micros64());
+        led_timer_tick(rcout_micros(), LED_OUTPUT_PERIOD_US);
     }
 }
 #endif // HAL_SERIAL_ENABLED
@@ -225,8 +225,8 @@ void RCOutput::led_thread()
 #if !defined(IOMCU_FW)
 void RCOutput::rcout_thread()
 {
-    uint64_t last_thread_run_us = 0; // last time we did a 1kHz run of rcout
-    uint64_t last_cycle_run_us = 0;
+    rcout_timer_t last_thread_run_us = 0; // last time we did a 1kHz run of rcout
+    rcout_timer_t last_cycle_run_us = 0;
 
     rcout_thread_ctx = chThdGetSelfX();
 
@@ -241,11 +241,11 @@ void RCOutput::rcout_thread()
         const auto mask = chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND);
         const bool have_pwm_event = (mask & (EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND)) != 0;
         // start the clock
-        last_thread_run_us = AP_HAL::micros64();
+        last_thread_run_us = rcout_micros();
 
         // this is when the cycle is supposed to start
         if (_dshot_cycle == 0 && have_pwm_event) {
-            last_cycle_run_us = AP_HAL::micros64();
+            last_cycle_run_us = rcout_micros();
             // register a timer for the next tick if push() will not be providing it
             if (_dshot_rate != 1) {
                 chVTSet(&_dshot_rate_timer, chTimeUS2I(_dshot_period_us), dshot_update_tick, this);
@@ -254,26 +254,25 @@ void RCOutput::rcout_thread()
 
         // if DMA sharing is in effect there can be quite a delay between the request to begin the cycle and
         // actually sending out data - thus we need to work out how much time we have left to collect the locks
-        uint64_t time_out_us = (_dshot_cycle + 1) * _dshot_period_us + last_cycle_run_us;
-        if (!_dshot_rate) {
-            time_out_us = last_thread_run_us + _dshot_period_us;
-        }
+        const rcout_timer_t timeout_period_us = _dshot_rate ? (_dshot_cycle + 1) * _dshot_period_us : _dshot_period_us;
+        // timeout is measured from the beginning of the push() that initiated it to preserve periodicity
+        const rcout_timer_t cycle_start_us = _dshot_rate ? last_cycle_run_us : last_thread_run_us;
 
         // main thread requested a new dshot send or we timed out - if we are not running
         // as a multiple of loop rate then ignore EVT_PWM_SEND events to preserve periodicity
         if (!in_soft_serial() && have_pwm_event) {
-            dshot_send_groups(time_out_us);
+            dshot_send_groups(cycle_start_us, timeout_period_us);
 
             // now unlock everything
-            dshot_collect_dma_locks(time_out_us);
+            dshot_collect_dma_locks(cycle_start_us, timeout_period_us);
 
             if (_dshot_rate > 0) {
                 _dshot_cycle = (_dshot_cycle + 1) % _dshot_rate;
             }
         }
 
         // process any pending RC output requests
-        timer_tick(time_out_us);
+        timer_tick(cycle_start_us, timeout_period_us);
 #if RCOU_DSHOT_TIMING_DEBUG
         static bool output_masks = true;
         if (AP_HAL::millis() > 5000 && output_masks) {
@@ -302,34 +301,32 @@ __RAMFUNC__ void RCOutput::dshot_update_tick(virtual_timer_t* vt, void* p)
 
 #if AP_HAL_SHARED_DMA_ENABLED
 // calculate how much time remains in the current cycle
-sysinterval_t RCOutput::calc_ticks_remaining(pwm_group &group, uint64_t time_out_us, uint32_t output_period_us)
+sysinterval_t RCOutput::calc_ticks_remaining(pwm_group &group, rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us, rcout_timer_t output_period_us)
 {
     // calculate how long we have left
-    uint64_t now = AP_HAL::micros64();
+    rcout_timer_t now = rcout_micros();
     // if we have time left wait for the event
-    const uint64_t pulse_elapsed_us = now - group.last_dmar_send_us;
-    uint32_t wait_us = 0;
-    if (now < time_out_us) {
-        wait_us = time_out_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 rcout_timer_t pulse_remaining_us
+        = AP_HAL::timeout_remaining(group.last_dmar_send_us, now, group.dshot_pulse_send_time_us);
+    const rcout_timer_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
+    rcout_timer_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
     // as minimum. Don't allow for a very long delay (over _dshot_period_us)
     // to prevent bugs in handling timer wrap
-    const uint32_t max_delay_us = output_period_us;
-    const uint32_t min_delay_us = 10; // matches our CH_CFG_ST_TIMEDELTA
+    const rcout_timer_t max_delay_us = output_period_us;
+    const rcout_timer_t min_delay_us = 10; // matches our CH_CFG_ST_TIMEDELTA
     wait_us = constrain_uint32(wait_us, min_delay_us, max_delay_us);
 
     return MIN(TIME_MAX_INTERVAL, chTimeUS2I(wait_us));
 }
 
 // release locks on the groups that are pending in reverse order
-void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread)
+void RCOutput::dshot_collect_dma_locks(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us, bool led_thread)
 {
     if (NUM_GROUPS == 0) {
         return;
@@ -344,7 +341,7 @@ void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread)
         // dma handle will only be unlocked if the send was aborted
         if (group.dma_handle != nullptr && group.dma_handle->is_locked()) {
             // if we have time left wait for the event
-            const sysinterval_t wait_ticks = calc_ticks_remaining(group, time_out_us,
+            const sysinterval_t wait_ticks = calc_ticks_remaining(group, cycle_start_us, timeout_period_us,
                                                                   led_thread ? LED_OUTPUT_PERIOD_US : _dshot_period_us);
             const eventmask_t mask = chEvtWaitOneTimeout(group.dshot_event_mask, wait_ticks);
 
@@ -829,7 +826,7 @@ void RCOutput::push_local(void)
     if (widest_pulse > 2300) {
         widest_pulse = 2300;
     }
-    trigger_widest_pulse = widest_pulse;
+    trigger_widest_pulse = widest_pulse + 50;
 
     trigger_groupmask = need_trigger;
 
@@ -931,7 +928,7 @@ void RCOutput::print_group_setup_error(pwm_group &group, const char* error_strin
   This is used for both DShot and serial output
  */
 bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high, const uint16_t buffer_length,
-                               uint32_t pulse_time_us, bool at_least_freq)
+                               rcout_timer_t pulse_time_us, bool at_least_freq)
 {
 #if HAL_DSHOT_ENABLED
     // for dshot we setup for DMAR based output
@@ -1362,15 +1359,17 @@ bool RCOutput::enable_px4io_sbus_out(uint16_t rate_hz)
 /*
   trigger output groups for oneshot or dshot modes
  */
-void RCOutput::trigger_groups(void)
+void RCOutput::trigger_groups()
 {
     if (!chMtxTryLock(&trigger_mutex)) {
         return;
     }
-    uint64_t now = AP_HAL::micros64();
-    if (now < min_pulse_trigger_us) {
+
+    rcout_timer_t now = rcout_micros();
+
+    if (!AP_HAL::timeout_expired(last_pulse_trigger_us, now, trigger_widest_pulse)) {
         // guarantee minimum pulse separation
-        hal.scheduler->delay_microseconds(min_pulse_trigger_us - now);
+        hal.scheduler->delay_microseconds(AP_HAL::timeout_remaining(last_pulse_trigger_us, now, trigger_widest_pulse));
     }
 
     osalSysLock();
@@ -1399,7 +1398,7 @@ void RCOutput::trigger_groups(void)
       calculate time that we are allowed to trigger next pulse
       to guarantee at least a 50us gap between pulses
     */
-    min_pulse_trigger_us = AP_HAL::micros64() + trigger_widest_pulse + 50;
+    last_pulse_trigger_us = rcout_micros();
 
     chMtxUnlock(&trigger_mutex);
 }
@@ -1408,20 +1407,17 @@ void RCOutput::trigger_groups(void)
   periodic timer. This is used for oneshot and dshot modes, plus for
   safety switch update. Runs every 1000us.
  */
-void RCOutput::timer_tick(uint64_t time_out_us)
+void RCOutput::timer_tick(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
 {
     if (in_soft_serial()) {
         return;
     }
 
-    if (min_pulse_trigger_us == 0) {
+    if (last_pulse_trigger_us == 0) {
         return;
     }
 
-    uint64_t now = AP_HAL::micros64();
-
-    if (now > min_pulse_trigger_us &&
-        now - min_pulse_trigger_us > 4000) {
+    if (AP_HAL::timeout_expired(last_pulse_trigger_us, rcout_micros(), trigger_widest_pulse + 4000U)) {
         // trigger at a minimum of 250Hz
         trigger_groups();
     }
@@ -1430,7 +1426,7 @@ void RCOutput::timer_tick(uint64_t time_out_us)
 /*
   periodic timer called from led thread. This is used for LED output
  */
-void RCOutput::led_timer_tick(uint64_t time_out_us)
+void RCOutput::led_timer_tick(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
 {
     if (in_soft_serial()) {
         return;
@@ -1444,12 +1440,12 @@ void RCOutput::led_timer_tick(uint64_t time_out_us)
         }
 
         // release locks on the groups that are pending in reverse order
-        dshot_collect_dma_locks(time_out_us, true);
+        dshot_collect_dma_locks(cycle_start_us, timeout_period_us, true);
     }
 }
 
 // send dshot for all groups that support it
-void RCOutput::dshot_send_groups(uint64_t time_out_us)
+void RCOutput::dshot_send_groups(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
 {
 #if HAL_DSHOT_ENABLED
     if (in_soft_serial()) {
@@ -1472,13 +1468,13 @@ void RCOutput::dshot_send_groups(uint64_t time_out_us)
             pulse_sent = true;
         // actually do a dshot send
         } else if (group.can_send_dshot_pulse()) {
-            dshot_send(group, time_out_us);
+            dshot_send(group, cycle_start_us, timeout_period_us);
             pulse_sent = true;
         }
 #if defined(HAL_WITH_BIDIR_DSHOT) && defined(HAL_TIM_UP_SHARED)
         // prevent the next send going out until the previous send has released its DMA channel
         if (pulse_sent && group.shared_up_dma && group.bdshot.enabled) {
-            chEvtWaitOneTimeout(DSHOT_CASCADE, calc_ticks_remaining(group, time_out_us, _dshot_period_us));
+            chEvtWaitOneTimeout(DSHOT_CASCADE, calc_ticks_remaining(group, cycle_start_us, timeout_period_us, _dshot_period_us));
         }
 #else
         (void)pulse_sent;
@@ -1611,7 +1607,7 @@ void RCOutput::fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16
   This call be called in blocking mode from the timer, in which case it waits for the DMA lock.
   In normal operation it doesn't wait for the DMA lock.
  */
-void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)
+void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
 {
 #if HAL_DSHOT_ENABLED
     if (soft_serial_waiting() || !is_dshot_send_allowed(group.dshot_state)) {
@@ -1627,7 +1623,7 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)
     // 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_SHARED_DMA_ENABLED
-    if (AP_HAL::micros64() + group.dshot_pulse_time_us > time_out_us) {
+    if (AP_HAL::timeout_remaining(cycle_start_us, rcout_micros(), timeout_period_us) < group.dshot_pulse_time_us) {
         group.dma_handle->unlock();
         return;
     }
@@ -1820,7 +1816,7 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length)
 
     dmaStreamEnable(group.dma);
     // record when the transaction was started
-    group.last_dmar_send_us = AP_HAL::micros64();
+    group.last_dmar_send_us = rcout_micros();
 #endif // HAL_DSHOT_ENABLED
 }