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

AP_IOMCU: propagate dshot rates through to IOMCU

Browse Source 
implement dshot ESC telemetry
add support for channel enablement/disablement
add stack checks and reporting for MSP stack
wait correct timeout in tickless mode
ensure that dshot sees all pwm updates as the occur in order to maintain periodicity
ensure dshot options are propagated on reset
implement dshot commands
ensure oneshot/125 and mode are setup correctly
add instrumentation for process stack
prevent illegal recursive locks
ignore requests for dshot 600
add support for soft reboot of iomcu
This commit is contained in:
Andy Piper 2023-05-24 21:43:17 +01:00 committed by Andrew Tridgell
parent 9a21297cd1
commit e6e0543b8a
5 changed files with 240 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -38,7 +38,10 @@ enum ioevents {
IOEVENT_SET_SAFETY_MASK,
IOEVENT_MIXING,
IOEVENT_GPIO,
IOEVENT_SET_OUTPUT_MODE
IOEVENT_SET_OUTPUT_MODE,
IOEVENT_SET_DSHOT_PERIOD,
IOEVENT_SET_CHANNEL_MASK,
IOEVENT_DSHOT,
};
// max number of consecutve protocol failures we accept before raising
@ -215,6 +218,14 @@ void AP_IOMCU::thread_main(void)
}
mask &= ~EVENT_MASK(IOEVENT_SET_DEFAULT_RATE);
if (mask & EVENT_MASK(IOEVENT_SET_DSHOT_PERIOD)) {
if (!write_registers(PAGE_SETUP, PAGE_REG_SETUP_DSHOT_PERIOD, sizeof(dshot_rate)/2, (const uint16_t *)&dshot_rate)) {
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_SET_DSHOT_PERIOD);
if (mask & EVENT_MASK(IOEVENT_SET_ONESHOT_ON)) {
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_ONESHOT)) {
event_failed(mask);
@ -237,6 +248,15 @@ void AP_IOMCU::thread_main(void)
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_SET_OUTPUT_MODE);
if (mask & EVENT_MASK(IOEVENT_SET_CHANNEL_MASK)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_CHANNEL_MASK, pwm_out.channel_mask)) {
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_SET_CHANNEL_MASK);
if (mask & EVENT_MASK(IOEVENT_SET_SAFETY_MASK)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_IGNORE_SAFETY, pwm_out.safety_mask)) {
@ -256,6 +276,15 @@ void AP_IOMCU::thread_main(void)
mask &= ~EVENT_MASK(IOEVENT_GPIO);
}
if (mask & EVENT_MASK(IOEVENT_DSHOT)) {
if (!write_registers(PAGE_DSHOT, 0, sizeof(dshot)/sizeof(uint16_t), (const uint16_t*)&dshot)) {
memset(&dshot, 0, sizeof(dshot));
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_DSHOT);
// check for regular timed events
uint32_t now = AP_HAL::millis();
if (now - last_rc_read_ms > 20) {
@ -314,8 +343,8 @@ void AP_IOMCU::send_servo_out()
n = MIN(n, IOMCU_MAX_CHANNELS);
}
uint32_t now = AP_HAL::micros();
if (now - last_servo_out_us >= 2000) {
// don't send data at more than 500Hz
if (now - last_servo_out_us >= 2000 || AP_BoardConfig::io_dshot()) {
// don't send data at more than 500Hz except when using dshot which is more timing sensitive
if (write_registers(PAGE_DIRECT_PWM, 0, n, pwm_out.pwm)) {
last_servo_out_us = now;
}
@ -413,10 +442,12 @@ void AP_IOMCU::write_log()
static uint32_t last_io_print;
if (now - last_io_print >= 5000) {
last_io_print = now;
debug("t=%lu num=%lu mem=%u terr=%lu nerr=%lu crc=%u opcode=%u rd=%u wr=%u ur=%u ndel=%lu\n",
debug("t=%lu num=%lu mem=%u mstack=%u pstack=%u terr=%lu nerr=%lu crc=%u opcode=%u rd=%u wr=%u ur=%u ndel=%lu\n",
now,
reg_status.total_pkts,
reg_status.freemem,
reg_status.freemstack,
reg_status.freepstack,
total_errors,
reg_status.num_errors,
reg_status.err_crc,
@ -797,6 +828,33 @@ void AP_IOMCU::set_brushed_mode(void)
rate.brushed_enabled = true;
}
#if HAL_DSHOT_ENABLED
// set output mode
void AP_IOMCU::set_dshot_period(uint16_t period_us, uint8_t drate)
{
dshot_rate.period_us = period_us;
dshot_rate.rate = drate;
trigger_event(IOEVENT_SET_DSHOT_PERIOD);
}
// set output mode
void AP_IOMCU::set_telem_request_mask(uint32_t mask)
{
dshot.telem_mask = mask;
trigger_event(IOEVENT_DSHOT);
}
void AP_IOMCU::send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority)
{
dshot.command = command;
dshot.chan = chan;
dshot.command_timeout_ms = command_timeout_ms;
dshot.repeat_count = repeat_count;
dshot.priority = priority;
trigger_event(IOEVENT_DSHOT);
}
#endif
// set output mode
void AP_IOMCU::set_output_mode(uint16_t mask, uint16_t mode)
{
@ -805,6 +863,23 @@ void AP_IOMCU::set_output_mode(uint16_t mask, uint16_t mode)
trigger_event(IOEVENT_SET_OUTPUT_MODE);
}
// setup channels
void AP_IOMCU::enable_ch(uint8_t ch)
{
if (!(pwm_out.channel_mask & 1U << ch)) {
pwm_out.channel_mask |= 1U << ch;
trigger_event(IOEVENT_SET_CHANNEL_MASK);
}
}
void AP_IOMCU::disable_ch(uint8_t ch)
{
if (pwm_out.channel_mask & 1U << ch) {
pwm_out.channel_mask &= ~(1U << ch);
trigger_event(IOEVENT_SET_CHANNEL_MASK);
}
}
// handling of BRD_SAFETYOPTION parameter
void AP_IOMCU::update_safety_options(void)
{
@ -952,6 +1027,16 @@ void AP_IOMCU::shutdown(void)
}
}
/*
reboot IOMCU
*/
void AP_IOMCU::soft_reboot(void)
{
const uint16_t magic = REBOOT_BL_MAGIC;
write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic);
}
/*
request bind on a DSM radio
*/
@ -1103,6 +1188,9 @@ void AP_IOMCU::check_iomcu_reset(void)
}
trigger_event(IOEVENT_SET_RATES);
trigger_event(IOEVENT_SET_DEFAULT_RATE);
trigger_event(IOEVENT_SET_DSHOT_PERIOD);
trigger_event(IOEVENT_SET_OUTPUT_MODE);
trigger_event(IOEVENT_SET_CHANNEL_MASK);
if (rate.oneshot_enabled) {
trigger_event(IOEVENT_SET_ONESHOT_ON);
}

24
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -101,12 +101,28 @@ public:
// set output mode
void set_output_mode(uint16_t mask, uint16_t mode);
#if HAL_DSHOT_ENABLED
// set dshot output period
void set_dshot_period(uint16_t period_us, uint8_t drate);
// set telem request mask
void set_telem_request_mask(uint32_t mask);
// send a dshot command
void send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority);
#endif
// setup channels
void enable_ch(uint8_t ch);
void disable_ch(uint8_t ch);
// check if IO is healthy
bool healthy(void);
// shutdown IO protocol (for reboot)
void shutdown();
void soft_reboot();
// setup for FMU failsafe mixing
bool setup_mixing(RCMapper *rcmap, int8_t override_chan,
float mixing_gain, uint16_t manual_rc_mask);
@ -207,6 +223,7 @@ private:
uint16_t failsafe_pwm[IOMCU_MAX_CHANNELS];
uint8_t failsafe_pwm_set;
uint8_t failsafe_pwm_sent;
uint16_t channel_mask;
} pwm_out;
// read back pwm values
@ -224,6 +241,13 @@ private:
bool brushed_enabled;
} rate;
struct {
uint16_t period_us;
uint16_t rate;
} dshot_rate;
struct page_dshot dshot;
struct page_GPIO GPIO;
// output mode values
struct {

128
libraries/AP_IOMCU/iofirmware/iofirmware.cpp
View File

@ -56,7 +56,7 @@ enum ioevents {
static void dma_rx_end_cb(UARTDriver *uart)
{
osalSysLockFromISR();
chSysLockFromISR();
uart->usart->CR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
(void)uart->usart->SR;
@ -80,7 +80,7 @@ static void dma_rx_end_cb(UARTDriver *uart)
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
dmaStreamEnable(uart->dmatx);
uart->usart->CR3 |= USART_CR3_DMAT;
osalSysUnlockFromISR();
chSysUnlockFromISR();
}
static void idle_rx_handler(UARTDriver *uart)
@ -92,7 +92,7 @@ static void idle_rx_handler(UARTDriver *uart)
USART_SR_FE |
USART_SR_PE)) { /* framing error - start/stop bit lost or line break */
/* send a line break - this will abort transmission/reception on the other end */
osalSysLockFromISR();
chSysLockFromISR();
uart->usart->SR = ~USART_SR_LBD;
uart->usart->CR1 |= USART_CR1_SBK;
iomcu.reg_status.num_errors++;
@ -110,7 +110,7 @@ static void idle_rx_handler(UARTDriver *uart)
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
dmaStreamEnable(uart->dmarx);
uart->usart->CR3 |= USART_CR3_DMAR;
osalSysUnlockFromISR();
chSysUnlockFromISR();
return;
}
@ -140,11 +140,6 @@ void setup(void)
{
hal.rcin->init();
hal.rcout->init();
for (uint8_t i = 0; i< 14; i++) {
hal.rcout->enable_ch(i);
}
iomcu.init();
iomcu.calculate_fw_crc();
@ -194,11 +189,45 @@ void AP_IOMCU_FW::init()
}
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
static void stackCheck(uint16_t& mstack, uint16_t& pstack) {
extern uint32_t __main_stack_base__[];
extern uint32_t __main_stack_end__[];
uint32_t stklimit = (uint32_t)__main_stack_end__;
uint32_t stkbase = (uint32_t)__main_stack_base__;
uint32_t *crawl = (uint32_t *)stkbase;
while (*crawl == 0x55555555 && crawl < (uint32_t *)stklimit) {
crawl++;
}
uint32_t free = (uint32_t)crawl - stkbase;
chDbgAssert(free > 0, "mstack exhausted");
mstack = (uint16_t)free;
extern uint32_t __main_thread_stack_base__[];
extern uint32_t __main_thread_stack_end__[];
stklimit = (uint32_t)__main_thread_stack_end__;
stkbase = (uint32_t)__main_thread_stack_base__;
crawl = (uint32_t *)stkbase;
while (*crawl == 0x55555555 && crawl < (uint32_t *)stklimit) {
crawl++;
}
free = (uint32_t)crawl - stkbase;
chDbgAssert(free > 0, "pstack exhausted");
pstack = (uint16_t)free;
}
#endif /* CH_DBG_ENABLE_STACK_CHECK == TRUE */
void AP_IOMCU_FW::update()
{
#if CH_CFG_ST_FREQUENCY == 1000000
eventmask_t mask = chEvtWaitAnyTimeout(~0, TIME_US2I(1000));
#else
// we are not running any other threads, so we can use an
// immediate timeout here for lowest latency
eventmask_t mask = chEvtWaitAnyTimeout(~0, TIME_IMMEDIATE);
#endif
// we get the timestamp once here, and avoid fetching it
// within the DMA callbacks
@ -256,13 +285,16 @@ void AP_IOMCU_FW::update()
heater_update();
rcin_update();
safety_update();
rcout_mode_update();
rcout_config_update();
rcin_serial_update();
hal.rcout->timer_tick();
if (dsm_bind_state) {
dsm_bind_step();
}
GPIO_write();
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
stackCheck(reg_status.freemstack, reg_status.freepstack);
#endif
}
}
@ -513,6 +545,14 @@ bool AP_IOMCU_FW::handle_code_write()
reg_setup.pwm_defaultrate = rx_io_packet.regs[0];
update_default_rate = true;
break;
case PAGE_REG_SETUP_DSHOT_PERIOD:
reg_setup.dshot_period_us = rx_io_packet.regs[0];
reg_setup.dshot_rate = rx_io_packet.regs[1];
hal.rcout->set_dshot_period(reg_setup.dshot_period_us, reg_setup.dshot_rate);
break;
case PAGE_REG_SETUP_CHANNEL_MASK:
reg_setup.channel_mask = rx_io_packet.regs[0];
break;
case PAGE_REG_SETUP_SBUS_RATE:
reg_setup.sbus_rate = rx_io_packet.regs[0];
sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
@ -637,19 +677,24 @@ bool AP_IOMCU_FW::handle_code_write()
return false;
}
memcpy(&GPIO, &rx_io_packet.regs[0] + rx_io_packet.offset, sizeof(GPIO));
if (GPIO.channel_mask != last_GPIO_channel_mask) {
for (uint8_t i=0; i<8; i++) {
if ((GPIO.channel_mask & (1U << i)) != 0) {
hal.rcout->disable_ch(i);
hal.gpio->pinMode(101+i, HAL_GPIO_OUTPUT);
} else {
hal.rcout->enable_ch(i);
}
}
last_GPIO_channel_mask = GPIO.channel_mask;
}
break;
case PAGE_DSHOT: {
uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
if (offset + num_values > sizeof(dshot)/2) {
return false;
}
memcpy(((uint16_t *)&dshot)+offset, &rx_io_packet.regs[0], num_values*2);
if(dshot.telem_mask) {
hal.rcout->set_telem_request_mask(dshot.telem_mask);
}
if (dshot.command) {
hal.rcout->send_dshot_command(dshot.command, dshot.chan, dshot.command_timeout_ms, dshot.repeat_count, dshot.priority);
}
break;
}
default:
break;
}
@ -763,17 +808,48 @@ void AP_IOMCU_FW::safety_update(void)
/*
update hal.rcout mode if needed
*/
void AP_IOMCU_FW::rcout_mode_update(void)
void AP_IOMCU_FW::rcout_config_update(void)
{
bool use_dshot = mode_out.mode >= AP_HAL::RCOutput::MODE_PWM_DSHOT150 && mode_out.mode <= AP_HAL::RCOutput::MODE_PWM_DSHOT600;
// channels cannot be changed from within a lock zone
// so needs to be done here
if (GPIO.channel_mask != last_GPIO_channel_mask) {
for (uint8_t i=0; i<8; i++) {
if ((GPIO.channel_mask & (1U << i)) != 0) {
hal.rcout->disable_ch(i);
hal.gpio->pinMode(101+i, HAL_GPIO_OUTPUT);
} else {
hal.rcout->enable_ch(i);
}
}
last_GPIO_channel_mask = GPIO.channel_mask;
}
if (last_channel_mask != reg_setup.channel_mask) {
for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
if (reg_setup.channel_mask & 1U << i) {
hal.rcout->enable_ch(i);
} else {
hal.rcout->disable_ch(i);
}
}
last_channel_mask = reg_setup.channel_mask;
}
bool use_dshot = mode_out.mode >= AP_HAL::RCOutput::MODE_PWM_DSHOT150
&& mode_out.mode <= AP_HAL::RCOutput::MODE_PWM_DSHOT300;
if (use_dshot && !dshot_enabled) {
dshot_enabled = true;
hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode);
// enabling dshot changes the memory allocation
reg_status.freemem = hal.util->available_memory();
}
bool use_oneshot = mode_out.mode == AP_HAL::RCOutput::MODE_PWM_ONESHOT;
bool use_oneshot = (mode_out.mode == AP_HAL::RCOutput::MODE_PWM_ONESHOT
|| mode_out.mode == AP_HAL::RCOutput::MODE_PWM_ONESHOT125);
if (use_oneshot && !oneshot_enabled) {
oneshot_enabled = true;
hal.rcout->set_output_mode(mode_out.mask, AP_HAL::RCOutput::MODE_PWM_ONESHOT);
// setup to use a 1Hz frequency, so we only get output when we trigger
hal.rcout->set_freq(mode_out.mask, 1);
hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode);
}
bool use_brushed = mode_out.mode == AP_HAL::RCOutput::MODE_PWM_BRUSHED;
if (use_brushed && !brushed_enabled) {
@ -784,8 +860,6 @@ void AP_IOMCU_FW::rcout_mode_update(void)
hal.rcout->set_output_mode(mode_out.mask, AP_HAL::RCOutput::MODE_PWM_BRUSHED);
hal.rcout->set_freq(mode_out.mask, reg_setup.pwm_altrate);
}
mode_out.mask = 0;
mode_out.mode = 0;
}
/*

10
libraries/AP_IOMCU/iofirmware/iofirmware.h
View File

@ -30,7 +30,7 @@ public:
bool handle_code_read();
void schedule_reboot(uint32_t time_ms);
void safety_update();
void rcout_mode_update();
void rcout_config_update();
void rcin_serial_init();
void rcin_serial_update();
void page_status_update(void);
@ -67,8 +67,13 @@ public:
uint16_t ignore_safety;
uint16_t heater_duty_cycle = 0xFFFFU;
uint16_t pwm_altclock = 1;
uint16_t dshot_period_us;
uint16_t dshot_rate;
uint16_t channel_mask;
} reg_setup;
uint16_t last_channel_mask;
// CONFIG values
struct page_config config;
@ -116,6 +121,9 @@ public:
uint8_t last_GPIO_channel_mask;
void GPIO_write();
// DSHOT runtime
struct page_dshot dshot;
// true when override channel active
bool override_active;

14
libraries/AP_IOMCU/iofirmware/ioprotocol.h
View File

@ -56,6 +56,7 @@ enum iopage {
PAGE_FAILSAFE_PWM = 55,
PAGE_MIXING = 200,
PAGE_GPIO = 201,
PAGE_DSHOT = 202,
};
// setup page registers
@ -86,6 +87,8 @@ enum iopage {
#define PAGE_REG_SETUP_HEATER_DUTY_CYCLE 21
#define PAGE_REG_SETUP_DSM_BIND 22
#define PAGE_REG_SETUP_RC_PROTOCOLS 23 // uses 2 slots, 23 and 24
#define PAGE_REG_SETUP_DSHOT_PERIOD 25
#define PAGE_REG_SETUP_CHANNEL_MASK 27
// config page registers
#define PAGE_CONFIG_PROTOCOL_VERSION 0
@ -107,6 +110,8 @@ struct page_config {
struct page_reg_status {
uint16_t freemem;
uint16_t freemstack;
uint16_t freepstack;
uint32_t timestamp_ms;
uint16_t vservo;
uint16_t vrssi;
@ -170,3 +175,12 @@ struct __attribute__((packed, aligned(2))) page_GPIO {
uint8_t channel_mask;
uint8_t output_mask;
};
struct __attribute__((packed, aligned(2))) page_dshot {
uint16_t telem_mask;
uint8_t command;
uint8_t chan;
uint32_t command_timeout_ms;
uint8_t repeat_count;
uint8_t priority;
};