Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: bdshot for f103 iofirmware

Browse Source 
add support to tell if shared DMA channel is actually shared
avoid starting and stopping the timer peripheral with bdshot
ensure that rcout DMA allocation and deallocation happens entirely within the lock
increase rcout thread working area for bdshot
fix mode mask that is sent to the iomcu
ensure iomcu rcout thread gets timeouts for callbacks
control bdshot input and output line levels on f103
use input capture channel pairs to read rising and falling edges of telemetry on f103
reset channel pairs together on iomcu
generalize the bdshot input path to support suitable buffer sizes for iomcu
generalize DMAR reading of CCR registers to read two at a time on iomcu
enable bi-directional dshot channels on PWM1-4 on iomcu
add methods to directly access erpm values from rcout
update erpm mask and esc telemetry correctly for firmware supporting dshot
add support for propagating bdmask to iomcu
dshot commands to all channels need to be aware of iomcu
ensure esc type is propagated to iomcu
cope with iomcu channel numbering when using EDT
ensure pwm driver is reset properly for dshot commands on iomcu
correctly reset pwm for dshot commands
correctly mask off bdshot bits going to iomcu
don't reset GPIO modes on disabled lines
don't reset pwm_started when sharing DMA channels
set thread name on iomcu rcout and reduce stack size on iomcu
ensure that bdshot pulses with no response are handled correctly
correctly setup DMA for input capture on f103
deal with out of order captured bytes when decoding bdshot telemetry
ensure DMA sharing on f103 does not pull lines low
only disable the timer peripheral when switching DMA channels on iomcu
add support for waiting for _UP to finish before proceeding with dshot
re-order iomcu dshot channels to let TIM4_UP go first
ensure that a cascading event will always come when expected on rcout
allow timeouts when using cascading dshot
always rotate telemetry channel after trying to capture input
cater for both in order and out-of-order bdshot telemetry packets
cope with reversed packets when decoding bdshot telemetry
ensure UP DMA channel is fully free on iomcu before starting next dshot cycle
refactor rcout for iofirmware into separate file
This commit is contained in:
Andy Piper 2023-09-24 14:09:23 +01:00 committed by Andrew Tridgell
parent e024f9fc8c
commit 9f30d01561
10 changed files with 607 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -336,8 +336,9 @@ void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread)
continue;
}
// dma handle will only be unlocked if the send was aborted
if (group.dma_handle != nullptr && group.dma_handle->is_locked()) {
// calculate how long we have left
// if we have time left wait for the event
const sysinterval_t wait_ticks = calc_ticks_remaining(group, time_out_us,
led_thread ? LED_OUTPUT_PERIOD_US : _dshot_period_us);
const eventmask_t mask = chEvtWaitOneTimeout(group.dshot_event_mask, wait_ticks);
@ -350,13 +351,11 @@ void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread)
#ifdef HAL_WITH_BIDIR_DSHOT
// if using input capture DMA then clean up
if (group.bdshot.enabled) {
// the channel index only moves on with success
const uint8_t chan = mask ? group.bdshot.prev_telem_chan
: group.bdshot.curr_telem_chan;
// only unlock if not shared
if (group.bdshot.ic_dma_handle[chan] != nullptr
&& group.bdshot.ic_dma_handle[chan] != group.dma_handle) {
group.bdshot.ic_dma_handle[chan]->unlock();
if (group.bdshot.curr_ic_dma_handle != nullptr
&& group.bdshot.curr_ic_dma_handle != group.dma_handle) {
group.bdshot.curr_ic_dma_handle->unlock();
group.bdshot.curr_ic_dma_handle = nullptr;
}
}
#endif
@ -593,6 +592,11 @@ void RCOutput::set_dshot_esc_type(DshotEscType dshot_esc_type)
DSHOT_BIT_1_TICKS = DSHOT_BIT_1_TICKS_DEFAULT;
break;
}
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
iomcu.set_dshot_esc_type(dshot_esc_type);
}
#endif
}
#endif // #if HAL_DSHOT_ENABLED
@ -1188,13 +1192,14 @@ void RCOutput::set_output_mode(uint32_t mask, const enum output_mode mode)
}
}
#if HAL_WITH_IO_MCU
const uint16_t iomcu_mask = (mask & ((1U<<chan_offset)-1));
if ((mode == MODE_PWM_ONESHOT ||
mode == MODE_PWM_ONESHOT125 ||
mode == MODE_PWM_BRUSHED ||
(mode >= MODE_PWM_DSHOT150 && mode <= MODE_PWM_DSHOT600)) &&
(mask & ((1U<<chan_offset)-1)) &&
iomcu_mask &&
AP_BoardConfig::io_enabled()) {
iomcu.set_output_mode(mask, mode);
iomcu.set_output_mode(iomcu_mask, mode);
return;
}
#endif
@ -1453,14 +1458,25 @@ void RCOutput::dshot_send_groups(uint64_t time_out_us)
}
for (auto &group : pwm_group_list) {
bool pulse_sent;
// send a dshot command
if (is_dshot_protocol(group.current_mode)
&& dshot_command_is_active(group)) {
command_sent = dshot_send_command(group, _dshot_current_command.command, _dshot_current_command.chan);
pulse_sent = true;
// actually do a dshot send
} else if (group.can_send_dshot_pulse()) {
dshot_send(group, time_out_us);
pulse_sent = true;
}
#ifdef HAL_WITH_BIDIR_DSHOT
// 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));
}
#else
(void)pulse_sent;
#endif
}
if (command_sent) {
@ -1484,16 +1500,17 @@ __RAMFUNC__ void RCOutput::dshot_send_next_group(void* p)
#if AP_HAL_SHARED_DMA_ENABLED
void RCOutput::dma_allocate(Shared_DMA *ctx)
{
chSysLock();
for (auto &group : pwm_group_list) {
if (group.dma_handle == ctx && group.dma == nullptr) {
chSysLock();
group.dma = dmaStreamAllocI(group.dma_up_stream_id, 10, dma_up_irq_callback, &group);
#if defined(IOMCU_FW)
if (group.pwm_started) {
pwmStart(group.pwm_drv, &group.pwm_cfg);
#if defined(STM32F1)
if (group.pwm_started && group.dma_handle->is_shared()) {
/* Timer configured and started.*/
group.pwm_drv->tim->CR1 = STM32_TIM_CR1_ARPE | STM32_TIM_CR1_URS | STM32_TIM_CR1_CEN;
group.pwm_drv->tim->DIER = group.pwm_drv->config->dier & ~STM32_TIM_DIER_IRQ_MASK;
}
#endif
chSysUnlock();
#if STM32_DMA_SUPPORTS_DMAMUX
if (group.dma) {
dmaSetRequestSource(group.dma, group.dma_up_channel);
@ -1501,6 +1518,7 @@ void RCOutput::dma_allocate(Shared_DMA *ctx)
#endif
}
}
chSysUnlock();
}
/*
@ -1508,21 +1526,23 @@ void RCOutput::dma_allocate(Shared_DMA *ctx)
*/
void RCOutput::dma_deallocate(Shared_DMA *ctx)
{
chSysLock();
for (auto &group : pwm_group_list) {
if (group.dma_handle == ctx && group.dma != nullptr) {
chSysLock();
#if defined(IOMCU_FW)
#if defined(STM32F1)
// leaving the peripheral running on IOMCU plays havoc with the UART that is
// also sharing this channel
if (group.pwm_started) {
pwmStop(group.pwm_drv);
// also sharing this channel, we only turn it off rather than resetting so
// that we don't have to worry about line modes etc
if (group.pwm_started && group.dma_handle->is_shared()) {
group.pwm_drv->tim->CR1 = 0;
group.pwm_drv->tim->DIER = 0;
}
#endif
dmaStreamFreeI(group.dma);
group.dma = nullptr;
chSysUnlock();
}
}
chSysUnlock();
}
#endif // AP_HAL_SHARED_DMA_ENABLED
@ -1588,7 +1608,7 @@ void RCOutput::fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16
void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)
{
#if HAL_DSHOT_ENABLED
if (soft_serial_waiting() || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
if (soft_serial_waiting() || !is_dshot_send_allowed(group.dshot_state)) {
// doing serial output or DMAR input, don't send DShot pulses
return;
}
@ -1601,7 +1621,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 (time_out_us > 0 && AP_HAL::micros64() + 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;
}
@ -1681,7 +1701,7 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)
}
}
chEvtGetAndClearEvents(group.dshot_event_mask);
chEvtGetAndClearEvents(group.dshot_event_mask | DSHOT_CASCADE);
// start sending the pulses out
send_pulses_DMAR(group, DSHOT_BUFFER_LENGTH);
#endif // HAL_DSHOT_ENABLED
@ -1700,7 +1720,7 @@ bool RCOutput::serial_led_send(pwm_group &group)
}
#if HAL_DSHOT_ENABLED
if (soft_serial_waiting() || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)
if (soft_serial_waiting() || !is_dshot_send_allowed(group.dshot_state)
|| AP_HAL::micros64() - group.last_dmar_send_us < (group.dshot_pulse_time_us + 50)) {
// doing serial output or DMAR input, don't send DShot pulses
return false;
@ -1721,7 +1741,7 @@ bool RCOutput::serial_led_send(pwm_group &group)
group.dshot_waiter = led_thread_ctx;
chEvtGetAndClearEvents(group.dshot_event_mask);
chEvtGetAndClearEvents(group.dshot_event_mask | DSHOT_CASCADE);
// start sending the pulses out
send_pulses_DMAR(group, group.dma_buffer_len);
@ -1766,10 +1786,18 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length)
#endif
dmaStreamSetMode(group.dma,
STM32_DMA_CR_CHSEL(group.dma_up_channel) |
STM32_DMA_CR_DIR_M2P | STM32_DMA_CR_PSIZE_WORD |
#if defined(IOMCU_FW)
STM32_DMA_CR_MSIZE_BYTE |
STM32_DMA_CR_DIR_M2P |
#if defined(STM32F1)
#ifdef HAL_WITH_BIDIR_DSHOT
STM32_DMA_CR_PSIZE_HWORD |
STM32_DMA_CR_MSIZE_HWORD |
#else
STM32_DMA_CR_PSIZE_WORD |
STM32_DMA_CR_MSIZE_BYTE |
#endif
#else
STM32_DMA_CR_PSIZE_WORD |
STM32_DMA_CR_MSIZE_WORD |
#endif
STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3) |
@ -1858,7 +1886,7 @@ void RCOutput::dma_cancel(pwm_group& group)
}
}
chVTResetI(&group.dma_timeout);
chEvtGetAndClearEventsI(group.dshot_event_mask);
chEvtGetAndClearEventsI(group.dshot_event_mask | DSHOT_CASCADE);
group.dshot_state = DshotState::IDLE;
chSysUnlock();

