diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 1817c3e6813eb9..d6157f0d96c69e 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/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,18 +254,17 @@ 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; @@ -273,7 +272,7 @@ void RCOutput::rcout_thread() } // 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 } diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 8fbfa6615b81f2..3dc76d6f043266 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/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); diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp index d8fd73fc8f7529..89c25c026f746c 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp +++ b/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])); diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp index c8d535dc5b4559..70959c03e433b3 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp @@ -75,15 +75,17 @@ void RCOutput::rcout_thread() { rcout_thread_ctx = chThdGetSelfX(); chRegSetThreadNameX(rcout_thread_ctx, rcout_thread_name); - uint64_t last_cycle_run_us = 0; + rcout_timer_t last_cycle_run_us = 0; while (true) { chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND); + // start the clock - const uint64_t last_thread_run_us = AP_HAL::micros64(); + const rcout_timer_t last_thread_run_us = rcout_micros(); + // this is when the cycle is supposed to start if (_dshot_cycle == 0) { - 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); @@ -92,10 +94,9 @@ 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; // DMA channel sharing on F10x is complicated. The allocations are // TIM2_UP - (1,2) @@ -124,9 +125,9 @@ void RCOutput::rcout_thread() { // TIM4_CH3 - unlock // TIM4_UP - unlock - dshot_send_groups(time_out_us); + dshot_send_groups(cycle_start_us, timeout_period_us); #if AP_HAL_SHARED_DMA_ENABLED - dshot_collect_dma_locks(time_out_us); + dshot_collect_dma_locks(cycle_start_us, timeout_period_us); #endif if (_dshot_rate > 0) { _dshot_cycle = (_dshot_cycle + 1) % _dshot_rate;