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

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

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

90
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
// 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) {
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
wait_us = MAX(wait_us, group.dshot_pulse_send_time_us - pulse_elapsed_us);
}
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
}

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;
#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 @@ public:
/*
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 @@ private:
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 @@ private:
#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 @@ private:
} _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 @@ private:
// 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 @@ private:
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);

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
}
#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]));

19
libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp
View File

@ -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;