Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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
This commit is contained in:
Andy Piper 2023-03-23 17:13:54 +00:00 committed by Peter Barker
parent f094ee66ec
commit b248ba5f1b
4 changed files with 77 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -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 // 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 // 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 #endif // HAL_SERIAL_ENABLED
@ -225,8 +225,8 @@ void RCOutput::led_thread()
#if !defined(IOMCU_FW) #if !defined(IOMCU_FW)
void RCOutput::rcout_thread() void RCOutput::rcout_thread()
{ {
uint64_t last_thread_run_us = 0; // last time we did a 1kHz run of rcout rcout_timer_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_cycle_run_us = 0;
rcout_thread_ctx = chThdGetSelfX(); rcout_thread_ctx = chThdGetSelfX();
@ -241,11 +241,11 @@ void RCOutput::rcout_thread()
const auto mask = chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND); 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; 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::micros64(); last_thread_run_us = rcout_micros();
// 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::micros64(); last_cycle_run_us = rcout_micros();
// 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);
@ -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 // 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
uint64_t time_out_us = (_dshot_cycle + 1) * _dshot_period_us + last_cycle_run_us; const rcout_timer_t timeout_period_us = _dshot_rate ? (_dshot_cycle + 1) * _dshot_period_us : _dshot_period_us;
if (!_dshot_rate) { // timeout is measured from the beginning of the push() that initiated it to preserve periodicity
time_out_us = last_thread_run_us + _dshot_period_us; 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 // 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 // as a multiple of loop rate then ignore EVT_PWM_SEND events to preserve periodicity
if (!in_soft_serial() && have_pwm_event) { 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 // now unlock everything
dshot_collect_dma_locks(time_out_us); dshot_collect_dma_locks(cycle_start_us, timeout_period_us);
if (_dshot_rate > 0) { if (_dshot_rate > 0) {
_dshot_cycle = (_dshot_cycle + 1) % _dshot_rate; _dshot_cycle = (_dshot_cycle + 1) % _dshot_rate;
@ -273,7 +272,7 @@ void RCOutput::rcout_thread()
} }
// process any pending RC output requests // process any pending RC output requests
timer_tick(time_out_us); timer_tick(cycle_start_us, timeout_period_us);
#if RCOU_DSHOT_TIMING_DEBUG #if RCOU_DSHOT_TIMING_DEBUG
static bool output_masks = true; static bool output_masks = true;
if (AP_HAL::millis() > 5000 && output_masks) { 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 #if AP_HAL_SHARED_DMA_ENABLED
// calculate how much time remains in the current cycle // 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 // 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 // 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; const rcout_timer_t pulse_remaining_us
if (now < time_out_us) { = AP_HAL::timeout_remaining(group.last_dmar_send_us, now, group.dshot_pulse_send_time_us);
wait_us = time_out_us - now; const rcout_timer_t timeout_remaining_us
} = AP_HAL::timeout_remaining(cycle_start_us, now, timeout_period_us);
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
// 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);
wait_us = MAX(wait_us, group.dshot_pulse_send_time_us - pulse_elapsed_us);
}
// waiting for a very short period of time can cause a // waiting for a very short period of time can cause a
// timer wrap with ChibiOS timers. Use CH_CFG_ST_TIMEDELTA // timer wrap with ChibiOS timers. Use CH_CFG_ST_TIMEDELTA
// as minimum. Don't allow for a very long delay (over _dshot_period_us) // as minimum. Don't allow for a very long delay (over _dshot_period_us)
// to prevent bugs in handling timer wrap // to prevent bugs in handling timer wrap
const uint32_t max_delay_us = output_period_us; const rcout_timer_t max_delay_us = output_period_us;
const uint32_t min_delay_us = 10; // matches our CH_CFG_ST_TIMEDELTA 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); wait_us = constrain_uint32(wait_us, min_delay_us, max_delay_us);
return MIN(TIME_MAX_INTERVAL, chTimeUS2I(wait_us)); return MIN(TIME_MAX_INTERVAL, chTimeUS2I(wait_us));
} }
// 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(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) { if (NUM_GROUPS == 0) {
return; 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 // dma handle will only be unlocked if the send was aborted
if (group.dma_handle != nullptr && group.dma_handle->is_locked()) { if (group.dma_handle != nullptr && group.dma_handle->is_locked()) {
// if we have time left wait for the event // 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); led_thread ? LED_OUTPUT_PERIOD_US : _dshot_period_us);
const eventmask_t mask = chEvtWaitOneTimeout(group.dshot_event_mask, wait_ticks); const eventmask_t mask = chEvtWaitOneTimeout(group.dshot_event_mask, wait_ticks);
@ -829,7 +826,7 @@ void RCOutput::push_local(void)
if (widest_pulse > 2300) { if (widest_pulse > 2300) {
widest_pulse = 2300; widest_pulse = 2300;
} }
trigger_widest_pulse = widest_pulse; trigger_widest_pulse = widest_pulse + 50;
trigger_groupmask = need_trigger; 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 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, 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 #if HAL_DSHOT_ENABLED
// for dshot we setup for DMAR based output // 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 trigger output groups for oneshot or dshot modes
*/ */
void RCOutput::trigger_groups(void) void RCOutput::trigger_groups()
{ {
if (!chMtxTryLock(&trigger_mutex)) { if (!chMtxTryLock(&trigger_mutex)) {
return; 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 // 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(); osalSysLock();
@ -1399,7 +1398,7 @@ void RCOutput::trigger_groups(void)
calculate time that we are allowed to trigger next pulse calculate time that we are allowed to trigger next pulse
to guarantee at least a 50us gap between pulses 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); chMtxUnlock(&trigger_mutex);
} }
@ -1408,20 +1407,17 @@ 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(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()) { if (in_soft_serial()) {
return; return;
} }
if (min_pulse_trigger_us == 0) { if (last_pulse_trigger_us == 0) {
return; return;
} }
uint64_t now = AP_HAL::micros64(); if (AP_HAL::timeout_expired(last_pulse_trigger_us, rcout_micros(), trigger_widest_pulse + 4000U)) {
if (now > min_pulse_trigger_us &&
now - min_pulse_trigger_us > 4000) {
// trigger at a minimum of 250Hz // trigger at a minimum of 250Hz
trigger_groups(); 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 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()) { if (in_soft_serial()) {
return; 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 // 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 // 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 HAL_DSHOT_ENABLED
if (in_soft_serial()) { if (in_soft_serial()) {
@ -1472,13 +1468,13 @@ void RCOutput::dshot_send_groups(uint64_t time_out_us)
pulse_sent = true; pulse_sent = true;
// actually do a dshot send // actually do a dshot send
} else if (group.can_send_dshot_pulse()) { } 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; pulse_sent = true;
} }
#if defined(HAL_WITH_BIDIR_DSHOT) && defined(HAL_TIM_UP_SHARED) #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 // 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) { 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 #else
(void)pulse_sent; (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. 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, 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 HAL_DSHOT_ENABLED
if (soft_serial_waiting() || !is_dshot_send_allowed(group.dshot_state)) { 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 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_SHARED_DMA_ENABLED #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(); group.dma_handle->unlock();
return; return;
} }
@ -1820,7 +1816,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::micros64(); group.last_dmar_send_us = rcout_micros();
#endif // HAL_DSHOT_ENABLED #endif // HAL_DSHOT_ENABLED
} }

36
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

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

2
libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
View File

@ -576,7 +576,7 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan)
#endif #endif
} }
#if !defined(IOMCU_FW) #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) { 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], 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])); 100.0f * float(group.bdshot.telem_err_rate[chan]) / (group.bdshot.telem_err_rate[chan] + group.bdshot.telem_rate[chan]));

19
libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp
View File

@ -75,15 +75,17 @@ void RCOutput::rcout_thread() {
rcout_thread_ctx = chThdGetSelfX(); rcout_thread_ctx = chThdGetSelfX();
chRegSetThreadNameX(rcout_thread_ctx, rcout_thread_name); chRegSetThreadNameX(rcout_thread_ctx, rcout_thread_name);
uint64_t last_cycle_run_us = 0; rcout_timer_t last_cycle_run_us = 0;
while (true) { while (true) {
chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND); chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND);
// start the clock // 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 // this is when the cycle is supposed to start
if (_dshot_cycle == 0) { 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 // 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);
@ -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 // 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
uint64_t time_out_us = (_dshot_cycle + 1) * _dshot_period_us + last_cycle_run_us; const rcout_timer_t timeout_period_us = _dshot_rate ? (_dshot_cycle + 1) * _dshot_period_us : _dshot_period_us;
if (!_dshot_rate) { // timeout is measured from the beginning of the push() that initiated it to preserve periodicity
time_out_us = last_thread_run_us + _dshot_period_us; 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 // DMA channel sharing on F10x is complicated. The allocations are
// TIM2_UP - (1,2) // TIM2_UP - (1,2)
@ -124,9 +125,9 @@ void RCOutput::rcout_thread() {
// TIM4_CH3 - unlock // TIM4_CH3 - unlock
// TIM4_UP - 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 #if AP_HAL_SHARED_DMA_ENABLED
dshot_collect_dma_locks(time_out_us); dshot_collect_dma_locks(cycle_start_us, timeout_period_us);
#endif #endif
if (_dshot_rate > 0) { if (_dshot_rate > 0) {
_dshot_cycle = (_dshot_cycle + 1) % _dshot_rate; _dshot_cycle = (_dshot_cycle + 1) % _dshot_rate;