41
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -25,10 +25,17 @@
#if HAL_USE_PWM == TRUE
#if defined(IOMCU_FW)
#if defined(STM32F1)
#ifdef HAL_WITH_BIDIR_DSHOT
typedef uint16_t dmar_uint_t; // save memory to allow dshot on IOMCU
typedef int16_t dmar_int_t;
#else
typedef uint8_t dmar_uint_t; // save memory to allow dshot on IOMCU
typedef int8_t dmar_int_t;
#endif
#else
typedef uint32_t dmar_uint_t;
typedef int32_t dmar_int_t;
#endif
#define RCOU_DSHOT_TIMING_DEBUG 0
@ -59,6 +66,11 @@ public:
float get_erpm_error_rate(uint8_t chan) const override {
return 100.0f * float(_bdshot.erpm_errors[chan]) / (1 + _bdshot.erpm_errors[chan] + _bdshot.erpm_clean_frames[chan]);
}
/*
allow all erpm values to be read and for new updates to be detected - primarily for IOMCU
*/
bool new_erpm() override { return _bdshot.update_mask != 0; }
uint32_t read_erpm(uint16_t* erpm, uint8_t len) override;
#endif
void set_output_mode(uint32_t mask, const enum output_mode mode) override;
enum output_mode get_output_mode(uint32_t& mask) override;
@ -277,7 +289,8 @@ private:
SEND_START = 1,
SEND_COMPLETE = 2,
RECV_START = 3,
RECV_COMPLETE = 4
RECV_COMPLETE = 4,
RECV_FAILED = 5
};
struct PACKED SerialLed {
@ -296,8 +309,11 @@ private:
static const uint16_t DSHOT_BUFFER_LENGTH = dshot_bit_length * 4 * sizeof(dmar_uint_t);
static const uint16_t MIN_GCR_BIT_LEN = 7;
static const uint16_t MAX_GCR_BIT_LEN = 22;
static const uint16_t TELEM_IC_SAMPLE = 16;
static const uint16_t GCR_TELEMETRY_BIT_LEN = MAX_GCR_BIT_LEN;
static const uint16_t GCR_TELEMETRY_BUFFER_LEN = GCR_TELEMETRY_BIT_LEN*sizeof(uint32_t);
// input capture is expecting TELEM_IC_SAMPLE (16) ticks per transition (22) so the maximum
// value of the counter in CCR registers is 16*22 == 352, so must be 16-bit
static const uint16_t GCR_TELEMETRY_BUFFER_LEN = GCR_TELEMETRY_BIT_LEN*sizeof(dmar_uint_t);
struct pwm_group {
// only advanced timers can do high clocks needed for more than 400Hz
@ -315,6 +331,7 @@ private:
uint8_t stream_id;
uint8_t channel;
} dma_ch[4];
bool shared_up_dma; // do we need to wait for TIMx_UP DMA to be finished after use
#endif
uint8_t alt_functions[4];
ioline_t pal_lines[4];
@ -380,8 +397,9 @@ private:
uint8_t telem_tim_ch[4];
uint8_t curr_telem_chan;
uint8_t prev_telem_chan;
Shared_DMA *curr_ic_dma_handle; // a shortcut to avoid logic errors involving the wrong lock
uint16_t telempsc;
uint32_t dma_buffer_copy[GCR_TELEMETRY_BUFFER_LEN];
dmar_uint_t dma_buffer_copy[GCR_TELEMETRY_BUFFER_LEN];
#if RCOU_DSHOT_TIMING_DEBUG
uint16_t telem_rate[4];
uint16_t telem_err_rate[4];
@ -525,6 +543,7 @@ private:
struct {
uint32_t mask;
uint16_t erpm[max_channels];
volatile uint32_t update_mask;
#ifdef HAL_WITH_BIDIR_DSHOT
uint16_t erpm_errors[max_channels];
uint16_t erpm_clean_frames[max_channels];
@ -593,6 +612,10 @@ private:
bool is_bidir_dshot_enabled() const { return _bdshot.mask != 0; }
static bool is_dshot_send_allowed(DshotState state) {
return state == DshotState::IDLE || state == DshotState::RECV_COMPLETE || state == DshotState::RECV_FAILED;
}
// are all the ESCs returning data
bool group_escs_active(const pwm_group& group) const {
return group.en_mask > 0 && (group.en_mask & _active_escs_mask) == group.en_mask;
@ -644,6 +667,8 @@ private:
uint16_t create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem);
void fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);
// event to allow dshot cascading
static const eventmask_t DSHOT_CASCADE = EVENT_MASK(16);
static const eventmask_t EVT_PWM_SEND = EVENT_MASK(11);
static const eventmask_t EVT_PWM_SYNTHETIC_SEND = EVENT_MASK(13);
@ -672,7 +697,8 @@ private:
*/
void bdshot_ic_dma_allocate(Shared_DMA *ctx);
void bdshot_ic_dma_deallocate(Shared_DMA *ctx);
static uint32_t bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count);
static uint32_t bdshot_decode_telemetry_packet(dmar_uint_t* buffer, uint32_t count);
static uint32_t bdshot_decode_telemetry_packet_f1(dmar_uint_t* buffer, uint32_t count, bool reversed);
bool bdshot_decode_telemetry_from_erpm(uint16_t erpm, uint8_t chan);
bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan);
static uint8_t bdshot_find_next_ic_channel(const pwm_group& group);
@ -681,8 +707,11 @@ private:
bool bdshot_setup_group_ic_DMA(pwm_group &group);
void bdshot_prepare_for_next_pulse(pwm_group& group);
static void bdshot_receive_pulses_DMAR(pwm_group* group);
static void bdshot_reset_pwm(pwm_group& group);
static void bdshot_receive_pulses_DMAR_f1(pwm_group* group);
static void bdshot_reset_pwm(pwm_group& group, uint8_t telem_channel);
static void bdshot_reset_pwm_f1(pwm_group& group, uint8_t telem_channel);
static void bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch);
static void bdshot_config_icu_dshot_f1(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch);
static uint32_t bdshot_get_output_rate_hz(const enum output_mode mode);
/*

151
libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
View File

@ -21,9 +21,20 @@
#include "hwdef/common/stm32_util.h"
#include <AP_InternalError/AP_InternalError.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#if HAL_WITH_IO_MCU
#include <AP_IOMCU/AP_IOMCU.h>
extern AP_IOMCU iomcu;
#endif
#ifdef HAL_WITH_BIDIR_DSHOT
#if defined(IOMCU_FW)
#undef INTERNAL_ERROR
#define INTERNAL_ERROR(x) do {} while (0)
#endif
using namespace ChibiOS;
extern const AP_HAL::HAL& hal;
@ -35,14 +46,18 @@ extern const AP_HAL::HAL& hal;
#define TOGGLE_PIN_CH_DEBUG(pin, channel) do {} while (0)
#endif
#define TELEM_IC_SAMPLE 16
/*
* enable bi-directional telemetry request for a mask of channels. This is used
* with DShot to get telemetry feedback
*/
void RCOutput::set_bidir_dshot_mask(uint32_t mask)
{
#if HAL_WITH_IO_MCU
const uint32_t iomcu_mask = ((1U<<chan_offset)-1);
if (AP_BoardConfig::io_dshot() && (mask & iomcu_mask)) {
iomcu.set_bidir_dshot_mask(mask & iomcu_mask);
}
#endif
_bdshot.mask = (mask >> chan_offset);
// we now need to reconfigure the DMA channels since they are affected by the value of the mask
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
@ -75,6 +90,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
// Return error
return false;
}
if (!group.bdshot.ic_dma_handle[i]) {
// share up channel if required
if (group.dma_ch[i].stream_id == group.dma_up_stream_id) {
@ -98,6 +114,11 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
if (group.chan[i] != CHAN_DISABLED && _bdshot.mask & group.ch_mask) {
// bi-directional dshot requires less than MID2 speed and PUSHPULL in order to avoid noise on the line
// when switching from output to input
#if defined(STM32F1)
// on F103 the line mode has to be managed manually
// PAL_MODE_STM32_ALTERNATE_PUSHPULL is 50Mhz, similar to the medieum speed on other MCUs
palSetLineMode(group.pal_lines[i], PAL_MODE_STM32_ALTERNATE_PUSHPULL);
#else
palSetLineMode(group.pal_lines[i], PAL_MODE_ALTERNATE(group.alt_functions[i])
| PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_PUPDR_PULLUP |
#ifdef PAL_STM32_OSPEED_MID1
@ -108,6 +129,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
#error "Cannot set bdshot line speed"
#endif
);
#endif
}
if (!group.is_chan_enabled(i) || !(_bdshot.mask & (1 << group.chan[i]))) {
@ -166,13 +188,12 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
*/
void RCOutput::bdshot_ic_dma_allocate(Shared_DMA *ctx)
{
chSysLock();
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
for (uint8_t icuch = 0; icuch < 4; icuch++) {
if (group.bdshot.ic_dma_handle[icuch] == ctx && group.bdshot.ic_dma[icuch] == nullptr) {
chSysLock();
group.bdshot.ic_dma[icuch] = dmaStreamAllocI(group.dma_ch[icuch].stream_id, 10, bdshot_dma_ic_irq_callback, &group);
chSysUnlock();
#if STM32_DMA_SUPPORTS_DMAMUX
if (group.bdshot.ic_dma[icuch]) {
dmaSetRequestSource(group.bdshot.ic_dma[icuch], group.dma_ch[icuch].channel);
@ -181,6 +202,7 @@ void RCOutput::bdshot_ic_dma_allocate(Shared_DMA *ctx)
}
}
}
chSysUnlock();
}
/*
@ -188,23 +210,23 @@ void RCOutput::bdshot_ic_dma_allocate(Shared_DMA *ctx)
*/
void RCOutput::bdshot_ic_dma_deallocate(Shared_DMA *ctx)
{
chSysLock();
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
for (uint8_t icuch = 0; icuch < 4; icuch++) {
if (group.bdshot.ic_dma_handle[icuch] == ctx && group.bdshot.ic_dma[icuch] != nullptr) {
chSysLock();
dmaStreamFreeI(group.bdshot.ic_dma[icuch]);
group.bdshot.ic_dma[icuch] = nullptr;
chSysUnlock();
}
}
}
chSysUnlock();
}
// setup bdshot for sending and receiving the next pulse
void RCOutput::bdshot_prepare_for_next_pulse(pwm_group& group)
{
// assume that we won't be able to get the input capture lock
// assume that we won't be able to get the input capture lock
group.bdshot.enabled = false;
uint32_t active_channels = group.ch_mask & group.en_mask;
@ -214,8 +236,10 @@ void RCOutput::bdshot_prepare_for_next_pulse(pwm_group& group)
// no locking required
group.bdshot.enabled = true;
} else {
osalDbgAssert(!group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan]->is_locked(), "DMA handle is already locked");
group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan]->lock();
osalDbgAssert(!group.bdshot.curr_ic_dma_handle, "IC DMA handle has not been released");
group.bdshot.curr_ic_dma_handle = group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan];
osalDbgAssert(group.shared_up_dma || !group.bdshot.curr_ic_dma_handle->is_locked(), "IC DMA handle is already locked");
group.bdshot.curr_ic_dma_handle->lock();
group.bdshot.enabled = true;
}
}
@ -236,31 +260,42 @@ void RCOutput::bdshot_prepare_for_next_pulse(pwm_group& group)
_bdshot.erpm_errors[chan] = 0;
_bdshot.erpm_last_stats_ms[chan] = now;
}
} else if (group.dshot_state == DshotState::RECV_FAILED) {
_bdshot.erpm_errors[group.bdshot.curr_telem_chan]++;
}
if (group.bdshot.enabled) {
if (group.pwm_started) {
bdshot_reset_pwm(group);
} else {
pwmStart(group.pwm_drv, &group.pwm_cfg);
bdshot_reset_pwm(group, group.bdshot.prev_telem_chan);
}
else {
pwmStart(group.pwm_drv, &group.pwm_cfg);
group.pwm_started = true;
}
group.pwm_started = true;
// we can be more precise for capture timer
group.bdshot.telempsc = (uint16_t)(lrintf(((float)group.pwm_drv->clock / bdshot_get_output_rate_hz(group.current_mode) + 0.01f)/TELEM_IC_SAMPLE) - 1);
}
}
void RCOutput::bdshot_reset_pwm(pwm_group& group)
// reset pwm driver to output mode without resetting the clock or the peripheral
// the code here is the equivalent of pwmStart()/pwmStop()
void RCOutput::bdshot_reset_pwm(pwm_group& group, uint8_t telem_channel)
{
#if defined(STM32F1)
bdshot_reset_pwm_f1(group, telem_channel);
#else
// on more capable MCUs we can do something very simple
pwmStop(group.pwm_drv);
pwmStart(group.pwm_drv, &group.pwm_cfg);
#endif
}
// see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625
// called from the interrupt
#pragma GCC push_options
#pragma GCC optimize("O2")
#if !defined(STM32F1)
void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group)
{
// make sure the transaction finishes or times out, this function takes a little time to run so the most
@ -268,13 +303,12 @@ void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group)
// should be plenty
chVTSetI(&group->dma_timeout, chTimeUS2I(group->dshot_pulse_send_time_us + 30U + 10U),
bdshot_finish_dshot_gcr_transaction, group);
uint8_t curr_ch = group->bdshot.curr_telem_chan;
group->pwm_drv->tim->CR1 = 0;
group->pwm_drv->tim->CCER = 0;
// Configure Timer
group->pwm_drv->tim->SR = 0;
group->pwm_drv->tim->CCER = 0;
group->pwm_drv->tim->CCMR1 = 0;
group->pwm_drv->tim->CCMR2 = 0;
group->pwm_drv->tim->DIER = 0;
@ -286,6 +320,7 @@ void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group)
//TOGGLE_PIN_CH_DEBUG(54, curr_ch);
group->pwm_drv->tim->ARR = 0xFFFF; // count forever
group->pwm_drv->tim->CNT = 0;
uint8_t curr_ch = group->bdshot.curr_telem_chan;
// Initialise ICU channels
bdshot_config_icu_dshot(group->pwm_drv->tim, curr_ch, group->bdshot.telem_tim_ch[curr_ch]);
@ -308,7 +343,9 @@ void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group)
#endif
dmaStreamSetMode(ic_dma,
STM32_DMA_CR_CHSEL(group->dma_ch[curr_ch].channel) |
STM32_DMA_CR_DIR_P2M | STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_WORD |
STM32_DMA_CR_DIR_P2M |
STM32_DMA_CR_PSIZE_WORD |
STM32_DMA_CR_MSIZE_WORD |
STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3) |
STM32_DMA_CR_TEIE | STM32_DMA_CR_TCIE);
@ -424,6 +461,7 @@ void RCOutput::bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t
break;
}
}
#endif
/*
unlock DMA channel after a bi-directional dshot transaction completes
@ -441,14 +479,21 @@ __RAMFUNC__ void RCOutput::bdshot_finish_dshot_gcr_transaction(virtual_timer_t*
// or the input channel buffer
const stm32_dma_stream_t *dma =
group->has_shared_ic_up_dma() ? group->dma : group->bdshot.ic_dma[curr_telem_chan];
osalDbgAssert(dma, "No DMA channel");
// record the transaction size before the stream is released
dmaStreamDisable(dma);
group->bdshot.dma_tx_size = MIN(uint16_t(GCR_TELEMETRY_BIT_LEN),
GCR_TELEMETRY_BIT_LEN - dmaStreamGetTransactionSize(dma));
stm32_cacheBufferInvalidate(group->dma_buffer, group->bdshot.dma_tx_size);
memcpy(group->bdshot.dma_buffer_copy, group->dma_buffer, sizeof(uint32_t) * group->bdshot.dma_tx_size);
group->dshot_state = DshotState::RECV_COMPLETE;
stm32_cacheBufferInvalidate(group->dma_buffer, group->bdshot.dma_tx_size);
memcpy(group->bdshot.dma_buffer_copy, group->dma_buffer, sizeof(dmar_uint_t) * group->bdshot.dma_tx_size);
// although it should be possible to start the next DMAR transaction concurrently with receiving
// telemetry, in practice it seems to interfere with the DMA engine
if (group->shared_up_dma && group->bdshot.enabled) {
// next dshot pulse can go out now
chEvtSignalI(group->dshot_waiter, DSHOT_CASCADE);
}
// if using input capture DMA and sharing the UP and CH channels then clean up
// by assigning the source back to UP
@ -458,9 +503,17 @@ __RAMFUNC__ void RCOutput::bdshot_finish_dshot_gcr_transaction(virtual_timer_t*
}
#endif
// rotate to the next input channel
group->bdshot.prev_telem_chan = group->bdshot.curr_telem_chan;
// rotate to the next input channel, we have to rotate even on failure otherwise
// we will not get data from active channels
group->bdshot.curr_telem_chan = bdshot_find_next_ic_channel(*group);
if (group->bdshot.dma_tx_size > 0) {
group->dshot_state = DshotState::RECV_COMPLETE;
} else {
group->dshot_state = DshotState::RECV_FAILED;
}
// tell the waiting process we've done the DMA
chEvtSignalI(group->dshot_waiter, group->dshot_event_mask);
#ifdef HAL_GPIO_LINE_GPIO56
@ -479,7 +532,12 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan)
}
// evaluate dshot telemetry
#if defined(STM32F1)
const bool reversed = (group.bdshot.telem_tim_ch[chan] & 1U) == 0;
group.bdshot.erpm[chan] = bdshot_decode_telemetry_packet_f1(group.bdshot.dma_buffer_copy, group.bdshot.dma_tx_size, reversed);
#else
group.bdshot.erpm[chan] = bdshot_decode_telemetry_packet(group.bdshot.dma_buffer_copy, group.bdshot.dma_tx_size);
#endif
group.dshot_state = DshotState::IDLE;
@ -496,7 +554,7 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan)
TOGGLE_PIN_DEBUG(57);
#endif
}
#if !defined(IOMCU_FW)
uint64_t now = AP_HAL::micros64();
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],
@ -511,6 +569,7 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan)
group.bdshot.telem_err_rate[chan] = 0;
group.bdshot.last_print = now;
}
#endif
#endif
return group.bdshot.erpm[chan] != 0xFFFF;
}
@ -549,13 +608,19 @@ __RAMFUNC__ void RCOutput::dma_up_irq_callback(void *p, uint32_t flags)
}
dmaStreamDisable(group->dma);
if (group->in_serial_dma && irq.waiter) {
if (soft_serial_waiting()) {
#if HAL_SERIAL_ESC_COMM_ENABLED
// tell the waiting process we've done the DMA
chEvtSignalI(irq.waiter, serial_event_mask);
#endif
} else if (!group->in_serial_dma && group->bdshot.enabled) {
group->dshot_state = DshotState::SEND_COMPLETE;
// sending is done, in 30us the ESC will send telemetry
#if defined(STM32F1)
bdshot_receive_pulses_DMAR_f1(group);
#else
bdshot_receive_pulses_DMAR(group);
#endif
} else {
// non-bidir case, this prevents us ever having two dshot pulses too close together
if (is_dshot_protocol(group->current_mode)) {
@ -604,20 +669,21 @@ uint32_t RCOutput::bdshot_get_output_rate_hz(const enum output_mode mode)
}
}
#define INVALID_ERPM 0xffffU
#define INVALID_ERPM 0xfffU
// decode a telemetry packet from a GCR encoded stride buffer, take from betaflight decodeTelemetryPacket
// see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 for a description of the protocol
uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count)
uint32_t RCOutput::bdshot_decode_telemetry_packet(dmar_uint_t* buffer, uint32_t count)
{
uint32_t value = 0;
uint32_t oldValue = buffer[0];
uint32_t bits = 0;
uint32_t len;
dmar_uint_t oldValue = buffer[0];
for (uint32_t i = 1; i <= count; i++) {
if (i < count) {
int32_t diff = buffer[i] - oldValue;
dmar_int_t diff = buffer[i] - oldValue;
if (bits >= 21U) {
break;
}
@ -631,6 +697,7 @@ uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t cou
oldValue = buffer[i];
bits += len;
}
if (bits != 21U) {
return INVALID_ERPM;
}
@ -665,8 +732,16 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
}
// eRPM = m << e (see https://github.com/bird-sanctuary/extended-dshot-telemetry)
uint8_t expo = ((encodederpm & 0xfffffe00U) >> 9U) & 0xffU;
uint16_t value = (encodederpm & 0x000001ffU);
uint8_t expo = ((encodederpm & 0xfffffe00U) >> 9U) & 0xffU; // 3bits
uint16_t value = (encodederpm & 0x000001ffU); // 9bits
#if HAL_WITH_ESC_TELEM
uint8_t normalized_chan = chan;
#endif
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
normalized_chan = chan + chan_offset;
}
#endif
if (!(value & 0x100U) && (_dshot_esc_type == DSHOT_ESC_BLHELI_EDT || _dshot_esc_type == DSHOT_ESC_BLHELI_EDT_S)) {
switch (expo) {
@ -675,7 +750,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
TelemetryData t {
.temperature_cdeg = int16_t(value * 100)
};
update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
#endif
return false;
}
@ -685,7 +760,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
TelemetryData t {
.voltage = 0.25f * value
};
update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::VOLTAGE);
update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::VOLTAGE);
#endif
return false;
}
@ -695,7 +770,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
TelemetryData t {
.current = float(value)
};
update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT);
update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT);
#endif
return false;
}
@ -726,11 +801,21 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
// update the ESC telemetry data
if (erpm < INVALID_ERPM) {
_bdshot.erpm[chan] = erpm;
_bdshot.update_mask |= 1U<<chan;
#if HAL_WITH_ESC_TELEM
update_rpm(chan, erpm * 200U / _bdshot.motor_poles, get_erpm_error_rate(chan));
update_rpm(normalized_chan, erpm * 200U / _bdshot.motor_poles, get_erpm_error_rate(chan));
#endif
}
return erpm < INVALID_ERPM;
}
uint32_t RCOutput::read_erpm(uint16_t* erpm, uint8_t len)
{
const uint8_t READ_LEN = MIN(len, uint8_t(max_channels));
memcpy(erpm, _bdshot.erpm, sizeof(uint16_t) * READ_LEN);
const uint32_t mask = _bdshot.update_mask;
_bdshot.update_mask = 0;
return mask;
}
#endif // HAL_WITH_BIDIR_DSHOT

356
libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp
View File

@ -36,22 +36,21 @@ using namespace ChibiOS;
extern const AP_HAL::HAL& hal;
#ifdef HAL_WITH_BIDIR_DSHOT
THD_WORKING_AREA(dshot_thread_wa, 512);
#else
THD_WORKING_AREA(dshot_thread_wa, 64);
#endif
static const char* rcout_thread_name = "rcout";
void RCOutput::timer_tick()
{
if (dshot_timer_setup) {
return;
}
bool dshot_enabled = false;
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
if (is_dshot_protocol(group.current_mode)) {
dshot_enabled = true;
break;
}
}
if (!dshot_timer_setup && dshot_enabled) {
uint32_t dshot_mask;
if (is_dshot_protocol(get_output_mode(dshot_mask))) {
chThdCreateStatic(dshot_thread_wa, sizeof(dshot_thread_wa),
APM_RCOUT_PRIORITY, &RCOutput::dshot_send_trampoline, this);
dshot_timer_setup = true;
@ -69,28 +68,65 @@ void RCOutput::dshot_send_trampoline(void *p)
*/
void RCOutput::rcout_thread() {
// don't start outputting until fully configured
while (!hal.scheduler->is_system_initialized()) {
while (!hal.scheduler->is_system_initialized() || _dshot_period_us == 0) {
hal.scheduler->delay_microseconds(1000);
}
rcout_thread_ctx = chThdGetSelfX();
chRegSetThreadNameX(rcout_thread_ctx, rcout_thread_name);
uint64_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();
// this is when the cycle is supposed to start
if (_dshot_cycle == 0) {
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);
}
}
// 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
dshot_send_groups(0);
// 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;
}
// DMA channel sharing on F10x is complicated. The allocations are
// TIM2_UP - (1,2)
// TIM4_UP - (1,7)
// TIM3_UP - (1,3)
// TIM2_CH2 - (1,7) - F103 only
// TIM4_CH3 - (1,5) - F103 only
// TIM3_CH4 - (1,3) - F103 only
// and (1,7) is also shared with USART2_TX
// locks have to be unlocked in reverse order, and shared CH locks do not need to be taken so the
// ordering that will work follows. This relies on recursive lock behaviour that allows us to relock
// a mutex without releasing it first:
// TIM4_UP - lock (shared)
// TIM4 - dshot send
// TIM4_CH3 - lock
// TIM2_UP - lock
// TIM2_CH2 - lock recursive (shared)
// TIM2 - dshot send
// TIM3_UP - lock
// [TIM3_CH4 - shared lock]
// TIM3 - dshot send
// [TIM3_CH4 - shared unlock]
// TIM3_UP - unlock
// TIM2_CH2 - unlock recursive (shared)
// TIM2_UP - unlock
// TIM4_CH3 - unlock
// TIM4_UP - unlock
dshot_send_groups(time_out_us);
#if AP_HAL_SHARED_DMA_ENABLED
dshot_collect_dma_locks(0);
dshot_collect_dma_locks(time_out_us);
#endif
if (_dshot_rate > 0) {
_dshot_cycle = (_dshot_cycle + 1) % _dshot_rate;
@ -98,6 +134,296 @@ void RCOutput::rcout_thread() {
}
}
#if defined(HAL_WITH_BIDIR_DSHOT) && defined(STM32F1)
// reset pwm driver to output mode without resetting the clock or the peripheral
// the code here is the equivalent of pwmStart()/pwmStop()
void RCOutput::bdshot_reset_pwm_f1(pwm_group& group, uint8_t telem_channel)
{
osalSysLock();
stm32_tim_t* TIMx = group.pwm_drv->tim;
// pwmStop sets these
TIMx->CR1 = 0; /* Timer disabled. */
TIMx->DIER = 0; /* All IRQs disabled. */
TIMx->SR = 0; /* Clear eventual pending IRQs. */
TIMx->CNT = 0;
TIMx->CCR[0] = 0; /* Comparator 1 disabled. */
TIMx->CCR[1] = 0; /* Comparator 2 disabled. */
TIMx->CCR[2] = 0; /* Comparator 3 disabled. */
TIMx->CCR[3] = 0; /* Comparator 4 disabled. */
// at the point this is called we will have done input capture on two CC channels
// we need to switch those channels back to output and the default settings
// all other channels will not have been modified
switch (group.bdshot.telem_tim_ch[telem_channel]) {
case 0: // CC1
case 1: // CC2
MODIFY_REG(TIMx->CCER, TIM_CCER_CC2E | TIM_CCER_CC1E, 0); // disable CC so that it can be modified
MODIFY_REG(TIMx->CCMR1, (TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC),
STM32_TIM_CCMR1_OC1M(6) | STM32_TIM_CCMR1_OC1PE);
MODIFY_REG(TIMx->CCMR1, (TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC),
STM32_TIM_CCMR1_OC2M(6) | STM32_TIM_CCMR1_OC2PE);
MODIFY_REG(TIMx->CCER, (TIM_CCER_CC1P | TIM_CCER_CC2P),
(TIM_CCER_CC1P | TIM_CCER_CC2P | TIM_CCER_CC1E | TIM_CCER_CC2E));
break;
case 2: // CC3
case 3: // CC4
MODIFY_REG(TIMx->CCER, TIM_CCER_CC3E | TIM_CCER_CC4E, 0); // disable CC so that it can be modified
MODIFY_REG(TIMx->CCMR2, (TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC),
STM32_TIM_CCMR2_OC3M(6) | STM32_TIM_CCMR2_OC3PE);
MODIFY_REG(TIMx->CCMR2, (TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC),
STM32_TIM_CCMR2_OC4M(6) | STM32_TIM_CCMR2_OC4PE);
MODIFY_REG(TIMx->CCER, (TIM_CCER_CC3P | TIM_CCER_CC4P),
(TIM_CCER_CC3P | TIM_CCER_CC4P | TIM_CCER_CC3E | TIM_CCER_CC4E));
break;
default:
break;
}
// pwmStart sets these
uint32_t psc = (group.pwm_drv->clock / group.pwm_drv->config->frequency) - 1;
TIMx->PSC = psc;
TIMx->ARR = group.pwm_drv->period - 1;
TIMx->CR2 = group.pwm_drv->config->cr2;
TIMx->EGR = STM32_TIM_EGR_UG; /* Update event. */
TIMx->SR = 0; /* Clear pending IRQs. */
TIMx->DIER = group.pwm_drv->config->dier & /* DMA-related DIER settings. */
~STM32_TIM_DIER_IRQ_MASK;
if (group.pwm_drv->has_bdtr) {
TIMx->BDTR = group.pwm_drv->config->bdtr | STM32_TIM_BDTR_MOE;
}
// we need to switch every output on the same input channel to avoid
// spurious line changes
for (uint8_t i = 0; i<4; i++) {
if (group.chan[i] == CHAN_DISABLED) {
continue;
}
if (group.bdshot.telem_tim_ch[telem_channel] == group.bdshot.telem_tim_ch[i]) {
palSetLineMode(group.pal_lines[i], PAL_MODE_STM32_ALTERNATE_PUSHPULL);
}
}
/* Timer configured and started.*/
TIMx->CR1 = STM32_TIM_CR1_ARPE | STM32_TIM_CR1_URS | STM32_TIM_CR1_CEN;
osalSysUnlock();
}
// see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625
// called from the interrupt
void RCOutput::bdshot_receive_pulses_DMAR_f1(pwm_group* group)
{
// make sure the transaction finishes or times out, this function takes a little time to run so the most
// accurate timing is from the beginning. the pulse time is slightly longer than we need so an extra 10U
// should be plenty
chVTSetI(&group->dma_timeout, chTimeUS2I(group->dshot_pulse_send_time_us + 30U + 10U),
bdshot_finish_dshot_gcr_transaction, group);
group->pwm_drv->tim->CR1 = 0;
// Configure Timer
group->pwm_drv->tim->SR = 0;
// do NOT set CCER to 0 here - this pulls the line low on F103 (at least)
// and since we are already doing bdshot the relevant options that are set for output
// also apply to input and bdshot_config_icu_dshot() will disable any channels that need
// disabling
group->pwm_drv->tim->DIER = 0;
group->pwm_drv->tim->CR2 = 0;
group->pwm_drv->tim->PSC = group->bdshot.telempsc;
group->dshot_state = DshotState::RECV_START;
//TOGGLE_PIN_CH_DEBUG(54, curr_ch);
group->pwm_drv->tim->ARR = 0xFFFF; // count forever
group->pwm_drv->tim->CNT = 0;
uint8_t curr_ch = group->bdshot.curr_telem_chan;
// we need to switch every input on the same input channel to allow
// the ESCs to drive the lines
for (uint8_t i = 0; i<4; i++) {
if (group->chan[i] == CHAN_DISABLED) {
continue;
}
if (group->bdshot.telem_tim_ch[curr_ch] == group->bdshot.telem_tim_ch[i]) {
palSetLineMode(group->pal_lines[i], PAL_MODE_INPUT_PULLUP);
}
}
// Initialise ICU channels
bdshot_config_icu_dshot_f1(group->pwm_drv->tim, curr_ch, group->bdshot.telem_tim_ch[curr_ch]);
const stm32_dma_stream_t *ic_dma =
group->has_shared_ic_up_dma() ? group->dma : group->bdshot.ic_dma[curr_ch];
// Configure DMA
dmaStreamSetPeripheral(ic_dma, &(group->pwm_drv->tim->DMAR));
dmaStreamSetMemory0(ic_dma, group->dma_buffer);
dmaStreamSetTransactionSize(ic_dma, GCR_TELEMETRY_BIT_LEN);
dmaStreamSetMode(ic_dma,
STM32_DMA_CR_CHSEL(group->dma_ch[curr_ch].channel) |
STM32_DMA_CR_DIR_P2M |
STM32_DMA_CR_PSIZE_HWORD |
STM32_DMA_CR_MSIZE_HWORD |
STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3) |
STM32_DMA_CR_TEIE | STM32_DMA_CR_TCIE);
// setup for transfers. 0x0D is the register
// address offset of the CCR registers in the timer peripheral
uint8_t telem_ch_pair = group->bdshot.telem_tim_ch[curr_ch] & ~1U; // round to the lowest of the channel pair
const uint8_t ccr_ofs = offsetof(stm32_tim_t, CCR)/4 + telem_ch_pair;
group->pwm_drv->tim->DCR = STM32_TIM_DCR_DBA(ccr_ofs) | STM32_TIM_DCR_DBL(1); // read two registers at a time
// Start Timer
group->pwm_drv->tim->EGR |= STM32_TIM_EGR_UG;
group->pwm_drv->tim->SR = 0;
group->pwm_drv->tim->CR1 = TIM_CR1_ARPE | STM32_TIM_CR1_URS | STM32_TIM_CR1_UDIS | STM32_TIM_CR1_CEN;
dmaStreamEnable(ic_dma);
}
void RCOutput::bdshot_config_icu_dshot_f1(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch)
{
// F103 does not support both edges input capture so we need to set up two channels
// both pointing at the same input to capture the data. The triggered channel
// needs to handle the second edge - so rising or falling - so that we get an
// even number of half-words in the DMA buffer
switch(ccr_ch) {
case 0:
case 1: {
// Disable the IC1 and IC2: Reset the CCxE Bit
MODIFY_REG(TIMx->CCER, TIM_CCER_CC1E | TIM_CCER_CC2E, 0);
// Select the Input and set the filter and the prescaler value
if (chan == 0) { // TI1
MODIFY_REG(TIMx->CCMR1,
(TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC),
(TIM_CCMR1_CC1S_0 | TIM_CCMR1_IC1F_1));// 4 samples per output transition
MODIFY_REG(TIMx->CCMR1,
(TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC),
(TIM_CCMR1_CC2S_1 | TIM_CCMR1_IC2F_1));
} else { // TI2
MODIFY_REG(TIMx->CCMR1,
(TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC),
(TIM_CCMR1_CC1S_1 | TIM_CCMR1_IC1F_1));
MODIFY_REG(TIMx->CCMR1,
(TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC),
(TIM_CCMR1_CC2S_0 | TIM_CCMR1_IC2F_1));
}
if (ccr_ch == 0) {
// Select the Polarity as falling on IC2 and rising on IC1
MODIFY_REG(TIMx->CCER, TIM_CCER_CC1P | TIM_CCER_CC2P, TIM_CCER_CC2P | TIM_CCER_CC1E | TIM_CCER_CC2E);
MODIFY_REG(TIMx->DIER, TIM_DIER_CC1DE | TIM_DIER_CC2DE, TIM_DIER_CC1DE);
} else {
// Select the Polarity as falling on IC1 and rising on IC2
MODIFY_REG(TIMx->CCER, TIM_CCER_CC1P | TIM_CCER_CC2P, TIM_CCER_CC1P | TIM_CCER_CC1E | TIM_CCER_CC2E);
MODIFY_REG(TIMx->DIER, TIM_DIER_CC1DE | TIM_DIER_CC2DE, TIM_DIER_CC2DE);
}
break;
}
case 2:
case 3: {
MODIFY_REG(TIMx->CCER, TIM_CCER_CC3E | TIM_CCER_CC4E, 0);
// Select the Input and set the filter and the prescaler value
if (chan == 2) { // TI3
MODIFY_REG(TIMx->CCMR2,
(TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC),
(TIM_CCMR2_CC3S_0 | TIM_CCMR2_IC3F_1));
MODIFY_REG(TIMx->CCMR2,
(TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC),
(TIM_CCMR2_CC4S_1 | TIM_CCMR2_IC4F_1));
} else { // TI4
MODIFY_REG(TIMx->CCMR2,
(TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC),
(TIM_CCMR2_CC3S_1 | TIM_CCMR2_IC3F_1));
MODIFY_REG(TIMx->CCMR2,
(TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC),
(TIM_CCMR2_CC4S_0 | TIM_CCMR2_IC4F_1));
}
if (ccr_ch == 2) {
// Select the Polarity as falling on IC4 and rising on IC3
MODIFY_REG(TIMx->CCER, TIM_CCER_CC3P | TIM_CCER_CC4P, TIM_CCER_CC4P | TIM_CCER_CC3E | TIM_CCER_CC4E);
MODIFY_REG(TIMx->DIER, TIM_DIER_CC3DE | TIM_DIER_CC4DE, TIM_DIER_CC3DE);
} else {
// Select the Polarity as falling on IC3 and rising on IC4
MODIFY_REG(TIMx->CCER, TIM_CCER_CC3P | TIM_CCER_CC4P, TIM_CCER_CC3P | TIM_CCER_CC3E | TIM_CCER_CC4E);
MODIFY_REG(TIMx->DIER, TIM_DIER_CC3DE | TIM_DIER_CC4DE, TIM_DIER_CC4DE);
}
break;
}
default:
break;
}
}
#define INVALID_ERPM 0xfffU
// decode a telemetry packet from a GCR encoded stride buffer, take from betaflight decodeTelemetryPacket
// see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 for a description of the protocol
uint32_t RCOutput::bdshot_decode_telemetry_packet_f1(dmar_uint_t* buffer, uint32_t count, bool reversed)
{
if (!reversed) {
return bdshot_decode_telemetry_packet(buffer, count);
}
uint32_t value = 0;
uint32_t bits = 0;
uint32_t len;
// on F103 we are reading one edge with ICn and the other with ICn+1, the DMA architecture only
// allows to trigger on a single register dictated by the DMA input capture channel being used.
// even though we are reading multiple registers per transfer we always cannot trigger on one or other
// of the registers and if the one we trigger on is the one that is numerically first each register
// pair that we read will be swapped in time. in this case we trigger on ICn and then read CCRn and CCRn+1
// giving us the new value of ICn and the old value of ICn+1. in order to avoid reading garbage on the
// first read we trigger ICn on the rising edge. this gives us all the data but with each pair of bytes
// transposed. we thus need to untranspose as we decode
dmar_uint_t oldValue = buffer[1];
for (int32_t i = 0; i <= count+1; ) {
if (i < count) {
dmar_int_t diff = buffer[i] - oldValue;
if (bits >= 21U) {
break;
}
len = (diff + TELEM_IC_SAMPLE/2U) / TELEM_IC_SAMPLE;
} else {
len = 21U - bits;
}
value <<= len;
value |= 1U << (len - 1U);
oldValue = buffer[i];
bits += len;
i += (i%2 ? -1 : 3);
}
if (bits != 21U) {
return INVALID_ERPM;
}
static const uint32_t decode[32] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15,
0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 };
uint32_t decodedValue = decode[value & 0x1fU];
decodedValue |= decode[(value >> 5U) & 0x1fU] << 4U;
decodedValue |= decode[(value >> 10U) & 0x1fU] << 8U;
decodedValue |= decode[(value >> 15U) & 0x1fU] << 12U;
uint32_t csum = decodedValue;
csum = csum ^ (csum >> 8U); // xor bytes
csum = csum ^ (csum >> 4U); // xor nibbles
if ((csum & 0xfU) != 0xfU) {
return INVALID_ERPM;
}
decodedValue >>= 4;
return decodedValue;
}
#endif // HAL_WITH_BIDIR_DSHOT && STM32F1
#endif // HAL_USE_PWM
#endif // IOMCU_FW && HAL_DSHOT_ENABLED

