Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: switched to 64 bit maths for DShot timings 

this fixes a timer wrap bug at 71 minutes after boot that impacts
bdshot
This commit is contained in:
Andrew Tridgell 2023-03-23 15:28:56 +11:00
parent b9e9d4cc4f
commit c55a2916f6
2 changed files with 23 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -150,8 +150,8 @@ void RCOutput::init()
*/ */
void RCOutput::rcout_thread() void RCOutput::rcout_thread()
{ {
uint32_t last_thread_run_us = 0; // last time we did a 1kHz run of rcout uint64_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_cycle_run_us = 0;
rcout_thread_ctx = chThdGetSelfX(); 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 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; const bool have_pwm_event = (mask & (EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND)) != 0;
// start the clock // 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 // this is when the cycle is supposed to start
if (_dshot_cycle == 0 && have_pwm_event) { 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 // register a timer for the next tick if push() will not be providing it
if (_dshot_rate != 1) { if (_dshot_rate != 1) {
chVTSet(&_dshot_rate_timer, chTimeUS2I(_dshot_period_us), dshot_update_tick, this); 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 // 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 // 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) { if (!_dshot_rate) {
time_out_us = last_thread_run_us + _dshot_period_us; 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 #if AP_HAL_SHARED_DMA_ENABLED
// release locks on the groups that are pending in reverse order // 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) { if (NUM_GROUPS == 0) {
return; return;
@ -235,10 +235,10 @@ void RCOutput::dshot_collect_dma_locks(uint32_t time_out_us)
pwm_group &group = pwm_group_list[i]; pwm_group &group = pwm_group_list[i];
if (group.dma_handle != nullptr && group.dma_handle->is_locked()) { if (group.dma_handle != nullptr && group.dma_handle->is_locked()) {
// calculate how long we have left // 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 // if we have time left wait for the event
eventmask_t mask = 0; 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; uint32_t wait_us = 0;
if (now < time_out_us) { if (now < time_out_us) {
wait_us = time_out_us - now; 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 periodic timer. This is used for oneshot and dshot modes, plus for
safety switch update. Runs every 1000us. 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) { if (serial_group) {
return; return;
} }
// if we have enough time left send out LED data // 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; serial_led_pending = false;
for (auto &group : pwm_group_list) { for (auto &group : pwm_group_list) {
serial_led_pending |= !serial_led_send(group); serial_led_pending |= !serial_led_send(group);
@ -1241,7 +1241,7 @@ void RCOutput::timer_tick(uint32_t time_out_us)
return; return;
} }
uint32_t now = AP_HAL::micros(); uint64_t now = AP_HAL::micros64();
if (now > min_pulse_trigger_us && if (now > min_pulse_trigger_us &&
now - min_pulse_trigger_us > 4000) { 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 // 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 #ifndef DISABLE_DSHOT
if (serial_group) { 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. 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. 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 #ifndef DISABLE_DSHOT
if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) { 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 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 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(); group.dma_handle->unlock();
return; return;
} }
@ -1604,7 +1605,7 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length)
dmaStreamEnable(group.dma); dmaStreamEnable(group.dma);
// record when the transaction was started // 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 #endif //#ifndef DISABLE_DSHOT
} }

14
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -102,7 +102,7 @@ public:
/* /*
timer push (for oneshot min rate) 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 setup for serial output to a set of ESCs, using the given
@ -320,9 +320,9 @@ private:
uint32_t bit_width_mul; uint32_t bit_width_mul;
uint32_t rc_frequency; uint32_t rc_frequency;
bool in_serial_dma; bool in_serial_dma;
uint32_t last_dmar_send_us; uint64_t last_dmar_send_us;
uint32_t dshot_pulse_time_us; uint64_t dshot_pulse_time_us;
uint32_t dshot_pulse_send_time_us; uint64_t dshot_pulse_send_time_us;
virtual_timer_t dma_timeout; virtual_timer_t dma_timeout;
// serial LED support // serial LED support
@ -599,13 +599,13 @@ private:
uint16_t create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem); 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 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_groups(uint64_t time_out_us);
void dshot_send(pwm_group &group, uint32_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); bool dshot_send_command(pwm_group &group, uint8_t command, uint8_t chan);
static void dshot_update_tick(void* p); static void dshot_update_tick(void* p);
static void dshot_send_next_group(void* p); static void dshot_send_next_group(void* p);
// release locks on the groups that are pending in reverse order // 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_up_irq_callback(void *p, uint32_t flags);
static void dma_unlock(void *p); static void dma_unlock(void *p);
void dma_cancel(pwm_group& group); void dma_cancel(pwm_group& group);