Skip to content

Commit

Permalink
AP_HAL_ChibiOS: use rcout_timer_t instead of uint32_t or uint64_t
Browse files Browse the repository at this point in the history
  • Loading branch information
@andyp1per
andyp1per committed Apr 9, 2023
1 parent daadeec commit 6553047
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 33 deletions.
42 changes: 21 additions & 21 deletions libraries/AP_HAL_ChibiOS/RCOutput.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
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();

Expand All @@ -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::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);
Expand All @@ -179,9 +179,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
const uint32_t timeout_period_us = _dshot_rate ? (_dshot_cycle + 1) * _dshot_period_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 uint32_t cycle_start_us = _dshot_rate ? last_cycle_run_us : last_thread_run_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
// as a multiple of loop rate then ignore EVT_PWM_SEND events to preserve periodicity
Expand All @@ -202,9 +202,9 @@ void RCOutput::rcout_thread()
static bool output_masks = true;
if (AP_HAL::millis() > 5000 && output_masks) {
output_masks = false;
hal.console->printf("bdmask 0x%x, en_mask 0x%lx, 3dmask 0x%x:\n", _bdshot.mask, en_mask, _reversible_mask);
hal.console->printf("bdmask 0x%lx, en_mask 0x%lx, 3dmask 0x%lx:\n", _bdshot.mask, en_mask, _reversible_mask);
for (auto &group : pwm_group_list) {
hal.console->printf(" timer %u: ch_mask 0x%x, en_mask 0x%x\n", group.timer_id, group.ch_mask, group.en_mask);
hal.console->printf(" timer %u: ch_mask 0x%lx, en_mask 0x%lx\n", group.timer_id, group.ch_mask, group.en_mask);
}
}
#endif
Expand All @@ -225,7 +225,7 @@ __RAMFUNC__ void RCOutput::dshot_update_tick(virtual_timer_t* vt, 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 cycle_start_us, uint32_t timeout_period_us)
void RCOutput::dshot_collect_dma_locks(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
{
if (NUM_GROUPS == 0) {
return;
Expand All @@ -234,22 +234,22 @@ void RCOutput::dshot_collect_dma_locks(uint32_t cycle_start_us, uint32_t timeout
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();
rcout_timer_t now = rcout_micros();
// if we have time left wait for the event
eventmask_t mask = 0;
const uint32_t pulse_remaining_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 uint32_t timeout_remaining_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
uint32_t wait_us = MAX(pulse_remaining_us, timeout_remaining_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 = _dshot_period_us;
const uint32_t min_delay_us = 10; // matches our CH_CFG_ST_TIMEDELTA
const rcout_timer_t max_delay_us = _dshot_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);
mask = chEvtWaitOneTimeout(group.dshot_event_mask, chTimeUS2I(wait_us));

Expand Down Expand Up @@ -817,7 +817,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 is_dshot)
rcout_timer_t pulse_time_us, bool is_dshot)
{
#ifndef DISABLE_DSHOT
// for dshot we setup for DMAR based output
Expand Down Expand Up @@ -1207,7 +1207,7 @@ void RCOutput::trigger_groups()
calculate time that we are allowed to trigger next pulse
to guarantee at least a 50us gap between pulses
*/
last_pulse_trigger_us = AP_HAL::micros();
last_pulse_trigger_us = rcout_micros();

chMtxUnlock(&trigger_mutex);
}
Expand All @@ -1216,7 +1216,7 @@ void RCOutput::trigger_groups()
periodic timer. This is used for oneshot and dshot modes, plus for
safety switch update. Runs every 1000us.
*/
void RCOutput::timer_tick(uint32_t cycle_start_us, uint32_t timeout_period_us)
void RCOutput::timer_tick(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
{
if (serial_group) {
return;
Expand Down Expand Up @@ -1244,7 +1244,7 @@ void RCOutput::timer_tick(uint32_t cycle_start_us, uint32_t timeout_period_us)
}

// send dshot for all groups that support it
void RCOutput::dshot_send_groups(uint32_t cycle_start_us, uint32_t timeout_period_us)
void RCOutput::dshot_send_groups(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
{
#ifndef DISABLE_DSHOT
if (serial_group) {
Expand Down Expand Up @@ -1373,7 +1373,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 cycle_start_us, uint32_t timeout_period_us)
void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
{
#ifndef DISABLE_DSHOT
if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
Expand Down Expand Up @@ -1599,7 +1599,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 //#ifndef DISABLE_DSHOT
}

Expand Down
30 changes: 19 additions & 11 deletions libraries/AP_HAL_ChibiOS/RCOutput.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,14 @@
#define DISABLE_DSHOT
#endif

#ifdef AP_RCOUT_USE_64BIT_TIME
typedef uint64_t rcout_timer_t;
#define rcout_micros() AP_HAL::micros64()
#else
typedef uint32_t rcout_timer_t;
#define rcout_micros() AP_HAL::micros()
#endif

#define RCOU_DSHOT_TIMING_DEBUG 0

class ChibiOS::RCOutput : public AP_HAL::RCOutput
Expand Down Expand Up @@ -102,7 +110,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
/*
timer push (for oneshot min rate)
*/
void timer_tick(uint32_t cycle_start_us, uint32_t timeout_period_us);
void timer_tick(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us);

/*
setup for serial output to a set of ESCs, using the given
Expand Down Expand Up @@ -320,9 +328,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
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;
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;

// serial LED support
Expand Down Expand Up @@ -369,7 +377,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
#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
#endif
} bdshot;
Expand Down Expand Up @@ -492,7 +500,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
} _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()
Expand Down Expand Up @@ -534,7 +542,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
uint32_t _active_escs_mask;

// last time pulse was triggererd used to prevent overlap
uint32_t last_pulse_trigger_us;
rcout_timer_t last_pulse_trigger_us;

// mutex for oneshot triggering
mutex_t trigger_mutex;
Expand Down Expand Up @@ -599,19 +607,19 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
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 cycle_start_us, uint32_t timeout_us);
void dshot_send(pwm_group &group, uint32_t cycle_start_us, uint32_t timeout_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
void dshot_collect_dma_locks(uint32_t cycle_start_us, uint32_t timeout_period_us);
void dshot_collect_dma_locks(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us);
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 is_dshot);
void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length);
void set_group_mode(pwm_group &group);
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -441,7 +441,7 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan)
#endif
}

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]));
Expand Down

0 comments on commit 6553047

Please sign in to comment.