22
libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
View File

@ -39,7 +39,7 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
return false;
}
if (soft_serial_waiting() || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
if (soft_serial_waiting() || !is_dshot_send_allowed(group.dshot_state)) {
// doing serial output or DMAR input, don't send DShot pulses
return false;
}
@ -57,18 +57,8 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
group.dshot_waiter = rcout_thread_ctx;
bool bdshot_telem = false;
#ifdef HAL_WITH_BIDIR_DSHOT
uint32_t active_channels = group.ch_mask & group.en_mask;
// no need to get the input capture lock
group.bdshot.enabled = false;
if ((_bdshot.mask & active_channels) == active_channels) {
bdshot_telem = true;
// it's not clear why this is required, but without it we get no output
if (group.pwm_started) {
pwmStop(group.pwm_drv);
}
pwmStart(group.pwm_drv, &group.pwm_cfg);
group.pwm_started = true;
}
bdshot_prepare_for_next_pulse(group);
bdshot_telem = group.bdshot.enabled;
#endif
memset((uint8_t *)group.dma_buffer, 0, DSHOT_BUFFER_LENGTH);
@ -121,7 +111,11 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman
DshotCommandPacket pkt;
pkt.command = command;
pkt.chan = chan - chan_offset;
if (chan != ALL_CHANNELS) {
pkt.chan = chan - chan_offset;
} else {
pkt.chan = ALL_CHANNELS;
}
if (command_timeout_ms == 0) {
pkt.cycle = MAX(10, repeat_count);
} else {

2
libraries/AP_HAL_ChibiOS/hwdef/iomcu-dshot/hwdef.inc
View File

@ -16,7 +16,7 @@ undef AP_HAL_SHARED_DMA_ENABLED
define STM32_TIM_TIM2_UP_DMA_STREAM STM32_DMA_STREAM_ID(1, 2)
define STM32_TIM_TIM2_UP_DMA_CHAN 1
# TIM4_UP (PWM 3/4) cannot be used with sharing as channels used by high speed USART2 RX/TX
# TIM4_UP (PWM 3/4) cannot be used without sharing as channels used by high speed USART2 RX/TX
define AP_HAL_SHARED_DMA_ENABLED 1
define STM32_TIM_TIM4_UP_DMA_STREAM STM32_DMA_STREAM_ID(1, 7)
define STM32_TIM_TIM4_UP_DMA_CHAN 1

37
libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat
View File

@ -12,3 +12,40 @@ DMA_NOMAP 1
# only four timers on F103xB so use TIM1 for system timer
STM32_ST_USE_TIMER 1
define HAL_WITH_ESC_TELEM 1
undef PA0 PA1 PB8 PB9
# the order is important here as it determines the order that timers are used to sending dshot
# TIM4 needs to go first so that TIM4_UP can be freed up to be used by input capture for TIM2
PB8 TIM4_CH3 TIM4 PWM(3) GPIO(103) BIDIR
PB9 TIM4_CH4 TIM4 PWM(4) GPIO(104)
PA0 TIM2_CH1 TIM2 PWM(1) GPIO(101)
PA1 TIM2_CH2 TIM2 PWM(2) GPIO(102) BIDIR # DMA channel 7, shared with TIM4_UP and USART2_TX
# currently no support for having mixed outputs on the same timer
# PB1 TIM3_CH4 TIM3 PWM(8) GPIO(108) BIDIR # DMA channel 3, shared with TIM3_UP
# TIM2_UP - (1,2)
# TIM4_UP - (1,7)
# TIM3_UP - (1,3)
# TIM2_CH2 (PWM 1/2)
define STM32_TIM_TIM2_CH2_DMA_STREAM STM32_DMA_STREAM_ID(1, 7)
define STM32_TIM_TIM2_CH2_DMA_CHAN 1
# TIM4_CH3 (PWM 3/4)
define STM32_TIM_TIM4_CH3_DMA_STREAM STM32_DMA_STREAM_ID(1, 5)
define STM32_TIM_TIM4_CH3_DMA_CHAN 1
# TIM3_CH4 (PWM 7-8)
define STM32_TIM_TIM3_CH4_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
define STM32_TIM_TIM3_CH4_DMA_CHAN 1
define HAL_TIM4_UP_SHARED 1
undef SHARED_DMA_MASK
define SHARED_DMA_MASK (1U<<STM32_TIM_TIM4_UP_DMA_STREAM | 1U<<STM32_TIM_TIM2_CH2_DMA_STREAM | 1U<<STM32_UART_USART2_TX_DMA_STREAM)
undef MAIN_STACK
MAIN_STACK 0x400

5
libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
View File

@ -2225,13 +2225,16 @@ INCLUDE common.ld
# define HAL_IC%u_CH%u_DMA_CONFIG false, 0, 0
#endif
''' % (n, i, n, i, n, i, n, i, n, i, n, i)
hal_icu_cfg += '}, \\'
hal_icu_cfg += '}, HAL_TIM%u_UP_SHARED, \\' % n
f.write('''#if defined(STM32_TIM_TIM%u_UP_DMA_STREAM) && defined(STM32_TIM_TIM%u_UP_DMA_CHAN)
# define HAL_PWM%u_DMA_CONFIG true, STM32_TIM_TIM%u_UP_DMA_STREAM, STM32_TIM_TIM%u_UP_DMA_CHAN
#else
# define HAL_PWM%u_DMA_CONFIG false, 0, 0
#endif\n%s''' % (n, n, n, n, n, n, hal_icu_def))
f.write('''#if !defined(HAL_TIM%u_UP_SHARED)
#define HAL_TIM%u_UP_SHARED false
#endif\n''' % (n, n))
f.write('''#define HAL_PWM_GROUP%u { %s, \\
{%u, %u, %u, %u}, \\
/* Group Initial Config */ \\

5
libraries/AP_HAL_ChibiOS/shared_dma.cpp
View File

@ -63,6 +63,11 @@ bool Shared_DMA::is_shared(uint8_t stream_id)
return (stream_id < SHARED_DMA_MAX_STREAM_ID) && ((1U<<stream_id) & SHARED_DMA_MASK) != 0;
}
bool Shared_DMA::is_shared()
{
return is_shared(stream_id1) || is_shared(stream_id2);
}
//remove any assigned deallocator or allocator
void Shared_DMA::unregister()
{

1
libraries/AP_HAL_ChibiOS/shared_dma.h
View File

@ -69,6 +69,7 @@ public:
// return true if a stream ID is shared between two peripherals
static bool is_shared(uint8_t stream_id);
bool is_shared();
private:
dma_allocate_fn_t allocate;