diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index cbb15e8c30..28a2e0fa7e 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -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); } diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index 77ec6295c9..7a15347b32 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -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 { diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index 8c36665649..9314629320 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -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; ienable_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; } /* diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h index 64d661e269..c4fe2b5305 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.h +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h @@ -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; diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h index 4f1f11070a..8ab26b6469 100644 --- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h +++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h @@ -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; +};