AP_HAL_ChibiOS: provide start time and timeout to all dshot APIs that…

… require it

correct timeout checking for dshot across timer wrap boundaries
fix trigger_groups timeout checks
use rcout_timer_t instead of uint32_t or uint64_t

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
 }
 

libraries/AP_HAL_ChibiOS/RCOutput.h

@@ -38,6 +38,14 @@ typedef uint32_t dmar_uint_t;
 typedef int32_t dmar_int_t;
 #endif
 
+#ifdef AP_RCOUT_USE_32BIT_TIME
+typedef uint32_t rcout_timer_t;
+#define rcout_micros() AP_HAL::micros()
+#else
+typedef uint64_t rcout_timer_t;
+#define rcout_micros() AP_HAL::micros64()
+#endif
+
 #define RCOU_DSHOT_TIMING_DEBUG 0
 
 class ChibiOS::RCOutput : public AP_HAL::RCOutput
@@ -104,12 +112,12 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
     /*
       timer push (for oneshot min rate)
      */
-    void timer_tick(uint64_t last_run_us);
+    void timer_tick(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us);
 
     /*
       LED push
      */
-    void led_timer_tick(uint64_t last_run_us);
+    void led_timer_tick(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us);
 
 #if defined(IOMCU_FW) && HAL_DSHOT_ENABLED
     void timer_tick() override;
@@ -355,9 +363,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
         uint32_t bit_width_mul;
         uint32_t rc_frequency;
         bool in_serial_dma;
-        uint64_t last_dmar_send_us;
-        uint64_t dshot_pulse_time_us;
-        uint64_t dshot_pulse_send_time_us;
+        rcout_timer_t last_dmar_send_us;
+        rcout_timer_t dshot_pulse_time_us;
+        rcout_timer_t dshot_pulse_send_time_us;
         virtual_timer_t dma_timeout;
 #if HAL_SERIALLED_ENABLED
         // serial LED support
@@ -406,7 +414,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
 #if RCOU_DSHOT_TIMING_DEBUG
             uint16_t telem_rate[4];
             uint16_t telem_err_rate[4];
-            uint64_t last_print;  // debug
+            rcout_timer_t last_print;  // debug
 #endif
         } bdshot;
 
@@ -564,7 +572,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
     } _bdshot;
 
     // dshot period
-    uint32_t _dshot_period_us = 400;
+    rcout_timer_t _dshot_period_us = 400;
     // dshot rate as a multiple of loop rate or 0 for 1Khz
     uint8_t _dshot_rate;
     // dshot periods since the last push()
@@ -605,8 +613,8 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
     // mask of active ESCs
     uint32_t _active_escs_mask;
 
-    // min time to trigger next pulse to prevent overlap
-    uint64_t min_pulse_trigger_us;
+    // last time pulse was triggererd used to prevent overlap
+    rcout_timer_t last_pulse_trigger_us;
 
     // mutex for oneshot triggering
     mutex_t trigger_mutex;
@@ -687,20 +695,20 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
     static const eventmask_t EVT_PWM_SEND  = EVENT_MASK(11);
     static const eventmask_t EVT_PWM_SYNTHETIC_SEND  = EVENT_MASK(13);
 
-    void dshot_send_groups(uint64_t time_out_us);
-    void dshot_send(pwm_group &group, uint64_t time_out_us);
+    void dshot_send_groups(rcout_timer_t cycle_start_us, rcout_timer_t timeout_us);
+    void dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_timer_t timeout_us);
     bool dshot_send_command(pwm_group &group, uint8_t command, uint8_t chan);
     static void dshot_update_tick(virtual_timer_t*, void* p);
     static void dshot_send_next_group(void* p);
     // release locks on the groups that are pending in reverse order
-    sysinterval_t calc_ticks_remaining(pwm_group &group, uint64_t time_out_us, uint32_t output_period_us);
-    void dshot_collect_dma_locks(uint64_t last_run_us, bool led_thread = false);
+    sysinterval_t calc_ticks_remaining(pwm_group &group, rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us, rcout_timer_t output_period_us);
+    void dshot_collect_dma_locks(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us, bool led_thread = false);
     static void dma_up_irq_callback(void *p, uint32_t flags);
     static void dma_unlock(virtual_timer_t*, void *p);
     void dma_cancel(pwm_group& group);
     bool mode_requires_dma(enum output_mode mode) const;
     bool 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,
+                         const uint16_t buffer_length, rcout_timer_t pulse_time_us,
                          bool at_least_freq);
     void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length);
     void set_group_mode(pwm_group &group);

libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp

@@ -576,7 +576,7 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan)
 #endif
     }
 #if !defined(IOMCU_FW)
-    uint64_t now = AP_HAL::micros64();
+    rcout_timer_t now = rcout_micros();
     if (chan == DEBUG_CHANNEL && (now  - group.bdshot.last_print) > 1000000) {
         hal.console->printf("TELEM: %d <%d Hz, %.1f%% err>", group.bdshot.erpm[chan], group.bdshot.telem_rate[chan],
             100.0f * float(group.bdshot.telem_err_rate[chan]) / (group.bdshot.telem_err_rate[chan] + group.bdshot.telem_rate[chan]));