From 726b3bca9d0d748a2fd8d866eee07a5297397e3b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 23 Mar 2023 15:28:56 +1100 Subject: [PATCH] HAL_ChibiOS: switched to 64 bit maths for DShot timings this fixes a timer wrap bug at 71 minutes after boot that impacts bdshot --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 31 ++++++++++++++------------- libraries/AP_HAL_ChibiOS/RCOutput.h | 14 ++++++------ 2 files changed, 23 insertions(+), 22 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 439e29f5fc..53e2b002d0 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -150,8 +150,8 @@ void RCOutput::init() */ void RCOutput::rcout_thread() { - uint32_t last_thread_run_us = 0; // last time we did a 1kHz run of rcout - uint32_t last_cycle_run_us = 0; + uint64_t last_thread_run_us = 0; // last time we did a 1kHz run of rcout + uint64_t last_cycle_run_us = 0; rcout_thread_ctx = chThdGetSelfX(); @@ -166,11 +166,11 @@ void RCOutput::rcout_thread() const auto mask = chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND | EVT_LED_SEND); const bool have_pwm_event = (mask & (EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND)) != 0; // start the clock - last_thread_run_us = AP_HAL::micros(); + last_thread_run_us = AP_HAL::micros64(); // this is when the cycle is supposed to start if (_dshot_cycle == 0 && have_pwm_event) { - last_cycle_run_us = AP_HAL::micros(); + last_cycle_run_us = AP_HAL::micros64(); // 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); @@ -179,7 +179,7 @@ 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 - uint32_t time_out_us = (_dshot_cycle + 1) * _dshot_period_us + last_cycle_run_us; + 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; } @@ -226,7 +226,7 @@ __RAMFUNC__ void RCOutput::dshot_update_tick(void* p) #if AP_HAL_SHARED_DMA_ENABLED // release locks on the groups that are pending in reverse order -void RCOutput::dshot_collect_dma_locks(uint32_t time_out_us) +void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us) { if (NUM_GROUPS == 0) { return; @@ -235,10 +235,10 @@ void RCOutput::dshot_collect_dma_locks(uint32_t time_out_us) pwm_group &group = pwm_group_list[i]; if (group.dma_handle != nullptr && group.dma_handle->is_locked()) { // calculate how long we have left - uint32_t now = AP_HAL::micros(); + uint64_t now = AP_HAL::micros64(); // 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; + 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; @@ -1220,14 +1220,14 @@ 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(uint32_t time_out_us) +void RCOutput::timer_tick(uint64_t time_out_us) { if (serial_group) { return; } // if we have enough time left send out LED data - if (serial_led_pending && (time_out_us > (AP_HAL::micros() + (_dshot_period_us >> 1)))) { + if (serial_led_pending && (time_out_us > (AP_HAL::micros64() + (_dshot_period_us >> 1)))) { serial_led_pending = false; for (auto &group : pwm_group_list) { serial_led_pending |= !serial_led_send(group); @@ -1241,7 +1241,7 @@ void RCOutput::timer_tick(uint32_t time_out_us) return; } - uint32_t now = AP_HAL::micros(); + uint64_t now = AP_HAL::micros64(); if (now > min_pulse_trigger_us && now - min_pulse_trigger_us > 4000) { @@ -1251,7 +1251,7 @@ void RCOutput::timer_tick(uint32_t time_out_us) } // send dshot for all groups that support it -void RCOutput::dshot_send_groups(uint32_t time_out_us) +void RCOutput::dshot_send_groups(uint64_t time_out_us) { #ifndef DISABLE_DSHOT if (serial_group) { @@ -1380,7 +1380,7 @@ void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t 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, uint32_t time_out_us) +void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us) { #ifndef DISABLE_DSHOT if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) { @@ -1394,7 +1394,8 @@ void RCOutput::dshot_send(pwm_group &group, uint32_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::micros() + group.dshot_pulse_time_us > time_out_us) { + + if (AP_HAL::micros64() + group.dshot_pulse_time_us > time_out_us) { group.dma_handle->unlock(); return; } @@ -1604,7 +1605,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::micros(); + group.last_dmar_send_us = AP_HAL::micros64(); #endif //#ifndef DISABLE_DSHOT } diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index f3e0894425..6ee05e85cd 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -102,7 +102,7 @@ public: /* timer push (for oneshot min rate) */ - void timer_tick(uint32_t last_run_us); + void timer_tick(uint64_t last_run_us); /* setup for serial output to a set of ESCs, using the given @@ -320,9 +320,9 @@ private: uint32_t bit_width_mul; uint32_t rc_frequency; bool in_serial_dma; - uint32_t last_dmar_send_us; - uint32_t dshot_pulse_time_us; - uint32_t dshot_pulse_send_time_us; + uint64_t last_dmar_send_us; + uint64_t dshot_pulse_time_us; + uint64_t dshot_pulse_send_time_us; virtual_timer_t dma_timeout; // serial LED support @@ -599,13 +599,13 @@ private: uint16_t create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem); void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul); - void dshot_send_groups(uint32_t time_out_us); - void dshot_send(pwm_group &group, uint32_t time_out_us); + void dshot_send_groups(uint64_t time_out_us); + void dshot_send(pwm_group &group, uint64_t time_out_us); bool dshot_send_command(pwm_group &group, uint8_t command, uint8_t chan); static void dshot_update_tick(void* p); static void dshot_send_next_group(void* p); // release locks on the groups that are pending in reverse order - void dshot_collect_dma_locks(uint32_t last_run_us); + void dshot_collect_dma_locks(uint64_t last_run_us); static void dma_up_irq_callback(void *p, uint32_t flags); static void dma_unlock(void *p); void dma_cancel(pwm_group& group);