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 committed by Randy Mackay
parent 7c0ccbb411
commit eb49e2ef17
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()
{
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
}

14
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -103,7 +103,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
@ -321,9 +321,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
@ -600,13 +600,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);