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

AP_HAL_ChibiOS: add support for DShot on IOMCU

Browse Source 
set timer counter size to be a byte wide
use HAL_DSHOT_ENABLED instead of DISABLE_DSHOT
build iomcu-dshot from existing iomcu
correct defines for DMAR size on iomcu
allow iomcu dshot rate to be configured from FMU
correct DMA allocation for dshot on iomcu
allow debug builds on iofirmware
ensure dshot is enabled on iomcu dshot
support proper iomcu dshot output thread triggered by FMU
allow selective disablement of serial LEDs and passthrough
disable serial LEDs and passthrough on iomcu-dshot
propagate ESC telemetry to iomcu
dshot_send_groups() for iomcu
remove use of ICU on iomcu for dshot. only allocate possible DMA channels
rename serial passthrough and dshot defines
update dshot docs
resize dshot iomcu main stack to minimum
correct dshot prescaler usage and bit_width_mul calculation
use ChibiOS in tickless mode on iomcu-dshot so that virtual timers can be used
propagate dshot commands to iomcu
passthrough oneshot125 to iomcu
This commit is contained in:
bugobliterator 2022-08-25 11:06:10 +05:30 committed by Andrew Tridgell
parent f233a65580
commit 9a21297cd1
8 changed files with 371 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

343
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -14,6 +14,10 @@
*
* Code by Andrew Tridgell and Siddharth Bharat Purohit
* Bi-directional dshot based on Betaflight, code by Andy Piper and Siddharth Bharat Purohit
*
* There really is no dshot reference. For information try these resources:
* https://blck.mn/2016/11/dshot-the-new-kid-on-the-block/
* https://www.swallenhardware.io/battlebots/2019/4/20/a-developers-guide-to-dshot-escs
*/
#include <hal.h>
@ -23,6 +27,7 @@
#include <AP_HAL/utility/RingBuffer.h>
#include "GPIO.h"
#include "Util.h"
#include "Scheduler.h"
#include "hwdef/common/stm32_util.h"
#include "hwdef/common/watchdog.h"
#include <AP_InternalError/AP_InternalError.h>
@ -61,7 +66,9 @@ extern AP_IOMCU iomcu;
#define TELEM_IC_SAMPLE 16
struct RCOutput::pwm_group RCOutput::pwm_group_list[] = { HAL_PWM_GROUPS };
#if HAL_SERIAL_ESC_COMM_ENABLED
struct RCOutput::irq_state RCOutput::irq;
#endif
const uint8_t RCOutput::NUM_GROUPS = ARRAY_SIZE(RCOutput::pwm_group_list);
// event mask for triggering a PWM send
@ -97,7 +104,7 @@ void RCOutput::init()
group.dshot_event_mask = EVENT_MASK(i);
for (uint8_t j = 0; j < 4; j++ ) {
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
#if !defined(IOMCU_FW)
uint8_t chan = group.chan[j];
if (SRV_Channels::is_GPIO(chan+chan_offset)) {
group.chan[j] = CHAN_DISABLED;
@ -153,6 +160,7 @@ void RCOutput::init()
_initialised = true;
}
#if HAL_SERIALLED_ENABLED
// start the led thread
bool RCOutput::start_led_thread(void)
{
@ -198,10 +206,12 @@ void RCOutput::led_thread()
led_timer_tick(LED_OUTPUT_PERIOD_US + AP_HAL::micros64());
}
}
#endif // HAL_SERIAL_ENABLED
/*
thread for handling RCOutput send
thread for handling RCOutput send on FMU
*/
#if !defined(IOMCU_FW)
void RCOutput::rcout_thread()
{
uint64_t last_thread_run_us = 0; // last time we did a 1kHz run of rcout
@ -240,7 +250,7 @@ void RCOutput::rcout_thread()
// 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 (!serial_group && have_pwm_event) {
if (!in_soft_serial() && have_pwm_event) {
dshot_send_groups(time_out_us);
// now unlock everything
@ -257,14 +267,15 @@ 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
}
}
#endif
__RAMFUNC__ void RCOutput::dshot_update_tick(virtual_timer_t* vt, void* p)
{
@ -496,6 +507,12 @@ void RCOutput::set_default_rate(uint16_t freq_hz)
*/
void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
{
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
iomcu.set_dshot_period(1000UL, 0);
}
#endif
// for low loop rates simply output at 1Khz on a timer
if (loop_rate_hz <= 100 || dshot_rate == 0) {
_dshot_period_us = 1000UL;
@ -528,9 +545,14 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
drate = _dshot_rate * loop_rate_hz;
}
_dshot_period_us = 1000000UL / drate;
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
iomcu.set_dshot_period(_dshot_period_us, _dshot_rate);
}
#endif
}
#ifndef DISABLE_DSHOT
#if HAL_DSHOT_ENABLED
/*
Set/get the dshot esc_type
*/
@ -551,7 +573,7 @@ void RCOutput::set_dshot_esc_type(DshotEscType dshot_esc_type)
break;
}
}
#endif
#endif // #if HAL_DSHOT_ENABLED
/*
find pwm_group and index in group given a channel number
@ -622,6 +644,12 @@ uint16_t RCOutput::get_freq(uint8_t chan)
void RCOutput::enable_ch(uint8_t chan)
{
#if HAL_WITH_IO_MCU
if (chan < chan_offset && AP_BoardConfig::io_enabled()) {
iomcu.enable_ch(chan);
return;
}
#endif
uint8_t i;
pwm_group *grp = find_chan(chan, i);
if (grp) {
@ -632,6 +660,12 @@ void RCOutput::enable_ch(uint8_t chan)
void RCOutput::disable_ch(uint8_t chan)
{
#if HAL_WITH_IO_MCU
if (chan < chan_offset && AP_BoardConfig::io_enabled()) {
iomcu.disable_ch(chan);
return;
}
#endif
uint8_t i;
pwm_group *grp = find_chan(chan, i);
if (grp) {
@ -659,13 +693,7 @@ void RCOutput::write(uint8_t chan, uint16_t period_us)
#if HAL_WITH_IO_MCU
// handle IO MCU channels
if (AP_BoardConfig::io_enabled()) {
uint16_t io_period_us = period_us;
if ((iomcu_mode == MODE_PWM_ONESHOT125) && ((1U<<chan) & io_fast_channel_mask)) {
// the iomcu only has one oneshot setting, so we need to scale by a factor
// of 8 here for oneshot125
io_period_us /= 8;
}
iomcu.write_channel(chan, io_period_us);
iomcu.write_channel(chan, period_us);
}
#endif
if (chan < chan_offset) {
@ -705,7 +733,7 @@ void RCOutput::push_local(void)
bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED;
for (auto &group : pwm_group_list) {
if (serial_group) {
if (in_soft_serial()) {
continue;
}
if (!group.pwm_started) {
@ -745,12 +773,12 @@ void RCOutput::push_local(void)
uint32_t width = (group.pwm_cfg.frequency/1000000U) * period_us;
pwmEnableChannel(group.pwm_drv, j, width);
}
#ifndef DISABLE_DSHOT
#if HAL_DSHOT_ENABLED
else if (is_dshot_protocol(group.current_mode) || is_led_protocol(group.current_mode)) {
// set period_us to time for pulse output, to enable very fast rates
period_us = group.dshot_pulse_time_us;
}
#endif //#ifndef DISABLE_DSHOT
#endif // HAL_DSHOT_ENABLED
if (group.current_mode == MODE_PWM_ONESHOT ||
group.current_mode == MODE_PWM_ONESHOT125 ||
group.current_mode == MODE_NEOPIXEL ||
@ -786,12 +814,7 @@ uint16_t RCOutput::read(uint8_t chan)
}
#if HAL_WITH_IO_MCU
if (chan < chan_offset) {
uint16_t period_us = iomcu.read_channel(chan);
if ((iomcu_mode == MODE_PWM_ONESHOT125) && ((1U<<chan) & io_fast_channel_mask)) {
// convert back to 1000 - 2000 range
period_us *= 8;
}
return period_us;
return iomcu.read_channel(chan);
}
#endif
chan -= chan_offset;
@ -806,10 +829,6 @@ void RCOutput::read(uint16_t* period_us, uint8_t len)
#if HAL_WITH_IO_MCU
for (uint8_t i=0; i<MIN(len, chan_offset); i++) {
period_us[i] = iomcu.read_channel(i);
if ((iomcu_mode == MODE_PWM_ONESHOT125) && ((1U<<i) & io_fast_channel_mask)) {
// convert back to 1000 - 2000 range
period_us[i] *= 8;
}
}
#endif
if (len <= chan_offset) {
@ -844,11 +863,11 @@ void RCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
*/
bool RCOutput::mode_requires_dma(enum output_mode mode) const
{
#ifdef DISABLE_DSHOT
#if !HAL_DSHOT_ENABLED
return false;
#else
return is_dshot_protocol(mode) || is_led_protocol(mode);
#endif //#ifdef DISABLE_DSHOT
#endif //#if !HAL_DSHOT_ENABLED
}
void RCOutput::print_group_setup_error(pwm_group &group, const char* error_string)
@ -883,8 +902,20 @@ void RCOutput::print_group_setup_error(pwm_group &group, const char* error_strin
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)
{
#ifndef DISABLE_DSHOT
#if HAL_DSHOT_ENABLED
// for dshot we setup for DMAR based output
#if !AP_HAL_SHARED_DMA_ENABLED
if (group.dma == nullptr) {
// we don't do dma sharing in non advanced mode
chSysLock();
group.dma = dmaStreamAllocI(group.dma_up_stream_id, 10, dma_up_irq_callback, &group);
chSysUnlock();
if (group.dma == nullptr) {
print_group_setup_error(group, "failed to allocate DMA");
return false;
}
}
#else
if (!group.dma_handle) {
group.dma_handle = new Shared_DMA(group.dma_up_stream_id, SHARED_DMA_NONE,
FUNCTOR_BIND_MEMBER(&RCOutput::dma_allocate, void, Shared_DMA *),
@ -894,17 +925,22 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
return false;
}
}
#endif
// hold the lock during setup, to ensure there isn't a DMA operation ongoing
#if AP_HAL_SHARED_DMA_ENABLED
group.dma_handle->lock();
#endif
if (!group.dma_buffer || buffer_length != group.dma_buffer_len) {
if (group.dma_buffer) {
hal.util->free_type(group.dma_buffer, group.dma_buffer_len, AP_HAL::Util::MEM_DMA_SAFE);
group.dma_buffer_len = 0;
}
group.dma_buffer = (uint32_t *)hal.util->malloc_type(buffer_length, AP_HAL::Util::MEM_DMA_SAFE);
group.dma_buffer = (dmar_uint_t *)hal.util->malloc_type(buffer_length, AP_HAL::Util::MEM_DMA_SAFE);
if (!group.dma_buffer) {
#if AP_HAL_SHARED_DMA_ENABLED
group.dma_handle->unlock();
#endif
print_group_setup_error(group, "failed to allocate DMA buffer");
return false;
}
@ -927,25 +963,30 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
pwmStop(group.pwm_drv);
group.pwm_started = false;
}
const uint32_t target_frequency = bitrate * bit_width;
const uint32_t prescaler = calculate_bitrate_prescaler(group.pwm_drv->clock, target_frequency, is_dshot);
if (prescaler > 0x8000) {
if (prescaler == 0 || prescaler > 0x8000) {
#if AP_HAL_SHARED_DMA_ENABLED
group.dma_handle->unlock();
#endif
print_group_setup_error(group, "failed to match clock speed");
return false;
}
const uint32_t freq = group.pwm_drv->clock / prescaler;
const uint32_t freq = group.pwm_drv->clock / (prescaler + 1);
// PSC is calculated by ChibiOS as (pwm_drv.clock / pwm_cfg.frequency) - 1;
group.pwm_cfg.frequency = freq;
group.pwm_cfg.period = bit_width;
group.pwm_cfg.dier = TIM_DIER_UDE;
group.pwm_cfg.cr2 = 0;
group.bit_width_mul = (freq + (target_frequency/2)) / target_frequency;
// ARR is calculated by ChibiOS as pwm_cfg.period -1
group.pwm_cfg.period = bit_width * group.bit_width_mul;
//hal.console->printf("CLOCK=%u BW=%u FREQ=%u BR=%u MUL=%u PRE=%u\n", unsigned(group.pwm_drv->clock), unsigned(bit_width), unsigned(group.pwm_cfg.frequency),
// unsigned(bitrate), unsigned(group.bit_width_mul), unsigned(prescaler));
//static char clock_setup[64];
//hal.util->snprintf(clock_setup, 64, "CLOCK=%u BW=%u FREQ=%u BR=%u MUL=%u PRE=%u\n", unsigned(group.pwm_drv->clock), unsigned(bit_width), unsigned(group.pwm_cfg.frequency),
// unsigned(bitrate), unsigned(group.bit_width_mul), unsigned(prescaler));
for (uint8_t j=0; j<4; j++) {
pwmmode_t mode = group.pwm_cfg.channels[j].mode;
@ -967,11 +1008,13 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
}
}
#if AP_HAL_SHARED_DMA_ENABLED
group.dma_handle->unlock();
#endif
return true;
#else
return false;
#endif //#ifndef DISABLE_DSHOT
#endif // HAL_DSHOT_ENABLED
}
/*
@ -984,9 +1027,9 @@ void RCOutput::set_group_mode(pwm_group &group)
pwmStop(group.pwm_drv);
group.pwm_started = false;
}
#ifdef HAL_WITH_BIDIR_DSHOT
memset(group.bdshot.erpm, 0, 4*sizeof(uint16_t));
#endif
switch (group.current_mode) {
case MODE_PWM_BRUSHED:
// force zero output initially
@ -1001,6 +1044,7 @@ void RCOutput::set_group_mode(pwm_group &group)
case MODE_NEOPIXEL:
case MODE_PROFILED:
#if HAL_SERIALLED_ENABLED
{
uint8_t bits_per_pixel = 24;
bool active_high = true;
@ -1032,6 +1076,7 @@ void RCOutput::set_group_mode(pwm_group &group)
}
break;
}
#endif
case MODE_PWM_DSHOT150 ... MODE_PWM_DSHOT1200: {
const uint32_t rate = protocol_bitrate(group.current_mode);
@ -1110,26 +1155,33 @@ void RCOutput::set_output_mode(uint32_t mask, const enum output_mode mode)
}
#if HAL_WITH_IO_MCU
if ((mode == MODE_PWM_ONESHOT ||
mode == MODE_PWM_ONESHOT125) &&
mode == MODE_PWM_ONESHOT125 ||
mode == MODE_PWM_BRUSHED ||
(mode >= MODE_PWM_DSHOT150 && mode <= MODE_PWM_DSHOT600)) &&
(mask & ((1U<<chan_offset)-1)) &&
AP_BoardConfig::io_enabled()) {
iomcu_mode = mode;
// also setup IO to use a 1Hz frequency, so we only get output
// when we trigger
iomcu.set_freq(io_fast_channel_mask, 1);
iomcu.set_oneshot_mode();
return;
}
if (mode == MODE_PWM_BRUSHED &&
(mask & ((1U<<chan_offset)-1)) &&
AP_BoardConfig::io_enabled()) {
iomcu_mode = mode;
iomcu.set_brushed_mode();
iomcu.set_output_mode(mask, mode);
return;
}
#endif
}
/*
enable telemetry request for a mask of channels. This is used
with DShot to get telemetry feedback
The mask uses servo channel numbering
*/
void RCOutput::set_telem_request_mask(uint32_t mask)
{
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot() && (mask & ((1U<<chan_offset)-1))) {
iomcu.set_telem_request_mask(mask);
}
#endif
telem_request_mask = (mask >> chan_offset);
}
/*
* get output mode banner to inform user of how outputs are configured
*/
@ -1252,7 +1304,7 @@ void RCOutput::trigger_groups(void)
osalSysLock();
for (auto &group : pwm_group_list) {
if (irq.waiter) {
if (soft_serial_waiting()) {
// doing serial output, don't send pulses
continue;
}
@ -1266,9 +1318,9 @@ void RCOutput::trigger_groups(void)
}
}
osalSysUnlock();
#ifndef HAL_NO_RCOUT_THREAD
#if !defined(HAL_NO_RCOUT_THREAD) || HAL_DSHOT_ENABLED
// trigger a PWM send
if (!serial_group && hal.scheduler->in_main_thread()) {
if (!in_soft_serial() && hal.scheduler->in_main_thread() && rcout_thread_ctx) {
chEvtSignal(rcout_thread_ctx, EVT_PWM_SEND);
}
#endif
@ -1287,7 +1339,7 @@ void RCOutput::trigger_groups(void)
*/
void RCOutput::timer_tick(uint64_t time_out_us)
{
if (serial_group) {
if (in_soft_serial()) {
return;
}
@ -1309,7 +1361,7 @@ void RCOutput::timer_tick(uint64_t time_out_us)
*/
void RCOutput::led_timer_tick(uint64_t time_out_us)
{
if (serial_group) {
if (in_soft_serial()) {
return;
}
@ -1325,11 +1377,73 @@ void RCOutput::led_timer_tick(uint64_t time_out_us)
}
}
#if defined(IOMCU_FW) && HAL_DSHOT_ENABLED
THD_WORKING_AREA(dshot_thread_wa, 64);
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) {
chThdCreateStatic(dshot_thread_wa, sizeof(dshot_thread_wa),
APM_RCOUT_PRIORITY, &RCOutput::dshot_send_trampoline, this);
dshot_timer_setup = true;
}
}
void RCOutput::dshot_send_trampoline(void *p)
{
RCOutput *rcout = (RCOutput *)p;
rcout->rcout_thread();
}
/*
thread for handling RCOutput send on IOMCU
*/
void RCOutput::rcout_thread() {
// don't start outputting until fully configured
while (!hal.scheduler->is_system_initialized()) {
hal.scheduler->delay_microseconds(1000);
}
rcout_thread_ctx = chThdGetSelfX();
while (true) {
chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND);
// this is when the cycle is supposed to start
if (_dshot_cycle == 0) {
// 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 (_dshot_rate > 0) {
_dshot_cycle = (_dshot_cycle + 1) % _dshot_rate;
}
}
}
#endif // IOMCU_FW && DISABLE_DSHOT
// send dshot for all groups that support it
void RCOutput::dshot_send_groups(uint64_t time_out_us)
{
#ifndef DISABLE_DSHOT
if (serial_group) {
#if HAL_DSHOT_ENABLED
if (in_soft_serial()) {
return;
}
@ -1354,7 +1468,7 @@ void RCOutput::dshot_send_groups(uint64_t time_out_us)
if (command_sent) {
_dshot_current_command.cycle--;
}
#endif //#ifndef DISABLE_DSHOT
#endif // HAL_DSHOT_ENABLED
}
__RAMFUNC__ void RCOutput::dshot_send_next_group(void* p)
@ -1369,6 +1483,7 @@ __RAMFUNC__ void RCOutput::dshot_send_next_group(void* p)
/*
allocate DMA channel
*/
#if AP_HAL_SHARED_DMA_ENABLED
void RCOutput::dma_allocate(Shared_DMA *ctx)
{
for (auto &group : pwm_group_list) {
@ -1399,6 +1514,7 @@ void RCOutput::dma_deallocate(Shared_DMA *ctx)
}
}
}
#endif // AP_HAL_SHARED_DMA_ENABLED
/*
create a DSHOT 16 bit packet. Based on prepareDshotPacket from betaflight
@ -1432,11 +1548,15 @@ uint16_t RCOutput::create_dshot_packet(const uint16_t value, bool telem_request,
/*
fill in a DMA buffer for dshot
the buffer format is (stride is always 4 for 4 channels):
[ch0b0][ch1b0][ch2b0][ch3b0][ch0b1][ch1b1][ch2b1][ch3b1]...[ch0bN][ch1bN][ch2bN][ch3bN]
where N = dshot_bit_length - 1
each value is a number of beats for the DMA engine to send in burst via DMAR to the 4 CCR registers
*/
void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul)
void RCOutput::fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul)
{
const uint32_t DSHOT_MOTOR_BIT_0 = DSHOT_BIT_0_TICKS * clockmul;
const uint32_t DSHOT_MOTOR_BIT_1 = DSHOT_BIT_1_TICKS * clockmul;
const dmar_uint_t DSHOT_MOTOR_BIT_0 = DSHOT_BIT_0_TICKS * clockmul;
const dmar_uint_t DSHOT_MOTOR_BIT_1 = DSHOT_BIT_1_TICKS * clockmul;
uint16_t i = 0;
for (; i < dshot_pre; i++) {
buffer[i * stride] = 0;
@ -1457,23 +1577,25 @@ void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t
*/
void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)
{
#ifndef DISABLE_DSHOT
if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
#if HAL_DSHOT_ENABLED
if (soft_serial_waiting() || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
// doing serial output or DMAR input, don't send DShot pulses
return;
}
#if AP_HAL_SHARED_DMA_ENABLED
// first make sure we have the DMA channel before anything else
osalDbgAssert(!group.dma_handle->is_locked(), "DMA handle is already locked");
group.dma_handle->lock();
#endif
// 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) {
group.dma_handle->unlock();
return;
}
#endif
// only the timer thread releases the locks
group.dshot_waiter = rcout_thread_ctx;
@ -1524,8 +1646,9 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)
}
#endif
bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED;
#if !defined(IOMCU_FW)
bool armed = hal.util->get_soft_armed();
#endif
memset((uint8_t *)group.dma_buffer, 0, DSHOT_BUFFER_LENGTH);
for (uint8_t i=0; i<4; i++) {
@ -1569,15 +1692,21 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)
if (value != 0) {
value += DSHOT_ZERO_THROTTLE;
}
#if !defined(IOMCU_FW)
if (!armed) {
// when disarmed we always send a zero value
value = 0;
}
#endif
// according to sskaug requesting telemetry while trying to arm may interfere with the good frame calc
bool request_telemetry = telem_request_mask & chan_mask;
uint16_t packet = create_dshot_packet(value, request_telemetry, group.bdshot.enabled);
uint16_t packet = create_dshot_packet(value, request_telemetry,
#ifdef HAL_WITH_BIDIR_DSHOT
group.bdshot.enabled
#else
false
#endif
);
if (request_telemetry) {
telem_request_mask &= ~chan_mask;
}
@ -1588,7 +1717,7 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)
chEvtGetAndClearEvents(group.dshot_event_mask);
// start sending the pulses out
send_pulses_DMAR(group, DSHOT_BUFFER_LENGTH);
#endif //#ifndef DISABLE_DSHOT
#endif // HAL_DSHOT_ENABLED
}
/*
@ -1596,14 +1725,15 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)
return true if send was successful
called from led thread
*/
#if HAL_SERIALLED_ENABLED
bool RCOutput::serial_led_send(pwm_group &group)
{
if (!group.serial_led_pending || !is_led_protocol(group.current_mode)) {
return true;
}
#ifndef DISABLE_DSHOT
if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
#if HAL_DSHOT_ENABLED
if (soft_serial_waiting() || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
// doing serial output or DMAR input, don't send DShot pulses
return false;
}
@ -1627,10 +1757,10 @@ bool RCOutput::serial_led_send(pwm_group &group)
// start sending the pulses out
send_pulses_DMAR(group, group.dma_buffer_len);
#endif //#ifndef DISABLE_DSHOT
#endif // HAL_DSHOT_ENABLED
return true;
}
#endif // HAL_SERIALLED_ENABLED
/*
send a series of pulses for a group using DMAR. Pulses must have
@ -1639,7 +1769,7 @@ bool RCOutput::serial_led_send(pwm_group &group)
*/
void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length)
{
#ifndef DISABLE_DSHOT
#if HAL_DSHOT_ENABLED
osalDbgAssert(group.dma && group.dma_buffer, "DMA structures are corrupt");
/*
The DMA approach we are using is based on the DMAR method from
@ -1660,18 +1790,24 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length)
dmaStreamSetPeripheral(group.dma, &(group.pwm_drv->tim->DMAR));
stm32_cacheBufferFlush(group.dma_buffer, buffer_length);
dmaStreamSetMemory0(group.dma, group.dma_buffer);
dmaStreamSetTransactionSize(group.dma, buffer_length/sizeof(uint32_t));
dmaStreamSetTransactionSize(group.dma, buffer_length / sizeof(dmar_uint_t));
#if STM32_DMA_ADVANCED
dmaStreamSetFIFO(group.dma, STM32_DMA_FCR_DMDIS | STM32_DMA_FCR_FTH_FULL);
#endif
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_DIR_M2P | STM32_DMA_CR_PSIZE_WORD |
#if defined(IOMCU_FW)
STM32_DMA_CR_MSIZE_BYTE |
#else
STM32_DMA_CR_MSIZE_WORD |
#endif
STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3) |
STM32_DMA_CR_TEIE | STM32_DMA_CR_TCIE);
// setup for burst strided transfers into the timers 4 CCR registers
const uint8_t ccr_ofs = offsetof(stm32_tim_t, CCR)/4;
// burst address (BA) of the CCR register, burst length (BL) of 4 (0b11)
group.pwm_drv->tim->DCR = STM32_TIM_DCR_DBA(ccr_ofs) | STM32_TIM_DCR_DBL(3);
group.dshot_state = DshotState::SEND_START;
@ -1680,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();
#endif //#ifndef DISABLE_DSHOT
#endif // HAL_DSHOT_ENABLED
}
/*
@ -1709,10 +1845,13 @@ __RAMFUNC__ void RCOutput::dma_up_irq_callback(void *p, uint32_t flags)
pwm_group *group = (pwm_group *)p;
chSysLockFromISR();
dmaStreamDisable(group->dma);
if (group->in_serial_dma && irq.waiter) {
#if HAL_SERIAL_ESC_COMM_ENABLED
if (group->in_serial_dma && soft_serial_waiting()) {
// tell the waiting process we've done the DMA
chEvtSignalI(irq.waiter, serial_event_mask);
} else {
} else
#endif
{
// this prevents us ever having two dshot pulses too close together
// dshot mandates a minimum pulse separation of 40us, WS2812 mandates 50us so we
// pick the higher value
@ -1763,6 +1902,7 @@ void RCOutput::dma_cancel(pwm_group& group)
While serial output is active normal output to the channel group is
suspended.
*/
#if HAL_SERIAL_ESC_COMM_ENABLED
bool RCOutput::serial_setup_output(uint8_t chan, uint32_t baudrate, uint32_t chanmask)
{
// account for IOMCU channels
@ -1788,7 +1928,7 @@ bool RCOutput::serial_setup_output(uint8_t chan, uint32_t baudrate, uint32_t cha
}
if (!new_serial_group) {
if (serial_group) {
if (in_soft_serial()) {
// shutdown old group
serial_end();
}
@ -1831,10 +1971,10 @@ bool RCOutput::serial_setup_output(uint8_t chan, uint32_t baudrate, uint32_t cha
/*
fill in a DMA buffer for a serial byte, assuming 1 start bit and 1 stop bit
*/
void RCOutput::fill_DMA_buffer_byte(uint32_t *buffer, uint8_t stride, uint8_t b, uint32_t bitval)
void RCOutput::fill_DMA_buffer_byte(dmar_uint_t *buffer, uint8_t stride, uint8_t b, uint32_t bitval)
{
const uint32_t BIT_0 = bitval;
const uint32_t BIT_1 = 0;
const dmar_uint_t BIT_0 = bitval;
const dmar_uint_t BIT_1 = 0;
// start bit
buffer[0] = BIT_0;
@ -1877,8 +2017,8 @@ bool RCOutput::serial_write_byte(uint8_t b)
*/
bool RCOutput::serial_write_bytes(const uint8_t *bytes, uint16_t len)
{
#ifndef DISABLE_DSHOT
if (!serial_group) {
#if HAL_DSHOT_ENABLED
if (!in_soft_serial()) {
return false;
}
serial_group->dma_handle->lock();
@ -2001,8 +2141,7 @@ bool RCOutput::serial_read_byte(uint8_t &b)
*/
uint16_t RCOutput::serial_read_bytes(uint8_t *buf, uint16_t len)
{
#ifndef DISABLE_SERIAL_ESC_COMM
if (serial_group == nullptr) {
if (!in_soft_serial()) {
return 0;
}
pwm_group &group = *serial_group;
@ -2062,10 +2201,6 @@ uint16_t RCOutput::serial_read_bytes(uint8_t *buf, uint16_t len)
palWriteLine(HAL_GPIO_LINE_GPIO54, 0);
#endif
return i;
#else
return false;
#endif //#ifndef DISABLE_SERIAL_ESC_COMM
}
/*
@ -2073,8 +2208,7 @@ uint16_t RCOutput::serial_read_bytes(uint8_t *buf, uint16_t len)
*/
void RCOutput::serial_end(void)
{
#ifndef DISABLE_SERIAL_ESC_COMM
if (serial_group) {
if (in_soft_serial()) {
if (serial_thread == chThdGetSelfX()) {
chThdSetPriority(serial_priority);
serial_thread = nullptr;
@ -2087,8 +2221,8 @@ void RCOutput::serial_end(void)
}
}
serial_group = nullptr;
#endif //#ifndef DISABLE_SERIAL_ESC_COMM
}
#endif // HAL_SERIAL_ESC_COMM_ENABLED
/*
get safety switch state for Util.cpp
@ -2228,6 +2362,7 @@ uint32_t RCOutput::protocol_bitrate(const enum output_mode mode)
setup serial led output for a given channel number, with
the given max number of LEDs in the chain.
*/
#if HAL_SERIALLED_ENABLED
bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, output_mode mode, uint32_t clock_mask)
{
if (!_initialised || num_leds == 0) {
@ -2280,8 +2415,6 @@ bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, ou
}
#pragma GCC push_options
#pragma GCC optimize("O2")
// Fill the group DMA buffer with data to be output
@ -2332,7 +2465,7 @@ void RCOutput::_set_neopixel_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led,
const uint8_t pad_start_bits = 1;
const uint8_t neopixel_bit_length = 24;
const uint8_t stride = 4;
uint32_t *buf = grp->dma_buffer + (led * neopixel_bit_length + pad_start_bits) * stride + idx;
dmar_uint_t *buf = grp->dma_buffer + (led * neopixel_bit_length + pad_start_bits) * stride + idx;
uint32_t bits = (green<<16) | (red<<8) | blue;
const uint32_t BIT_0 = NEOP_BIT_0_TICKS * grp->bit_width_mul;
const uint32_t BIT_1 = NEOP_BIT_1_TICKS * grp->bit_width_mul;
@ -2351,7 +2484,7 @@ void RCOutput::_set_profiled_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led,
const uint8_t pad_start_bits = 1;
const uint8_t bit_length = 25;
const uint8_t stride = 4;
uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
dmar_uint_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
uint32_t bits = 0x1000000 | (blue<<16) | (red<<8) | green;
const uint32_t BIT_1 = PROFI_BIT_1_TICKS * grp->bit_width_mul;
for (uint16_t b=0; b < bit_length; b++) {
@ -2369,7 +2502,7 @@ void RCOutput::_set_profiled_blank_frame(pwm_group *grp, uint8_t idx, uint8_t le
const uint8_t pad_start_bits = 1;
const uint8_t bit_length = 25;
const uint8_t stride = 4;
uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
dmar_uint_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
const uint32_t BIT_1 = PROFI_BIT_1_TICKS * grp->bit_width_mul;
for (uint16_t b=0; b < bit_length; b++) {
buf[b * stride] = BIT_1;
@ -2384,7 +2517,7 @@ void RCOutput::_set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led)
const uint8_t pad_start_bits = 1;
const uint8_t bit_length = 25;
const uint8_t stride = 4;
uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
dmar_uint_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
const uint32_t BIT_1 = PROFI_BIT_0_TICKS * grp->bit_width_mul;
for (uint16_t b=0; b < bit_length; b++) {
buf[b * stride] = BIT_1;
@ -2520,6 +2653,7 @@ void RCOutput::serial_led_send(const uint16_t chan)
serial_led_pending = true;
}
}
#endif // HAL_SERIALLED_ENABLED
void RCOutput::timer_info(ExpandingString &str)
{
@ -2528,9 +2662,12 @@ void RCOutput::timer_info(ExpandingString &str)
for (auto &group : pwm_group_list) {
uint32_t target_freq;
#if HAL_SERIAL_ESC_COMM_ENABLED
if (&group == serial_group) {
target_freq = 19200 * 10;
} else if (is_dshot_protocol(group.current_mode)) {
} else
#endif // HAL_SERIAL_ESC_COMM_ENABLED
if (is_dshot_protocol(group.current_mode)) {
target_freq = protocol_bitrate(group.current_mode) * DSHOT_BIT_WIDTH_TICKS;
} else {
target_freq = protocol_bitrate(group.current_mode) * NEOP_BIT_WIDTH_TICKS;

84
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -16,18 +16,19 @@
*/
#pragma once
#include <hal.h>
#include "AP_HAL_ChibiOS.h"
#include <AP_HAL/Semaphores.h>
#include <AP_ESC_Telem/AP_ESC_Telem.h>
#include "shared_dma.h"
#include "ch.h"
#include "hal.h"
#if HAL_USE_PWM == TRUE
#if !STM32_DMA_ADVANCED && !defined(STM32G4) && !defined(STM32L4)
#define DISABLE_DSHOT
#if defined(IOMCU_FW)
typedef uint8_t dmar_uint_t; // save memory to allow dshot on IOMCU
#else
typedef uint32_t dmar_uint_t;
#endif
#define RCOU_DSHOT_TIMING_DEBUG 0
@ -109,12 +110,17 @@ public:
*/
void led_timer_tick(uint64_t last_run_us);
#if defined(IOMCU_FW) && HAL_DSHOT_ENABLED
void timer_tick() override;
static void dshot_send_trampoline(void *p);
#endif
/*
setup for serial output to a set of ESCs, using the given
baudrate. Assumes 1 start bit, 1 stop bit, LSB first and 8
databits. This is used for ESC configuration and firmware
flashing
*/
#if HAL_SERIAL_ESC_COMM_ENABLED
bool setup_serial_output(uint32_t chan_mask, ByteBuffer *buffer, uint32_t baudrate);
/*
@ -148,13 +154,14 @@ public:
serial_setup_output()
*/
void serial_end(void) override;
#endif
/*
enable telemetry request for a mask of channels. This is used
with Dshot to get telemetry feedback
The mask uses servo channel numbering
*/
void set_telem_request_mask(uint32_t mask) override { telem_request_mask = (mask >> chan_offset); }
void set_telem_request_mask(uint32_t mask) override;
#ifdef HAL_WITH_BIDIR_DSHOT
/*
@ -172,7 +179,18 @@ public:
*/
void set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) override;
#ifndef DISABLE_DSHOT
#if defined(IOMCU_FW)
/*
Get/Set the dshot period in us, only for use by the IOMCU
*/
void set_dshot_period(uint32_t period_us, uint8_t dshot_rate) override {
_dshot_period_us = period_us;
_dshot_rate = dshot_rate;
}
uint32_t get_dshot_period_us() const override { return _dshot_period_us; }
#endif
#if HAL_DSHOT_ENABLED
/*
Set/get the dshot esc_type
*/
@ -196,7 +214,7 @@ public:
*/
void set_safety_mask(uint32_t mask) { safety_mask = mask; }
#ifndef DISABLE_DSHOT
#if HAL_DSHOT_ENABLED
/*
* mark the channels in chanmask as reversible. This is needed for some ESC types (such as Dshot)
* so that output scaling can be performed correctly. The chanmask passed is added (ORed) into any existing mask.
@ -240,6 +258,7 @@ public:
setup serial LED output for a given channel number, with
the given max number of LEDs in the chain.
*/
#if HAL_SERIALLED_ENABLED
bool set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, output_mode mode = MODE_PWM_NONE, uint32_t clock_mask = 0) override;
/*
@ -252,7 +271,7 @@ public:
trigger send of serial LED data
*/
void serial_led_send(const uint16_t chan) override;
#endif
/*
rcout thread
*/
@ -285,7 +304,7 @@ private:
static const uint8_t dshot_pre = 1;
static const uint8_t dshot_post = 2;
static const uint16_t dshot_bit_length = 16 + dshot_pre + dshot_post;
static const uint16_t DSHOT_BUFFER_LENGTH = dshot_bit_length * 4 * sizeof(uint32_t);
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 GCR_TELEMETRY_BIT_LEN = MAX_GCR_BIT_LEN;
@ -318,8 +337,10 @@ private:
// mask of channels that are enabled and active
uint32_t en_mask;
const stm32_dma_stream_t *dma;
#if AP_HAL_SHARED_DMA_ENABLED
Shared_DMA *dma_handle;
uint32_t *dma_buffer;
#endif
dmar_uint_t *dma_buffer;
uint16_t dma_buffer_len;
bool pwm_started;
uint32_t bit_width_mul;
@ -329,7 +350,7 @@ private:
uint64_t dshot_pulse_time_us;
uint64_t dshot_pulse_send_time_us;
virtual_timer_t dma_timeout;
#if HAL_SERIALLED_ENABLED
// serial LED support
volatile uint8_t serial_nleds;
uint8_t clock_mask;
@ -340,10 +361,11 @@ private:
// structure to hold serial LED data until it can be transferred
// to the DMA buffer
SerialLed* serial_led_data[4];
#endif
eventmask_t dshot_event_mask;
thread_t* dshot_waiter;
#if HAL_SERIAL_ESC_COMM_ENABLED
// serial output
struct {
// expected time per bit
@ -355,10 +377,11 @@ private:
// thread waiting for byte to be written
thread_t *waiter;
} serial;
#endif
// support for bi-directional dshot
volatile DshotState dshot_state;
#ifdef HAL_WITH_BIDIR_DSHOT
struct {
uint16_t erpm[4];
volatile bool enabled;
@ -378,7 +401,7 @@ private:
#endif
#endif
} bdshot;
#endif
#ifdef HAL_WITH_BIDIR_DSHOT
// do we have an input capture dma channel
bool has_ic_dma() const {
@ -418,6 +441,7 @@ private:
*/
thread_t *rcout_thread_ctx;
#if HAL_SERIALLED_ENABLED
/*
timer thread for use by led events
*/
@ -428,7 +452,9 @@ private:
*/
HAL_Semaphore led_thread_sem;
bool led_thread_created;
#endif
#if HAL_SERIAL_ESC_COMM_ENABLED
/*
structure for IRQ handler for soft-serial input
*/
@ -462,11 +488,27 @@ private:
bool timed_out;
} irq;
// the group being used for serial output
struct pwm_group *serial_group;
thread_t *serial_thread;
tprio_t serial_priority;
#endif
static bool soft_serial_waiting() {
#if HAL_SERIAL_ESC_COMM_ENABLED
return irq.waiter != nullptr;
#else
return false;
#endif
}
bool in_soft_serial() const {
#if HAL_SERIAL_ESC_COMM_ENABLED
return serial_group != nullptr;
#else
return false;
#endif
}
static pwm_group pwm_group_list[];
static const uint8_t NUM_GROUPS;
@ -516,7 +558,7 @@ private:
// virtual timer for post-push() pulses
virtual_timer_t _dshot_rate_timer;
#ifndef DISABLE_DSHOT
#if HAL_DSHOT_ENABLED
// dshot commands
// RingBuffer to store outgoing request.
struct DshotCommandPacket {
@ -561,6 +603,8 @@ private:
// widest pulse for oneshot triggering
uint16_t trigger_widest_pulse;
bool dshot_timer_setup;
// iomcu output mode (pwm, oneshot or oneshot125)
enum output_mode iomcu_mode = MODE_PWM_NORMAL;
@ -617,7 +661,7 @@ private:
void dma_allocate(Shared_DMA *ctx);
void dma_deallocate(Shared_DMA *ctx);
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 fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);
void dshot_send_groups(uint64_t time_out_us);
void dshot_send(pwm_group &group, uint64_t time_out_us);
@ -665,14 +709,14 @@ private:
void _set_profiled_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue);
void _set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led);
void _set_profiled_blank_frame(pwm_group *grp, uint8_t idx, uint8_t led);
#if AP_HAL_SHARED_DMA_ENABLED
// serial output support
bool serial_write_byte(uint8_t b);
bool serial_read_byte(uint8_t &b);
void fill_DMA_buffer_byte(uint32_t *buffer, uint8_t stride, uint8_t b , uint32_t bitval);
void fill_DMA_buffer_byte(dmar_uint_t *buffer, uint8_t stride, uint8_t b , uint32_t bitval);
static void serial_bit_irq(void);
static void serial_byte_timeout(virtual_timer_t* vt, void *ctx);
#endif
};
#if RCOU_DSHOT_TIMING_DEBUG

20
libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
View File

@ -16,12 +16,18 @@
#include <hal.h>
#include "RCOutput.h"
#include <AP_Math/AP_Math.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include "hwdef/common/stm32_util.h"
#include <AP_InternalError/AP_InternalError.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#if HAL_USE_PWM == TRUE
#ifndef DISABLE_DSHOT
#if HAL_DSHOT_ENABLED
#if HAL_WITH_IO_MCU
#include <AP_IOMCU/AP_IOMCU.h>
extern AP_IOMCU iomcu;
#endif
using namespace ChibiOS;
@ -33,7 +39,7 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
return false;
}
if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
if (soft_serial_waiting() || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
// doing serial output or DMAR input, don't send DShot pulses
return false;
}
@ -42,9 +48,10 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
TOGGLE_PIN_DEBUG(81);
#endif
// first make sure we have the DMA channel before anything else
#if AP_HAL_SHARED_DMA_ENABLED
osalDbgAssert(!group.dma_handle->is_locked(), "DMA handle is already locked");
group.dma_handle->lock();
#endif
// only the timer thread releases the locks
group.dshot_waiter = rcout_thread_ctx;
@ -102,6 +109,11 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman
}
// not an FMU channel
if (chan < chan_offset) {
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
iomcu.send_dshot_command(command, chan, command_timeout_ms, repeat_count, priority);
}
#endif
return;
}
@ -167,5 +179,5 @@ void RCOutput::update_channel_masks() {
#endif
}
#endif // DISABLE_DSHOT
#endif // HAL_DSHOT_ENABLED
#endif // HAL_USE_PWM

2
libraries/AP_HAL_ChibiOS/Util.cpp
View File

@ -137,7 +137,7 @@ void *Util::heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size
void *new_mem = chHeapAlloc((memory_heap_t *)heap, new_size);
if (new_mem != nullptr) {
const size_t old_size2 = chHeapGetSize(ptr);
#ifdef HAL_DEBUG_BUILD
#if defined(HAL_DEBUG_BUILD) && !defined(IOMCU_FW)
if (new_size != 0 && old_size2 != old_size) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
}

1
libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc
View File

@ -322,6 +322,7 @@ define HAL_GPIO_PWM_VOLT_3v3 1
# is "ROMFS ROMFS-filename source-filename". Paths are relative to the
# ardupilot root.
ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_dshot_highpolh.bin
DMA_NOSHARE SPI1* SPI4* USART6*

43
libraries/AP_HAL_ChibiOS/hwdef/iomcu-dshot/hwdef.dat Normal file
View File

@ -0,0 +1,43 @@
# hw definition file for processing by chibios_pins.py
include ../iomcu/hwdef.dat
undef AP_FASTBOOT_ENABLED AP_BOOTLOADER_FLASHING_ENABLED
undef HAL_DSHOT_ENABLED
# PPM uses a DMA channel that is required for TIM2, and no remapping of PA8 is possible
undef PA8
undef STM32_ICU_USE_TIM1 RCIN_ICU_TIMER STM32_RCIN_DMA_STREAM HAL_USE_ICU
undef PORT_INT_REQUIRED_STACK MAIN_STACK
undef CH_CFG_ST_TIMEDELTA CH_CFG_ST_FREQUENCY
define HAL_NO_LED_THREAD 1
# TIM2_UP required for PWM1/2
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 as channels used by high speed USART2 RX/TX
# It's possible that DMA sharing might make this workable
#define STM32_TIM_TIM4_UP_DMA_STREAM STM32_DMA_STREAM_ID(1, 7)
#define STM32_TIM_TIM4_UP_DMA_CHAN 1
# TIM3_UP required for PWM5-8
define STM32_TIM_TIM3_UP_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
define STM32_TIM_TIM3_UP_DMA_CHAN 1
define NO_FASTBOOT
define HAL_WITH_DSP FALSE
define HAL_DSHOT_ENABLED TRUE
define HAL_SERIALLED_ENABLED FALSE
# Tickless mode is required in order for the virtual timers used by dshot to work correctly
STM32_ST_USE_TIMER 15
define CH_CFG_ST_RESOLUTION 16
define CH_CFG_ST_TIMEDELTA 2
define CH_CFG_ST_FREQUENCY 1000000
# ChibiOS already correctly sets stack sizes and considers 64 extra as conservative
define PORT_INT_REQUIRED_STACK 64
# "main" stack is only used for exceptions and ISR, but they can be nested
MAIN_STACK 0x100

5
libraries/AP_HAL_ChibiOS/hwdef/iomcu/hwdef.dat
View File

@ -134,7 +134,7 @@ define SOFTSIG_MAX_SIGNAL_TRANSITIONS 128
define IOMCU_FW TRUE
define AP_FASTBOOT_ENABLED 0
IOMCU_FW 1
MAIN_STACK 0x200
MAIN_STACK 0x100
PROCESS_STACK 0x250
define HAL_DISABLE_LOOP_DELAY
define AP_BOOTLOADER_FLASHING_ENABLED 0
@ -150,3 +150,6 @@ define AP_RCPROTOCOL_ST24_ENABLED 1
define AP_RCPROTOCOL_FPORT_ENABLED 1
AUTOBUILD_TARGETS iofirmware
define HAL_WITH_DSP FALSE
define HAL_DSHOT_ENABLED FALSE

2
libraries/AP_HAL_ChibiOS/hwdef/iomcu_f103_8MHz/hwdef.dat
View File

@ -151,3 +151,5 @@ define AP_RCPROTOCOL_ST24_ENABLED 1
define AP_RCPROTOCOL_FPORT_ENABLED 1
AUTOBUILD_TARGETS iofirmware
define HAL_WITH_DSP FALSE
define HAL_DSHOT_ENABLED FALSE