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

HAL_ChibiOS: support DShot output

Browse Source 
use DMAR burst DMA to minimise number of DMA channels needed

thanks to betaflight for the great reference implementation!
This commit is contained in:
Andrew Tridgell 2018-03-14 17:06:30 +11:00
parent 294aac6955
commit bc32276966
12 changed files with 378 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_HAL_ChibiOS/I2CDevice.cpp
View File

@ -49,8 +49,8 @@ I2CBus I2CDeviceManager::businfo[ARRAY_SIZE_SIMPLE(I2CD)];
void I2CBus::dma_init(void)
{
dma_handle = new Shared_DMA(I2CD[busnum].dma_channel_tx, I2CD[busnum].dma_channel_rx,
FUNCTOR_BIND_MEMBER(&I2CBus::dma_allocate, void),
FUNCTOR_BIND_MEMBER(&I2CBus::dma_deallocate, void));
FUNCTOR_BIND_MEMBER(&I2CBus::dma_allocate, void, Shared_DMA *),
FUNCTOR_BIND_MEMBER(&I2CBus::dma_deallocate, void, Shared_DMA *));
}
// Clear Bus to avoid bus lockup
@ -135,7 +135,7 @@ I2CDevice::~I2CDevice()
/*
allocate DMA channel
*/
void I2CBus::dma_allocate(void)
void I2CBus::dma_allocate(Shared_DMA *ctx)
{
if (!i2c_started) {
osalDbgAssert(I2CD[busnum].i2c->state == I2C_STOP, "i2cStart state");
@ -148,7 +148,7 @@ void I2CBus::dma_allocate(void)
/*
deallocate DMA channel
*/
void I2CBus::dma_deallocate(void)
void I2CBus::dma_deallocate(Shared_DMA *)
{
if (i2c_started) {
osalDbgAssert(I2CD[busnum].i2c->state == I2C_READY, "i2cStart state");

4
libraries/AP_HAL_ChibiOS/I2CDevice.h
View File

@ -39,8 +39,8 @@ public:
bool i2c_started;
bool i2c_active;
void dma_allocate(void);
void dma_deallocate(void);
void dma_allocate(Shared_DMA *);
void dma_deallocate(Shared_DMA *);
void dma_init(void);
static void clear_all(void);
static void clear_bus(ioline_t scl_line, uint8_t scl_af);

359
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -39,13 +39,20 @@ void RCOutput::init()
{
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
//Start Pwm groups
pwmStart(pwm_group_list[i].pwm_drv, &pwm_group_list[i].pwm_cfg);
pwm_group &group = pwm_group_list[i];
group.ch_mask = 0;
group.current_mode = MODE_PWM_NORMAL;
for (uint8_t j = 0; j < 4; j++ ) {
if (pwm_group_list[i].chan[j] != CHAN_DISABLED) {
total_channels = MAX(total_channels, pwm_group_list[i].chan[j]+1);
if (group.chan[j] != CHAN_DISABLED) {
total_channels = MAX(total_channels, group.chan[j]+1);
group.ch_mask |= (1U<<group.chan[j]);
}
}
if (group.ch_mask != 0) {
pwmStart(group.pwm_drv, &group.pwm_cfg);
}
}
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
iomcu.init();
@ -61,11 +68,6 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
{
//check if the request spans accross any of the channel groups
uint8_t update_mask = 0;
// greater than 400 doesn't give enough room at higher periods for
// the down pulse
if (freq_hz > 400 && _output_mode != MODE_PWM_BRUSHED) {
freq_hz = 400;
}
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
@ -79,13 +81,14 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
}
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
uint16_t grp_ch_mask = 0;
for (uint8_t j=0; j<4; j++) {
if (pwm_group_list[i].chan[j] != CHAN_DISABLED) {
grp_ch_mask |= (1U<<pwm_group_list[i].chan[j]);
// greater than 400 doesn't give enough room at higher periods for
// the down pulse. This still allows for high rate with oneshot and dshot.
pwm_group &group = pwm_group_list[i];
uint16_t group_freq = freq_hz;
if (group_freq > 400 && group.current_mode != MODE_PWM_BRUSHED) {
group_freq = 400;
}
}
if ((grp_ch_mask & chmask) != 0) {
if ((group.ch_mask & chmask) != 0) {
/*
we enable the new frequency on all groups that have one
of the requested channels. This means we may enable high
@ -93,41 +96,41 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
is needed in order to fly a vehicle such a a hex
multicopter properly
*/
update_mask |= grp_ch_mask;
uint16_t freq_set = freq_hz;
uint32_t old_clock = pwm_group_list[i].pwm_cfg.frequency;
update_mask |= group.ch_mask;
uint16_t freq_set = group_freq;
uint32_t old_clock = group.pwm_cfg.frequency;
if (freq_set > 400 && pwm_group_list[i].pwm_cfg.frequency == 1000000) {
if (freq_set > 400 && group.pwm_cfg.frequency == 1000000) {
// need to change to an 8MHz clock
pwm_group_list[i].pwm_cfg.frequency = 8000000;
} else if (freq_set <= 400 && pwm_group_list[i].pwm_cfg.frequency == 8000000) {
group.pwm_cfg.frequency = 8000000;
} else if (freq_set <= 400 && group.pwm_cfg.frequency == 8000000) {
// need to change to an 1MHz clock
pwm_group_list[i].pwm_cfg.frequency = 1000000;
group.pwm_cfg.frequency = 1000000;
}
// check if the frequency is possible, and keep halving
// down to 1MHz until it is OK with the hardware timer we
// are using. If we don't do this we'll hit an assert in
// the ChibiOS PWM driver on some timers
PWMDriver *pwmp = pwm_group_list[i].pwm_drv;
PWMDriver *pwmp = group.pwm_drv;
uint32_t psc = (pwmp->clock / pwmp->config->frequency) - 1;
while ((psc > 0xFFFF || ((psc + 1) * pwmp->config->frequency) != pwmp->clock) &&
pwm_group_list[i].pwm_cfg.frequency > 1000000) {
pwm_group_list[i].pwm_cfg.frequency /= 2;
group.pwm_cfg.frequency > 1000000) {
group.pwm_cfg.frequency /= 2;
psc = (pwmp->clock / pwmp->config->frequency) - 1;
}
if (old_clock != pwm_group_list[i].pwm_cfg.frequency) {
if (old_clock != group.pwm_cfg.frequency) {
// we need to stop and start to setup the new clock
pwmStop(pwm_group_list[i].pwm_drv);
pwmStart(pwm_group_list[i].pwm_drv, &pwm_group_list[i].pwm_cfg);
pwmStop(group.pwm_drv);
pwmStart(group.pwm_drv, &group.pwm_cfg);
}
pwmChangePeriod(pwm_group_list[i].pwm_drv,
pwm_group_list[i].pwm_cfg.frequency/freq_set);
pwmChangePeriod(group.pwm_drv,
group.pwm_cfg.frequency/freq_set);
}
if (group_freq > 50) {
fast_channel_mask |= group.ch_mask;
}
if (freq_hz > 50) {
fast_channel_mask |= update_mask;
}
if (chmask != update_mask) {
hal.console->printf("RCOutput: Failed to set PWM frequency req %x set %x\n", (unsigned)chmask, (unsigned)update_mask);
@ -145,18 +148,13 @@ void RCOutput::set_default_rate(uint16_t freq_hz)
}
#endif
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
uint16_t grp_ch_mask = 0;
for (uint8_t j=0; j<4; j++) {
if (pwm_group_list[i].chan[j] != CHAN_DISABLED) {
grp_ch_mask |= (1U<<pwm_group_list[i].chan[j]);
}
}
if (grp_ch_mask & fast_channel_mask) {
pwm_group &group = pwm_group_list[i];
if ((group.ch_mask & fast_channel_mask) || group.ch_mask == 0) {
// don't change fast channels
continue;
}
pwmChangePeriod(pwm_group_list[i].pwm_drv,
pwm_group_list[i].pwm_cfg.frequency/freq_hz);
pwmChangePeriod(group.pwm_drv,
group.pwm_cfg.frequency/freq_hz);
}
}
@ -173,9 +171,10 @@ uint16_t RCOutput::get_freq(uint8_t chan)
chan -= chan_offset;
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
for (uint8_t j = 0; j < 4; j++) {
if (pwm_group_list[i].chan[j] == chan) {
return pwm_group_list[i].pwm_drv->config->frequency / pwm_group_list[i].pwm_drv->period;
if (group.chan[j] == chan) {
return group.pwm_drv->config->frequency / group.pwm_drv->period;
}
}
}
@ -194,8 +193,9 @@ void RCOutput::enable_ch(uint8_t chan)
chan -= chan_offset;
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
for (uint8_t j = 0; j < 4; j++) {
if ((pwm_group_list[i].chan[j] == chan) && !(en_mask & 1<<chan)) {
if ((group.chan[j] == chan) && !(en_mask & 1<<chan)) {
en_mask |= 1<<chan;
}
}
@ -213,9 +213,10 @@ void RCOutput::disable_ch(uint8_t chan)
chan -= chan_offset;
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
for (uint8_t j = 0; j < 4; j++) {
if (pwm_group_list[i].chan[j] == chan) {
pwmDisableChannel(pwm_group_list[i].pwm_drv, j);
if (group.chan[j] == chan) {
pwmDisableChannel(group.pwm_drv, j);
en_mask &= ~(1<<chan);
}
}
@ -262,47 +263,52 @@ void RCOutput::push_local(void)
uint8_t need_trigger = 0;
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
for (uint8_t j = 0; j < 4; j++) {
uint8_t chan = pwm_group_list[i].chan[j];
uint8_t chan = group.chan[j];
if (chan == CHAN_DISABLED) {
continue;
}
if (outmask & (1UL<<chan)) {
uint32_t period_us = period[chan];
if(_output_mode == MODE_PWM_BRUSHED && (fast_channel_mask & (1UL<<chan))) {
// note that we only use brushed signals on fast
// channels. This allows for ordinary PWM on
// servos attached to a brushed vehicle
if (group.current_mode == MODE_PWM_BRUSHED) {
if (period_us <= _esc_pwm_min) {
period_us = 0;
} else if (period_us >= _esc_pwm_max) {
period_us = PWM_FRACTION_TO_WIDTH(pwm_group_list[i].pwm_drv, 1, 1);
period_us = PWM_FRACTION_TO_WIDTH(group.pwm_drv, 1, 1);
} else {
period_us = PWM_FRACTION_TO_WIDTH(pwm_group_list[i].pwm_drv,\
period_us = PWM_FRACTION_TO_WIDTH(group.pwm_drv,\
(_esc_pwm_max - _esc_pwm_min), (period_us - _esc_pwm_min));
}
pwmEnableChannel(pwm_group_list[i].pwm_drv, j, period_us);
} else {
uint32_t width = (pwm_group_list[i].pwm_cfg.frequency/1000000) * period_us;
pwmEnableChannel(pwm_group_list[i].pwm_drv, j, width);
pwmEnableChannel(group.pwm_drv, j, period_us);
} else if (group.current_mode < MODE_PWM_DSHOT150) {
uint32_t width = (group.pwm_cfg.frequency/1000000) * period_us;
pwmEnableChannel(group.pwm_drv, j, width);
} else if (group.current_mode >= MODE_PWM_DSHOT150 && group.current_mode <= MODE_PWM_DSHOT1200) {
// set period_us to time for pulse output, to enable very fast rates
period_us = dshot_pulse_time_us;
}
if (period_us > widest_pulse) {
widest_pulse = period_us;
}
if (group.current_mode == MODE_PWM_ONESHOT ||
(group.current_mode >= MODE_PWM_DSHOT150 &&
group.current_mode <= MODE_PWM_DSHOT1200)) {
need_trigger |= (1U<<i);
}
}
}
}
if (widest_pulse > 2300) {
widest_pulse = 2300;
}
trigger_widest_pulse = widest_pulse;
trigger_groups = need_trigger;
trigger_groupmask = need_trigger;
if (trigger_groups && _output_mode == MODE_PWM_ONESHOT) {
trigger_oneshot();
if (trigger_groupmask) {
trigger_groups();
}
}
@ -360,26 +366,80 @@ void RCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
/*
setup output mode
*/
void RCOutput::set_output_mode(enum output_mode mode)
void RCOutput::set_output_mode(uint16_t mask, enum output_mode mode)
{
_output_mode = mode;
if (_output_mode == MODE_PWM_BRUSHED) {
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
if (((group.ch_mask << chan_offset) & mask) == 0) {
// this group is not affected
continue;
}
group.current_mode = mode;
if (mode == MODE_PWM_BRUSHED) {
// force zero output initially
for (uint8_t i=chan_offset; i<chan_offset+num_channels; i++) {
for (uint8_t i=chan_offset; i<16; i++) {
if ((group.ch_mask << chan_offset) & (1U<<i)) {
write(i, 0);
}
}
if (_output_mode == MODE_PWM_ONESHOT) {
}
if (mode >= MODE_PWM_DSHOT150 && mode <= MODE_PWM_DSHOT1200) {
if (!group.have_up_dma) {
// fallback to ONESHOT125
hal.console->printf("DShot unavailable on mask 0x%04x\n", group.ch_mask);
group.current_mode = MODE_PWM_NORMAL;
} else {
if (!group.dma_buffer) {
group.dma_buffer = (uint32_t *)hal.util->malloc_type(dshot_buffer_length, AP_HAL::Util::MEM_DMA_SAFE);
if (!group.dma_buffer) {
hal.console->printf("DShot setup no memory\n");
group.current_mode = MODE_PWM_NORMAL;
continue;
}
}
// for dshot we setup for DMAR based output
if (!group.dma) {
group.dma = STM32_DMA_STREAM(group.dma_up_stream_id);
group.dma_handle = new Shared_DMA(group.dma_up_stream_id, SHARED_DMA_NONE,
FUNCTOR_BIND_MEMBER(&RCOutput::dma_allocate, void, Shared_DMA *),
FUNCTOR_BIND_MEMBER(&RCOutput::dma_deallocate, void, Shared_DMA *));
if (!group.dma_handle) {
hal.console->printf("DShot setup no memory\n");
group.current_mode = MODE_PWM_NORMAL;
continue;
}
}
const uint16_t rates[(1 + MODE_PWM_DSHOT1200) - MODE_PWM_DSHOT150] = { 150, 300, 600, 1200 };
uint32_t rate = rates[uint8_t(mode - MODE_PWM_DSHOT150)] * 1000UL;
const uint32_t bit_period = 19;
// configure timer driver for DMAR at requested rate
pwmStop(group.pwm_drv);
group.pwm_cfg.frequency = rate * bit_period;
group.pwm_cfg.period = bit_period;
group.pwm_cfg.dier = TIM_DIER_UDE;
group.pwm_cfg.cr2 = 0;
pwmStart(group.pwm_drv, &group.pwm_cfg);
for (uint8_t j=0; j<4; j++) {
if (group.chan[j] != CHAN_DISABLED) {
pwmEnableChannel(group.pwm_drv, j, 0);
}
}
// calculate min time between pulses
dshot_pulse_time_us = 1000000UL * dshot_bit_length / rate;
}
}
if (group.current_mode == MODE_PWM_ONESHOT) {
// for oneshot we force 1Hz output and then trigger on each loop
for (uint8_t i=0; i< NUM_GROUPS; i++) {
pwmChangePeriod(pwm_group_list[i].pwm_drv, pwm_group_list[i].pwm_cfg.frequency);
pwmChangePeriod(group.pwm_drv, group.pwm_cfg.frequency);
}
}
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
if (mode == MODE_PWM_ONESHOT &&
(mask & ((1U<<chan_offset)-1)) &&
AP_BoardConfig::io_enabled()) {
return iomcu.set_oneshot_mode();
}
#endif
}
}
/*
@ -448,9 +508,9 @@ bool RCOutput::enable_px4io_sbus_out(uint16_t rate_hz)
}
/*
trigger output groups for oneshot mode
trigger output groups for oneshot or dshot modes
*/
void RCOutput::trigger_oneshot(void)
void RCOutput::trigger_groups(void)
{
if (!chMtxTryLock(&trigger_mutex)) {
return;
@ -461,18 +521,25 @@ void RCOutput::trigger_oneshot(void)
hal.scheduler->delay_microseconds(min_pulse_trigger_us - now);
}
osalSysLock();
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
if (trigger_groups & (1U<<i)) {
for (uint8_t i = 0; i < NUM_GROUPS; i++) {
pwm_group &group = pwm_group_list[i];
if (group.current_mode == MODE_PWM_ONESHOT) {
if (trigger_groupmask & (1U<<i)) {
// this triggers pulse output for a channel group
pwm_group_list[i].pwm_drv->tim->EGR = STM32_TIM_EGR_UG;
group.pwm_drv->tim->EGR = STM32_TIM_EGR_UG;
}
} else if (group.current_mode >= MODE_PWM_DSHOT150 && group.current_mode <= MODE_PWM_DSHOT1200) {
dshot_send(group, false);
}
}
osalSysUnlock();
/*
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;
chMtxUnlock(&trigger_mutex);
}
@ -485,16 +552,154 @@ void RCOutput::trigger_oneshot(void)
*/
void RCOutput::timer_tick(void)
{
if (_output_mode != MODE_PWM_ONESHOT ||
trigger_groups == 0 ||
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
if (dshot_delayed_trigger_mask & (1U<<i)) {
// do a blocking send now
dshot_send(group, true);
}
}
if (trigger_groupmask == 0 ||
min_pulse_trigger_us == 0) {
return;
}
uint64_t now = AP_HAL::micros64();
if (now - min_pulse_trigger_us > 10000) {
if (now > min_pulse_trigger_us &&
now - min_pulse_trigger_us > 10000) {
// trigger at a minimum of 100Hz
trigger_oneshot();
trigger_groups();
}
}
/*
allocate DMA channel
*/
void RCOutput::dma_allocate(Shared_DMA *ctx)
{
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
if (group.dma_handle == ctx) {
dmaStreamAllocate(group.dma, 10, dma_irq_callback, &group);
}
}
}
/*
deallocate DMA channel
*/
void RCOutput::dma_deallocate(Shared_DMA *ctx)
{
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
if (group.dma_handle == ctx) {
dmaStreamRelease(group.dma);
}
}
}
/*
create a DSHOT 16 bit packet. Based on prepareDshotPacket from betaflight
*/
uint16_t RCOutput::create_dshot_packet(const uint16_t value)
{
uint16_t packet = (value << 1); // no telemetry request
// compute checksum
uint16_t csum = 0;
uint16_t csum_data = packet;
for (uint8_t i = 0; i < 3; i++) {
csum ^= csum_data;
csum_data >>= 4;
}
csum &= 0xf;
// append checksum
packet = (packet << 4) | csum;
return packet;
}
/*
fill in a DMA buffer for dshot
*/
void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet)
{
const uint8_t DSHOT_MOTOR_BIT_0 = 7;
const uint8_t DSHOT_MOTOR_BIT_1 = 14;
for (uint16_t i = 0; i < 16; i++) {
buffer[i * stride] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0;
packet <<= 1;
}
}
/*
send a set of DShot packets for a channel group
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, bool blocking)
{
uint8_t groupidx = &group - pwm_group_list;
if (blocking) {
group.dma_handle->lock();
dshot_delayed_trigger_mask &= ~(1U<<groupidx);
} else {
if (!group.dma_handle->lock_nonblock()) {
dshot_delayed_trigger_mask |= 1U<<groupidx;
return;
}
}
for (uint8_t i=0; i<4; i++) {
uint8_t chan = group.chan[i];
if (chan != CHAN_DISABLED) {
uint16_t pwm = period[chan];
pwm = constrain_int16(pwm, _esc_pwm_min, _esc_pwm_max);
uint16_t value = 2000UL * uint32_t(pwm - _esc_pwm_min) / uint32_t(_esc_pwm_max - _esc_pwm_min);
if (value != 0) {
// dshot values are from 48 to 2047. Zero means off.
value += 47;
}
uint16_t packet = create_dshot_packet(value);
fill_DMA_buffer_dshot(group.dma_buffer + i, 4, packet);
}
}
/*
The DMA approach we are using is based on the DMAR method from
betaflight. We use the TIMn_UP DMA channel for the timer, and
setup an interleaved set of pulse durations, with a stride of 4
(for the 4 channels). We use the DMAR register to point the DMA
engine at the 4 CCR registers of the timer, so it fills in the
pulse widths for each timer in turn. This means we only use a
single DMA channel for groups of 4 timer channels. See the "DMA
address for full transfer TIMx_DMAR" section of the
datasheet. Many thanks to the betaflight developers for coming
up with this great method.
*/
dmaStreamSetPeripheral(group.dma, &(group.pwm_drv->tim->DMAR));
dmaStreamSetMemory0(group.dma, group.dma_buffer);
dmaStreamSetTransactionSize(group.dma, dshot_buffer_length/sizeof(uint32_t));
dmaStreamSetFIFO(group.dma, STM32_DMA_FCR_DMDIS | STM32_DMA_FCR_FTH_FULL);
dmaStreamSetMode(group.dma,
STM32_DMA_CR_CHSEL(group.dma_up_channel) |
STM32_DMA_CR_DIR_M2P | 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);
// setup for 4 burst strided transfers. 0x0D is the register
// address offset of the CCR registers in the timer peripheral
group.pwm_drv->tim->DCR = 0x0D | STM32_TIM_DCR_DBL(3);
dmaStreamEnable(group.dma);
}
/*
DMA interrupt handler. Used to mark DMA completed for DShot
*/
void RCOutput::dma_irq_callback(void *p, uint32_t flags)
{
pwm_group *group = (pwm_group *)p;
dmaStreamDisable(group->dma);
group->dma_handle->unlock();
}
#endif // HAL_USE_PWM

36
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -17,6 +17,7 @@
#pragma once
#include "AP_HAL_ChibiOS.h"
#include "shared_dma.h"
#include "ch.h"
#include "hal.h"
@ -38,7 +39,7 @@ public:
_esc_pwm_min = min_pwm;
_esc_pwm_max = max_pwm;
}
void set_output_mode(enum output_mode mode) override;
void set_output_mode(uint16_t mask, enum output_mode mode) override;
float scale_esc_to_unity(uint16_t pwm) override {
return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0;
@ -76,8 +77,16 @@ private:
uint8_t chan[4]; // chan number, zero based, 255 for disabled
PWMConfig pwm_cfg;
PWMDriver* pwm_drv;
bool have_up_dma; // can we do DMAR outputs for DShot?
uint8_t dma_up_stream_id;
uint8_t dma_up_channel;
enum output_mode current_mode;
uint16_t ch_mask;
const stm32_dma_stream_t *dma;
Shared_DMA *dma_handle;
uint32_t *dma_buffer;
bool have_lock;
};
enum output_mode _output_mode = MODE_PWM_NORMAL;
static pwm_group pwm_group_list[];
uint16_t _esc_pwm_min;
@ -107,7 +116,10 @@ private:
mutex_t trigger_mutex;
// which output groups need triggering
uint8_t trigger_groups;
uint8_t trigger_groupmask;
// mask of groups needing retriggering for DShot
uint8_t dshot_delayed_trigger_mask;
// widest pulse for oneshot triggering
uint16_t trigger_widest_pulse;
@ -115,8 +127,22 @@ private:
// push out values to local PWM
void push_local(void);
// trigger oneshot pulses
void trigger_oneshot(void);
// trigger group pulses
void trigger_groups(void);
/*
DShot handling
*/
const uint8_t dshot_post = 2;
const uint16_t dshot_bit_length = 16 + dshot_post;
const uint16_t dshot_buffer_length = dshot_bit_length*4*sizeof(uint32_t);
uint32_t dshot_pulse_time_us;
void dma_allocate(Shared_DMA *ctx);
void dma_deallocate(Shared_DMA *ctx);
uint16_t create_dshot_packet(const uint16_t value);
void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet);
void dshot_send(pwm_group &group, bool blocking);
static void dma_irq_callback(void *p, uint32_t flags);
};
#endif // HAL_USE_PWM

8
libraries/AP_HAL_ChibiOS/SPIDevice.cpp
View File

@ -56,15 +56,15 @@ SPIBus::SPIBus(uint8_t _bus) :
// allow for sharing of DMA channels with other peripherals
dma_handle = new Shared_DMA(spi_devices[bus].dma_channel_rx,
spi_devices[bus].dma_channel_tx,
FUNCTOR_BIND_MEMBER(&SPIBus::dma_allocate, void),
FUNCTOR_BIND_MEMBER(&SPIBus::dma_deallocate, void));
FUNCTOR_BIND_MEMBER(&SPIBus::dma_allocate, void, Shared_DMA *),
FUNCTOR_BIND_MEMBER(&SPIBus::dma_deallocate, void, Shared_DMA *));
}
/*
allocate DMA channel
*/
void SPIBus::dma_allocate(void)
void SPIBus::dma_allocate(Shared_DMA *ctx)
{
// nothing to do as we call spiStart() on each transaction
}
@ -72,7 +72,7 @@ void SPIBus::dma_allocate(void)
/*
deallocate DMA channel
*/
void SPIBus::dma_deallocate(void)
void SPIBus::dma_deallocate(Shared_DMA *ctx)
{
// another non-SPI peripheral wants one of our DMA channels
if (spi_started) {

4
libraries/AP_HAL_ChibiOS/SPIDevice.h
View File

@ -31,8 +31,8 @@ public:
struct spi_dev_s *dev;
uint8_t bus;
SPIConfig spicfg;
void dma_allocate(void);
void dma_deallocate(void);
void dma_allocate(Shared_DMA *ctx);
void dma_deallocate(Shared_DMA *ctx);
bool spi_started;
};

8
libraries/AP_HAL_ChibiOS/UARTDriver.cpp
View File

@ -208,8 +208,8 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
// cannot be shared
dma_handle = new Shared_DMA(sdef.dma_tx_stream_id,
SHARED_DMA_NONE,
FUNCTOR_BIND_MEMBER(&UARTDriver::dma_tx_allocate, void),
FUNCTOR_BIND_MEMBER(&UARTDriver::dma_tx_deallocate, void));
FUNCTOR_BIND_MEMBER(&UARTDriver::dma_tx_allocate, void, Shared_DMA *),
FUNCTOR_BIND_MEMBER(&UARTDriver::dma_tx_deallocate, void, Shared_DMA *));
}
_device_initialised = true;
}
@ -261,7 +261,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
set_flow_control(_flow_control);
}
void UARTDriver::dma_tx_allocate(void)
void UARTDriver::dma_tx_allocate(Shared_DMA *ctx)
{
#if HAL_USE_SERIAL == TRUE
osalDbgAssert(txdma == nullptr, "double DMA allocation");
@ -277,7 +277,7 @@ void UARTDriver::dma_tx_allocate(void)
#endif // HAL_USE_SERIAL
}
void UARTDriver::dma_tx_deallocate(void)
void UARTDriver::dma_tx_deallocate(Shared_DMA *ctx)
{
chSysLock();
dmaStreamRelease(txdma);

4
libraries/AP_HAL_ChibiOS/UARTDriver.h
View File

@ -132,8 +132,8 @@ private:
static void rxbuff_full_irq(void* self, uint32_t flags);
static void tx_complete(void* self, uint32_t flags);
void dma_tx_allocate(void);
void dma_tx_deallocate(void);
void dma_tx_allocate(Shared_DMA *ctx);
void dma_tx_deallocate(Shared_DMA *ctx);
void update_rts_line(void);
void check_dma_tx_completion(void);

16
libraries/AP_HAL_ChibiOS/examples/DShot/DShot.cpp
View File

@ -14,9 +14,9 @@ void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
// use TIM1_CH1 to TIM1_CH4
#define DMA_STREAM STM32_DMA_STREAM_ID(2,5) // TIM1_UP DMA
#define DMA_STREAM STM32_TIM_TIM1_UP_DMA_STREAM
#define PWMD PWMD1
#define DMA_CH 6 // TIM1_UP is on channel 6
#define DMA_CH STM32_TIM_TIM1_UP_DMA_CHAN
// choose DShot rate. Can be 150, 300, 600 or 1200
#define DSHOT_RATE 600U
@ -56,6 +56,10 @@ void setup(void) {
hal.console->printf("Starting DShot test\n");
dma = STM32_DMA_STREAM(DMA_STREAM);
buffer = (uint32_t *)hal.util->malloc_type(buffer_length, AP_HAL::Util::MEM_DMA_SAFE);
dmaStreamAllocate(dma, 10, NULL, NULL);
pwmStart(&PWMD, &pwm_config);
}
@ -99,7 +103,6 @@ static void test_dshot_send(void)
fill_DMA_buffer_dshot(buffer + i, 4, packets[i]);
}
dmaStreamAllocate(dma, 10, NULL, NULL);
dmaStreamSetPeripheral(dma, &(PWMD.tim->DMAR));
dmaStreamSetMemory0(dma, buffer);
dmaStreamSetTransactionSize(dma, buffer_length/sizeof(uint32_t));
@ -109,7 +112,6 @@ static void test_dshot_send(void)
STM32_DMA_CR_DIR_M2P | STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_WORD |
STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3));
pwmStart(&PWMD, &pwm_config);
for (uint8_t i=0; i<4; i++) {
pwmEnableChannel(&PWMD, i, 0);
}
@ -123,16 +125,16 @@ static void test_dshot_send(void)
static void test_dshot_cleanup(void)
{
pwmStop(&PWMD);
dmaStreamRelease(dma);
dmaStreamDisable(dma);
}
void loop(void)
{
hal.console->printf(".");
hal.console->printf("tick\n");
test_dshot_send();
hal.scheduler->delay(1);
test_dshot_cleanup();
hal.scheduler->delay(1000);
if (hal.console->available() > 10) {
hal.scheduler->reboot(false);
}

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

@ -687,6 +687,11 @@ def write_PWM_config(f):
advanced_timer = 'false'
pwm_clock = 1000000
period = 20000 * pwm_clock / 1000000
f.write('''#ifdef STM32_TIM_TIM%u_UP_DMA_STREAM
# 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''' % (n, n, n, n, n))
f.write('''#define HAL_PWM_GROUP%u { %s, \\
{%u, %u, %u, %u}, \\
/* Group Initial Config */ \\
@ -700,10 +705,11 @@ def write_PWM_config(f):
{%s, NULL}, \\
{%s, NULL}, \\
{%s, NULL} \\
}, 0, 0}, &PWMD%u}\n''' %
}, 0, 0}, &PWMD%u, \\
HAL_PWM%u_DMA_CONFIG }\n''' %
(group, advanced_timer, chan_list[0], chan_list[1],
chan_list[2], chan_list[3], pwm_clock, period, chan_mode[0],
chan_mode[1], chan_mode[2], chan_mode[3], n))
chan_mode[1], chan_mode[2], chan_mode[3], n, n))
f.write('#define HAL_PWM_GROUPS %s\n\n' % ','.join(groups))
@ -826,7 +832,6 @@ def write_hwdef_header(outfilename):
write_USB_config(f)
write_I2C_config(f)
write_SPI_config(f)
write_PWM_config(f)
write_ADC_config(f)
write_GPIO_config(f)
@ -838,6 +843,8 @@ def write_hwdef_header(outfilename):
dma_priority=get_config('DMA_PRIORITY',default='TIM* SPI*'),
dma_noshare=get_config('DMA_NOSHARE',default=''))
write_PWM_config(f)
write_UART_config(f)
f.write('''

10
libraries/AP_HAL_ChibiOS/shared_dma.cpp
View File

@ -47,12 +47,12 @@ Shared_DMA::Shared_DMA(uint8_t _stream_id1,
void Shared_DMA::unregister()
{
if (locks[stream_id1].obj == this) {
locks[stream_id1].deallocate();
locks[stream_id1].deallocate(this);
locks[stream_id1].obj = nullptr;
}
if (locks[stream_id2].obj == this) {
locks[stream_id2].deallocate();
locks[stream_id2].deallocate(this);
locks[stream_id2].obj = nullptr;
}
}
@ -64,18 +64,18 @@ void Shared_DMA::lock_core(void)
// deallocation function
if (stream_id1 != SHARED_DMA_NONE &&
locks[stream_id1].obj && locks[stream_id1].obj != this) {
locks[stream_id1].deallocate();
locks[stream_id1].deallocate(locks[stream_id1].obj);
locks[stream_id1].obj = nullptr;
}
if (stream_id2 != SHARED_DMA_NONE &&
locks[stream_id2].obj && locks[stream_id2].obj != this) {
locks[stream_id2].deallocate();
locks[stream_id2].deallocate(locks[stream_id2].obj);
locks[stream_id2].obj = nullptr;
}
if ((stream_id1 != SHARED_DMA_NONE && locks[stream_id1].obj == nullptr) ||
(stream_id2 != SHARED_DMA_NONE && locks[stream_id2].obj == nullptr)) {
// allocate the DMA channels and put our deallocation function in place
allocate();
allocate(this);
if (stream_id1 != SHARED_DMA_NONE) {
locks[stream_id1].deallocate = deallocate;
locks[stream_id1].obj = this;

4
libraries/AP_HAL_ChibiOS/shared_dma.h
View File

@ -26,8 +26,8 @@
class ChibiOS::Shared_DMA
{
public:
FUNCTOR_TYPEDEF(dma_allocate_fn_t, void);
FUNCTOR_TYPEDEF(dma_deallocate_fn_t, void);
FUNCTOR_TYPEDEF(dma_allocate_fn_t, void, Shared_DMA *);
FUNCTOR_TYPEDEF(dma_deallocate_fn_t, void, Shared_DMA *);
// the use of two stream IDs is for support of peripherals that
// need both a RX and TX DMA channel