diff --git a/Tools/IO_Firmware/iofirmware_cubered.bin b/Tools/IO_Firmware/iofirmware_cubered.bin new file mode 100755 index 0000000000000..20bbfd153fb9f Binary files /dev/null and b/Tools/IO_Firmware/iofirmware_cubered.bin differ diff --git a/Tools/scripts/build_iofirmware.py b/Tools/scripts/build_iofirmware.py index c2f67096a82fc..307880ae57c4d 100755 --- a/Tools/scripts/build_iofirmware.py +++ b/Tools/scripts/build_iofirmware.py @@ -72,3 +72,8 @@ def run_program(cmd_list): 'Tools/IO_Firmware/iofirmware_f103_8MHz_dshot_lowpolh.bin') shutil.copy('build/iomcu-f103-8MHz-dshot/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_f103_8MHz_dshot_highpolh.bin') + +run_program(["./waf", "configure", "--board", 'CubeRedSecondary-IO']) +run_program(["./waf", "clean"]) +run_program(["./waf", "iofirmware"]) +shutil.copy('build/CubeRedSecondary-IO/bin/iofirmware.bin', 'Tools/IO_Firmware/iofirmware_cubered.bin') diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 5d4717eee002f..a646837658b40 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -95,6 +95,14 @@ THD_WORKING_AREA(_monitor_thread_wa, MONITOR_THD_WA_SIZE); #define AP_HAL_CHIBIOS_IN_EXPECTED_DELAY_WHEN_NOT_INITIALISED 1 #endif +#if defined(STM32H7) && defined(IOMCU_FW) +void AP_memory_guard_error(uint16_t error); +void AP_memory_guard_error(uint16_t error) { + // simply fast reboot for now + hal.scheduler->reboot(false); +} +#endif + Scheduler::Scheduler() { } diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index 035444cbeede4..9bb060c34117b 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -545,7 +545,7 @@ void UARTDriver::dma_rx_enable(void) dmamode |= STM32_DMA_CR_CHSEL(sdef.dma_rx_channel_id); dmamode |= STM32_DMA_CR_PL(0); #if defined(STM32H7) - dmamode |= 1<<20; // TRBUFF See 2.3.1 in the H743 errata + dmamode |= DMA_SxCR_TRBUFF; // TRBUFF See 2.3.1 in the H743 errata #endif rx_bounce_idx ^= 1; stm32_cacheBufferInvalidate(rx_bounce_buf[rx_bounce_idx], RX_BOUNCE_BUFSIZE); @@ -901,7 +901,7 @@ void UARTDriver::write_pending_bytes_DMA(uint32_t n) dmamode |= STM32_DMA_CR_CHSEL(sdef.dma_tx_channel_id); dmamode |= STM32_DMA_CR_PL(0); #if defined(STM32H7) - dmamode |= 1<<20; // TRBUFF See 2.3.1 in the H743 errata + dmamode |= DMA_SxCR_TRBUFF; // TRBUFF See 2.3.1 in the H743 errata #endif dmaStreamSetMode(txdma, dmamode | STM32_DMA_CR_DIR_M2P | STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE); diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 1012216b023c8..daeaa07b52f5f 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -658,14 +658,17 @@ void Util::uart_info(ExpandingString &str) for (uint8_t i = 0; i < HAL_UART_NUM_SERIAL_PORTS; i++) { auto *uart = hal.serial(i); if (uart) { - str.printf("SERIAL%u ", i); +#if HAL_WITH_IO_MCU + if (i == HAL_UART_IOMCU_IDX) { + str.printf("IOMCU "); + } else +#endif + { + str.printf("SERIAL%u ", i); + } uart->uart_info(str, sys_uart_stats.serial[i], dt_ms); } } -#if HAL_WITH_IO_MCU - str.printf("IOMCU "); - uart_io.uart_info(str, sys_uart_stats.io, dt_ms); -#endif } // Log UART message for each serial port @@ -684,10 +687,6 @@ void Util::uart_log() uart->log_stats(i, log_uart_stats.serial[i], dt_ms); } } -#if HAL_WITH_IO_MCU - // Use magic instance 100 for IOMCU - uart_io.log_stats(100, log_uart_stats.io, dt_ms); -#endif } #endif // HAL_LOGGING_ENABLED #endif // HAL_UART_STATS_ENABLED diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/hwdef.dat index 0dcf4bba30872..38ddac35e7e7b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/hwdef.dat @@ -6,6 +6,7 @@ undef HAL_WITH_IO_MCU_BIDIR_DSHOT undef COMPASS undef BARO undef DEFAULT_SERIAL7_PROTOCOL +undef IOMCU_UART define AP_ADVANCEDFAILSAFE_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat index f0ea70db24920..d5577719e325d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat @@ -291,7 +291,16 @@ INT_FLASH_PRIMARY 1 define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_PPP define DEFAULT_SERIAL7_BAUD 8000000 +IOMCU_UART UART7 +define HAL_IOMCU_UART_OPTIONS 8 + define AP_NETWORKING_DEFAULT_STATIC_IP_ADDR "192.168.144.100" define AP_NETWORKING_DEFAULT_STATIC_NETMASK "255.255.255.0" define AP_NETWORKING_DEFAULT_STATIC_GW_ADDR "192.168.144.11" define AP_NETWORKING_BACKEND_PPP 1 + +ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cubered.bin +define HAL_IOMCU_FW_FLASH_SIZE (0x200000 - 0x20000 - 0x40000) +define HAL_IOMCU_BOOTLOADER_BAUDRATE 2000000 + +DMA_NOSHARE SPI1* SPI2* SPI4* UART7* diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary-IO/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary-IO/hwdef.dat new file mode 100644 index 0000000000000..dc9c177c7455f --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary-IO/hwdef.dat @@ -0,0 +1,170 @@ +# hw definition file for processing by chibios_hwdef.py +# for H757 + +# MCU class and specific type +MCU STM32H7xx STM32H757xx + +define CORE_CM7 +define SMPS_PWR + +env OPTIMIZE -O3 + +# crystal frequency +OSCILLATOR_HZ 24000000 + +# board ID for firmware load +APJ_BOARD_ID 1070 + +FLASH_SIZE_KB 2048 + +# bootloader is installed at zero offset +FLASH_RESERVE_START_KB 128 + +# the location where the bootloader will put the firmware +# the H743 has 128k sectors +FLASH_BOOTLOADER_LOAD_KB 128 + +define HAL_LED_ON 0 + +define IOMCU_FW TRUE +define AP_FASTBOOT_ENABLED 1 +IOMCU_FW 1 + +define HAL_USE_RTC FALSE +define HAL_NO_UARTDRIVER TRUE +define HAL_LOGGING_ENABLED 0 +define AP_CRASHDUMP_ENABLED 0 +define HAL_ENABLE_SAVE_PERSISTENT_PARAMS 0 + +define HAL_USE_EMPTY_STORAGE 1 +define HAL_STORAGE_SIZE 16384 + +# avoid timer threads to save memory +define HAL_NO_TIMER_THREAD +define HAL_NO_RCOUT_THREAD # also disables LED thread + +# ADC setup +PF11 FMU_SERVORAIL_VCC_SENS ADC1 +PA6 RSSI_IN ADC1 SCALE(2) + +define HAL_IOMCU_RSSI_ADC_CHANNEL 3 + +# CAN config +PB14 GPIOCAN2_TERM OUTPUT LOW +PC12 GPIOCAN1_SHUTDOWN OUTPUT HIGH +PF1 GPIOCAN2_SHUTDOWN OUTPUT HIGH + +define HAL_CHIBIOS_ARCH_FMUV3 1 + +define BOARD_TYPE_DEFAULT 3 + +# RCIN +PC8 TIM8_CH3 TIM8 RCININT PULLDOWN + +# SWD +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# GPIO +PD10 AMBER_LED OUTPUT HIGH OPENDRAIN GPIO(0) +PE15 VDD_BRICK_nVALID INPUT PULLUP +PG0 VDD_BRICK2_nVALID INPUT PULLUP +PG5 VDD_SERVO_FAULT INPUT PULLUP +PG1 PWM_VOLT_SEL OUTPUT HIGH GPIO(3) + +# Control of Spektrum power pin +PB2 SPEKTRUM_PWR_EN OUTPUT HIGH GPIO(73) +define HAL_GPIO_SPEKTRUM_PWR 73 + +# Spektrum Power is Active High +define HAL_SPEKTRUM_PWR_ENABLED 1 + + +# Timer +PA8 TIM1_CH1 TIM1 PWM(1) GPIO(50) +PA9 TIM1_CH2 TIM1 PWM(2) GPIO(51) +PA10 TIM1_CH3 TIM1 PWM(3) GPIO(52) +PA11 TIM1_CH4 TIM1 PWM(4) GPIO(53) +PA5 TIM2_CH1 TIM2 PWM(5) GPIO(54) +PA1 TIM2_CH2 TIM2 PWM(6) GPIO(55) +PB10 TIM2_CH3 TIM2 PWM(7) GPIO(56) +PB11 TIM2_CH4 TIM2 PWM(8) GPIO(57) + +# Correctly set Direction of PWMs +# if UNIDIR is set then BIDIR must be reset +PA7 HP_UNIDIR_ENABLED OUTPUT HIGH GPIO(5) + + +# UART connected to FMU, uses DMA +PE7 UART7_RX UART7 SPEED_VERYLOW +PE8 UART7_TX UART7 SPEED_VERYLOW + +# UART for SBUS out +PC7 USART6_RX USART6 SPEED_HIGH LOW +PC6 USART6_TX USART6 + +# UART for DSM input +# TX side is for IO debug, and is unused +PC10 USART3_TX USART3 SPEED_HIGH +PC11 USART3_RX USART3 SPEED_HIGH + +# UART for debug +PE1 UART8_TX UART8 +PE0 UART8_RX UART8 + +# UART for RCIN +PD1 UART4_TX UART4 + +# USART for future use +PD5 USART2_TX USART2 SPEED_HIGH +PD6 USART2_RX USART2 SPEED_HIGH +PD4 USART2_RTS USART2 SPEED_HIGH +PD3 USART2_CTS USART2 SPEED_HIGH + +# order of UARTs +SERIAL_ORDER EMPTY EMPTY EMPTY EMPTY + +define HAL_USE_UART TRUE +define STM32_UART_USE_UART7 TRUE +define HAL_IO_FMU_COMMS UARTD7 +define HAL_IO_FMU_COMMS_TX_DMA_STREAM STM32_UART_UART7_TX_DMA_STREAM +define HAL_IO_FMU_COMMS_TX_DMA_CHANNEL STM32_UART_UART7_TX_DMA_CHAN +define HAL_IO_FMU_COMMS_TX_IRQ_PRIORITY STM32_UART_UART7_IRQ_PRIORITY +define HAL_IO_FMU_COMMS_RX_DMA_STREAM STM32_UART_UART7_RX_DMA_STREAM +define HAL_IO_FMU_COMMS_RX_DMA_CHANNEL STM32_UART_UART7_RX_DMA_CHAN + +define HAL_USE_SERIAL TRUE +define STM32_SERIAL_USE_UART7 FALSE +define STM32_SERIAL_USE_UART4 TRUE +define STM32_SERIAL_USE_USART3 TRUE +define STM32_SERIAL_USE_USART6 TRUE + +define HAL_IOMCU_RCIN_SERIAL_DRIVER SD4 +define HAL_IOMCU_DSM_SERIAL_DRIVER SD3 +define HAL_IOMCU_SBUS_OUT_SERIAL_DRIVER SD6 +define HAL_GPIO_PIN_SPEKTRUM_OUT PAL_LINE(GPIOC,11U) + + +define AP_NETWORKING_BACKEND_PPP 1 + +# only use pulse input for PPM, other protocols +# are on serial +define HAL_RCIN_PULSE_PPM_ONLY + + +define HAL_DSHOT_ENABLED TRUE +define HAL_IOMCU_SEPARATE_SBUS_OUT_RCIN 1 + + +PC4 SAFETY_INPUT INPUT PULLDOWN +PE3 SAFETY_LED OUTPUT HIGH OPENDRAIN + +define HAL_IOMCU_APP_SIZE_MAX (0x200000 - 0x20000 - 0x40000) +define HAL_IOMCU_APP_LOAD_ADDRESS 0x08020000 + +define IOMCU_DEBUG SD8 +define STM32_SERIAL_USE_UART8 TRUE +define HAL_BL_IOMCU_FW_DETECT 1 + +define AP_HAL_UARTDRIVER_ENABLED 0 +define HAL_SERIAL_SBUS_SWAPPED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/common_mixf.ld b/libraries/AP_HAL_ChibiOS/hwdef/common/common_mixf.ld index 68895095cc704..79e175b2636ef 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/common_mixf.ld +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/common_mixf.ld @@ -74,6 +74,7 @@ SECTIONS *(.ap_romfs*) /* moving GCS_MAVLink to ext_flash */ lib/lib*.a:GCS*.*(.text* .rodata*) + lib/lib*.a:tables.*(.text* .rodata*) /* tables for AP_Declination */ *(.extflash) } > ext_flash diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index fb9cd62a61bdc..b0ed40371b4da 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -1052,6 +1052,12 @@ def write_mcu_config(self, f): else: self.env_vars['IOMCU_FW'] = 0 + # check if heater pin defined + if 'HEATER' in self.bylabel.keys(): + self.env_vars['IOMCU_FW_WITH_HEATER'] = 1 + else: + self.env_vars['IOMCU_FW_WITH_HEATER'] = 0 + if self.get_config('PERIPH_FW', required=False): self.env_vars['PERIPH_FW'] = self.get_config('PERIPH_FW') else: @@ -1866,6 +1872,8 @@ def get_extra_bylabel(self, label, name, default=None): def write_UART_config(self, f): '''write UART config defines''' serial_list = self.get_config('SERIAL_ORDER', required=False, aslist=True) + if 'IOMCU_UART' in self.config and self.config['IOMCU_UART'][0] not in serial_list: + serial_list.append(self.config['IOMCU_UART'][0]) if serial_list is None: return while len(serial_list) < 3: # enough ports for CrashCatcher UART discovery @@ -1903,11 +1911,15 @@ def write_UART_config(self, f): self.error("Need io_firmware.bin in ROMFS for IOMCU") self.write_defaulting_define(f, 'HAL_WITH_IO_MCU', 1) - f.write('#define HAL_UART_IOMCU_IDX %u\n' % len(serial_list)) - f.write( - '#define HAL_UART_IO_DRIVER ChibiOS::UARTDriver uart_io(HAL_UART_IOMCU_IDX)\n' - ) - serial_list.append(self.config['IOMCU_UART'][0]) + + if self.config['IOMCU_UART'][0]: + # get index of serial port in serial_list + index = serial_list.index(self.config['IOMCU_UART'][0]) + f.write('#define HAL_UART_IOMCU_IDX %u\n' % int(index)) + f.write( + '#define HAL_UART_IO_DRIVER constexpr ChibiOS::UARTDriver &uart_io = serial%sDriver;\n' % (index) + ) + f.write('#define HAL_HAVE_SERVO_VOLTAGE 1\n') # make the assumption that IO gurantees servo monitoring # all IOMCU capable boards have SBUS out f.write('#define AP_FEATURE_SBUS_OUT 1\n') @@ -2054,8 +2066,6 @@ def get_RTS_alt_function(): #endif ''') num_ports = len(devlist) - if 'IOMCU_UART' in self.config: - num_ports -= 1 if num_ports > 10: self.error("Exceeded max num SERIALs of 10 (%u)" % num_ports) f.write('#define HAL_UART_NUM_SERIAL_PORTS %u\n' % num_ports) diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index defd5f1916fb4..aed2a51fe5197 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -21,6 +21,7 @@ #include #include #include +#include extern const AP_HAL::HAL &hal; @@ -78,6 +79,9 @@ void AP_IOMCU::init(void) { // uart runs at 1.5MBit uart.begin(1500*1000, 128, 128); +#ifdef HAL_IOMCU_UART_OPTIONS + uart.set_options(HAL_IOMCU_UART_OPTIONS); +#endif uart.set_unbuffered_writes(true); #if IOMCU_DEBUG_ENABLE @@ -1092,10 +1096,14 @@ void AP_IOMCU::send_rc_protocols() /* check ROMFS firmware against CRC on IOMCU, and if incorrect then upload new firmware */ +#ifndef HAL_IOMCU_FW_FLASH_SIZE +#define HAL_IOMCU_FW_FLASH_SIZE (0x10000 - 0x1000) +#endif + bool AP_IOMCU::check_crc(void) { // flash size minus 4k bootloader - const uint32_t flash_size = 0x10000 - 0x1000; + const uint32_t flash_size = HAL_IOMCU_FW_FLASH_SIZE; const char *path = AP_BoardConfig::io_dshot() ? dshot_fw_name : fw_name; fw = AP_ROMFS::find_decompress(path, fw_size); @@ -1188,6 +1196,10 @@ bool AP_IOMCU::healthy(void) */ void AP_IOMCU::shutdown(void) { + if (!initialised) { + // we're not initialised yet, so cannot shutdown + return; + } do_shutdown = true; while (!done_shutdown) { hal.scheduler->delay(1); diff --git a/libraries/AP_IOMCU/fw_uploader.cpp b/libraries/AP_IOMCU/fw_uploader.cpp index 31858b00f117f..4a2303f1d7e42 100644 --- a/libraries/AP_IOMCU/fw_uploader.cpp +++ b/libraries/AP_IOMCU/fw_uploader.cpp @@ -60,13 +60,17 @@ extern const AP_HAL::HAL &hal; +#ifndef HAL_IOMCU_BOOTLOADER_BAUDRATE +#define HAL_IOMCU_BOOTLOADER_BAUDRATE 115200 +#endif + /* upload a firmware to the IOMCU */ bool AP_IOMCU::upload_fw(void) { // set baudrate for bootloader - uart.begin(115200, 256, 256); + uart.begin(HAL_IOMCU_BOOTLOADER_BAUDRATE, 256, 256); bool ret = false; @@ -84,11 +88,11 @@ bool AP_IOMCU::upload_fw(void) return false; } - uint32_t bl_rev; - ret = get_info(INFO_BL_REV, bl_rev); - - if (!ret) { - debug("Err: failed to contact bootloader"); + uint32_t bl_rev, unused; + if (!get_info(INFO_BL_REV, bl_rev) || + !get_info(INFO_BOARD_ID, unused) || + !get_info(INFO_FLASH_SIZE, unused)) { + debug("Err: failed to get bootloader info"); return false; } if (bl_rev > BL_REV) { @@ -97,6 +101,14 @@ bool AP_IOMCU::upload_fw(void) } debug("found bootloader revision: %u", unsigned(bl_rev)); + if (bl_rev > 2) { + // verify the CRC of the IO firmware + if (verify_rev3(fw_size)) { + // already up to date, no need to update, just reboot + reboot(); + return true; + } + } ret = erase(); if (!ret) { debug("erase failed"); @@ -281,7 +293,7 @@ bool AP_IOMCU::erase() debug("erase..."); send(PROTO_CHIP_ERASE); send(PROTO_EOC); - return get_sync(10000); + return get_sync(20000); // 20s timeout for erase } /* @@ -417,12 +429,13 @@ bool AP_IOMCU::verify_rev3(uint32_t fw_size_local) /* compare the CRC sum from the IO with the one calculated */ if (sum != crc) { debug("CRC wrong: received: 0x%x, expected: 0x%x", (unsigned)crc, (unsigned)sum); + get_sync(); return false; } crc_is_ok = true; - return true; + return get_sync(); } /* diff --git a/libraries/AP_IOMCU/iofirmware/analog.cpp b/libraries/AP_IOMCU/iofirmware/analog.cpp index 1da6d7f4e461f..7004330a5c572 100644 --- a/libraries/AP_IOMCU/iofirmware/analog.cpp +++ b/libraries/AP_IOMCU/iofirmware/analog.cpp @@ -22,6 +22,8 @@ #include "hal.h" #include "analog.h" +#ifdef HAL_DISABLE_ADC_DRIVER + #if HAL_USE_ADC != TRUE #error "HAL_USE_ADC must be set" #endif @@ -157,3 +159,40 @@ OSAL_IRQ_HANDLER(STM32_ADC1_HANDLER) { OSAL_IRQ_EPILOGUE(); } + +#else +#include + +extern const AP_HAL::HAL& hal; + +AP_HAL::AnalogSource *vrssi; + +void adc_init(void) +{ + vrssi = hal.analogin->channel(HAL_IOMCU_RSSI_ADC_CHANNEL); +} + +/* + unused stub methods + */ +void adc_enable_vrssi(void) {} +void adc_disable_vrssi(void) {} +void adc_sample_channels() {} + +/* + capture VSERVO in mV + */ +uint16_t adc_vservo(void) +{ + return hal.analogin->servorail_voltage() * 1000; +} + +/* + capture VRSSI in mV + */ +uint16_t adc_vrssi(void) +{ + return vrssi->voltage_average() * 1000; +} + +#endif // HAL_DISABLE_ADC_DRIVER diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index e3277b764e295..9ce45f6e78d23 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -25,6 +25,12 @@ #include "analog.h" #include "rc.h" #include +#if defined(__DCACHE_PRESENT) +#include +#else +#define stm32_cacheBufferInvalidate(addr, size) do {} while(0) +#define stm32_cacheBufferFlush(addr, size) do {} while(0) +#endif extern const AP_HAL::HAL &hal; @@ -38,6 +44,27 @@ static AP_IOMCU_FW iomcu; void setup(); void loop(); +#ifndef HAL_IO_FMU_COMMS +#define HAL_IO_FMU_COMMS UARTD2 +#endif + +#ifndef HAL_IOMCU_APP_SIZE_MAX +#define HAL_IOMCU_APP_SIZE_MAX 0xf000 +#define HAL_IOMCU_APP_LOAD_ADDRESS 0x08001000 +#endif + +#define HAL_IOMCU_MAGIC_ADDRESS ((uint32_t*)(HAL_IOMCU_APP_LOAD_ADDRESS + HAL_IOMCU_APP_SIZE_MAX - 4)) +#define HAL_IOMCU_MAGIC 0x937E638A + +#define DEBUG_ENABLED 0 + +#if defined(IOMCU_DEBUG) && DEBUG_ENABLED +#include "chprintf.h" +#define DEBUG(...) do { chprintf((BaseSequentialStream*)&IOMCU_DEBUG, __VA_ARGS__); } while (0) +#else +#define DEBUG(...) +#endif + #undef CH_DBG_ENABLE_STACK_CHECK #define CH_DBG_ENABLE_STACK_CHECK FALSE @@ -73,9 +100,17 @@ static void setup_rx_dma(hal_uart_driver* uart) dmaStreamDisable(uart->dmarx); dmaStreamSetMemory0(uart->dmarx, &iomcu.rx_io_packet); dmaStreamSetTransactionSize(uart->dmarx, sizeof(iomcu.rx_io_packet)); +#if defined(STM32H7) + dmaStreamSetPeripheral(uart->dmarx, &(uart->usart->RDR)); +#else dmaStreamSetPeripheral(uart->dmarx, &(uart->usart->DR)); +#endif dmaStreamSetMode(uart->dmarx, uart->dmarxmode | STM32_DMA_CR_DIR_P2M | - STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE); + STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE +#if defined(STM32H7) + | DMA_SxCR_TRBUFF +#endif + ); dmaStreamEnable(uart->dmarx); uart->usart->CR3 |= USART_CR3_DMAR; } @@ -87,11 +122,23 @@ static void setup_tx_dma(hal_uart_driver* uart) dmaStreamSetMemory0(uart->dmatx, &iomcu.tx_io_packet); dmaStreamSetTransactionSize(uart->dmatx, iomcu.tx_io_packet.get_size()); // starting the UART allocates the peripheral statically, so we need to reinstate it after swapping +#if defined(STM32H7) + dmaStreamSetPeripheral(uart->dmatx, &(uart->usart->TDR)); +#else dmaStreamSetPeripheral(uart->dmatx, &(uart->usart->DR)); +#endif dmaStreamSetMode(uart->dmatx, uart->dmatxmode | STM32_DMA_CR_DIR_M2P | - STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE); + STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE +#if defined(STM32H7) + | DMA_SxCR_TRBUFF +#endif + ); // enable transmission complete interrupt +#if defined(STM32H7) + uart->usart->ICR |= USART_ICR_TCCF; +#else uart->usart->SR &= ~USART_SR_TC; +#endif uart->usart->CR1 |= USART_CR1_TCIE; dmaStreamEnable(uart->dmatx); @@ -106,8 +153,12 @@ static void dma_rx_end_cb(hal_uart_driver *uart) dmaStreamDisable(uart->dmarx); - iomcu.process_io_packet(); + // reload the rx packet into CPU cache from RAM + stm32_cacheBufferInvalidate(&iomcu.rx_io_packet, sizeof(iomcu.rx_io_packet)); + iomcu.process_io_packet(); + // flush the tx packet from CPU cache to RAM + stm32_cacheBufferFlush(&iomcu.tx_io_packet, sizeof(iomcu.tx_io_packet)); setup_rx_dma(uart); #if AP_HAL_SHARED_DMA_ENABLED @@ -129,9 +180,15 @@ static void dma_tx_end_cb(hal_uart_driver *uart) // DMA stream has already been disabled at this point uart->usart->CR3 &= ~USART_CR3_DMAT; +#if defined(STM32H7) + (void)uart->usart->ICR; + (void)uart->usart->TDR; + (void)uart->usart->TDR; +#else (void)uart->usart->SR; (void)uart->usart->DR; (void)uart->usart->DR; +#endif #ifdef HAL_GPIO_LINE_GPIO108 TOGGLE_PIN_DEBUG(108); @@ -147,8 +204,31 @@ static void dma_tx_end_cb(hal_uart_driver *uart) /* replacement for ChibiOS uart_lld_serve_interrupt() */ static void idle_rx_handler(hal_uart_driver *uart) { +#if defined(STM32H7) + #define USART_SR_REG uart->usart->ISR + #define USART_DR_REG uart->usart->RDR + #define USART_SR_LBD USART_ISR_LBDF + #define USART_SR_ORE USART_ISR_ORE + #define USART_SR_NE USART_ISR_NE + #define USART_SR_FE USART_ISR_FE + #define USART_SR_PE USART_ISR_PE + #define USART_SR_TC USART_ISR_TC + #define USART_SR_IDLE USART_ISR_IDLE + #define CLEAR_ISR_FLAG(flag) uart->usart->ICR = USART_ICR_##flag##CF + #define SEND_BREAK() uart->usart->RQR = USART_RQR_SBKRQ + #define CLEAR_IDLE() uart->usart->ICR = USART_ICR_IDLECF + #define CLEAR_ERRORS() uart->usart->ICR = USART_ICR_ORECF | USART_ICR_NECF | USART_ICR_FECF | USART_ICR_PECF +#else + #define USART_SR_REG uart->usart->SR + #define USART_DR_REG uart->usart->DR + #define CLEAR_ISR_FLAG(flag) uart->usart->SR = ~USART_SR_##flag + #define SEND_BREAK() uart->usart->CR1 = cr1 | USART_CR1_SBK + #define CLEAR_IDLE() (void)uart->usart->DR + #define CLEAR_ERRORS() (void)USART_DR_REG +#endif + volatile uint16_t sr; - sr = uart->usart->SR; /* SR reset step 1.*/ + sr = USART_SR_REG; /* SR reset step 1.*/ uint32_t cr1 = uart->usart->CR1; if (sr & (USART_SR_LBD | USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ @@ -156,13 +236,11 @@ static void idle_rx_handler(hal_uart_driver *uart) USART_SR_FE | USART_SR_PE)) { /* framing error - start/stop bit lost or line break */ - (void)uart->usart->DR; /* SR reset step 2 - clear ORE | FE.*/ - + CLEAR_ERRORS(); /* SR reset step 2 - clear ORE | FE.*/ /* send a line break - this will abort transmission/reception on the other end */ chSysLockFromISR(); - uart->usart->SR = ~USART_SR_LBD; - uart->usart->CR1 = cr1 | USART_CR1_SBK; - + CLEAR_ISR_FLAG(LBD); + SEND_BREAK(); iomcu.reg_status.num_errors++; iomcu.reg_status.err_uart++; @@ -176,7 +254,7 @@ static void idle_rx_handler(hal_uart_driver *uart) if ((sr & USART_SR_TC) && (cr1 & USART_CR1_TCIE)) { /* TC interrupt cleared and disabled.*/ - uart->usart->SR &= ~USART_SR_TC; + CLEAR_ISR_FLAG(TC); uart->usart->CR1 = cr1 & ~USART_CR1_TCIE; #ifdef HAL_GPIO_LINE_GPIO105 TOGGLE_PIN_DEBUG(105); @@ -187,7 +265,7 @@ static void idle_rx_handler(hal_uart_driver *uart) } if ((sr & USART_SR_IDLE) && (cr1 & USART_CR1_IDLEIE)) { - (void)uart->usart->DR; /* SR reset step 2 - clear IDLE.*/ + CLEAR_IDLE(); /* SR reset step 2 - clear IDLE.*/ /* the DMA size is the maximum packet size, but smaller packets are perfectly possible leading to an IDLE ISR. The data still must be processed. */ @@ -200,6 +278,11 @@ static void idle_rx_handler(hal_uart_driver *uart) using namespace ChibiOS; #if AP_HAL_SHARED_DMA_ENABLED +#ifndef HAL_IO_FMU_COMMS_TX_DMA_STREAM +#define HAL_IO_FMU_COMMS_TX_DMA_STREAM STM32_UART_USART2_TX_DMA_STREAM +#define HAL_IO_FMU_COMMS_TX_IRQ_PRIORITY STM32_UART_USART2_IRQ_PRIORITY +#endif + /* copy of uart_lld_serve_tx_end_irq() from ChibiOS hal_uart_lld that is re-instated upon switching the DMA channel @@ -214,13 +297,13 @@ static void uart_lld_serve_tx_end_irq(hal_uart_driver *uart, uint32_t flags) void AP_IOMCU_FW::tx_dma_allocate(Shared_DMA *ctx) { - hal_uart_driver *uart = &UARTD2; + hal_uart_driver *uart = &HAL_IO_FMU_COMMS; chSysLock(); if (uart->dmatx == nullptr) { - uart->dmatx = dmaStreamAllocI(STM32_UART_USART2_TX_DMA_STREAM, - STM32_UART_USART2_IRQ_PRIORITY, - (stm32_dmaisr_t)uart_lld_serve_tx_end_irq, - (void *)uart); + uart->dmatx = dmaStreamAllocI(HAL_IO_FMU_COMMS_TX_DMA_STREAM, + HAL_IO_FMU_COMMS_TX_IRQ_PRIORITY, + (stm32_dmaisr_t)uart_lld_serve_tx_end_irq, + (void *)uart); } chSysUnlock(); } @@ -230,7 +313,7 @@ void AP_IOMCU_FW::tx_dma_allocate(Shared_DMA *ctx) */ void AP_IOMCU_FW::tx_dma_deallocate(Shared_DMA *ctx) { - hal_uart_driver *uart = &UARTD2; + hal_uart_driver *uart = &HAL_IO_FMU_COMMS; chSysLock(); if (uart->dmatx != nullptr) { // defensively make sure the DMA is fully shutdown before swapping @@ -255,28 +338,50 @@ static UARTConfig uart_cfg = { nullptr, // error idle_rx_handler, // global irq nullptr, // idle +#if defined(STM32H7) +0, // timeout +#endif 1500000, //1.5MBit - USART_CR1_IDLEIE, + USART_CR1_IDLEIE +#if defined(STM32H7) + | USART_CR1_FIFOEN +#endif + , 0, 0 }; +#ifdef IOMCU_DEBUG +// usart3 is for SBUS input and output +static const SerialConfig debug_serial_cfg = { + 2000000, // speed + 0, // cr1, enable even parity + 0, // cr2, two stop bits + 0, // cr3 + nullptr, // irq_cb + nullptr, // ctx +}; +#endif + void setup(void) { hal.rcin->init(); hal.rcout->init(); iomcu.init(); - iomcu.calculate_fw_crc(); +#ifdef IOMCU_DEBUG + sdStart(&IOMCU_DEBUG, &debug_serial_cfg); +#endif - uartStart(&UARTD2, &uart_cfg); - uartStartReceive(&UARTD2, sizeof(iomcu.rx_io_packet), &iomcu.rx_io_packet); + iomcu.calculate_fw_crc(); + uartStart(&HAL_IO_FMU_COMMS, &uart_cfg); + uartStartReceive(&HAL_IO_FMU_COMMS, sizeof(iomcu.rx_io_packet), &iomcu.rx_io_packet); #if AP_HAL_SHARED_DMA_ENABLED iomcu.tx_dma_handle->unlock(); #endif // disable the pieces from the UART which will get enabled later chSysLock(); - UARTD2.usart->CR3 &= ~USART_CR3_DMAT; + HAL_IO_FMU_COMMS.usart->CR3 &= ~USART_CR3_DMAT; chSysUnlock(); } @@ -303,7 +408,7 @@ void AP_IOMCU_FW::init() thread_ctx = chThdGetSelfX(); #if AP_HAL_SHARED_DMA_ENABLED - tx_dma_handle = NEW_NOTHROW ChibiOS::Shared_DMA(STM32_UART_USART2_TX_DMA_STREAM, SHARED_DMA_NONE, + tx_dma_handle = NEW_NOTHROW ChibiOS::Shared_DMA(HAL_IO_FMU_COMMS_TX_DMA_STREAM, SHARED_DMA_NONE, FUNCTOR_BIND_MEMBER(&AP_IOMCU_FW::tx_dma_allocate, void, Shared_DMA *), FUNCTOR_BIND_MEMBER(&AP_IOMCU_FW::tx_dma_deallocate, void, Shared_DMA *)); tx_dma_handle->lock(); @@ -311,6 +416,7 @@ void AP_IOMCU_FW::init() tx_dma_deallocate(tx_dma_handle); #endif +#ifdef HAL_GPIO_PIN_IO_HW_DETECT1 if (palReadLine(HAL_GPIO_PIN_IO_HW_DETECT1) == 1 && palReadLine(HAL_GPIO_PIN_IO_HW_DETECT2) == 0) { has_heater = true; } @@ -321,6 +427,9 @@ void AP_IOMCU_FW::init() } else { palSetLineMode(HAL_GPIO_PIN_HEATER, PAL_MODE_OUTPUT_OPENDRAIN); } +#else + has_heater = false; +#endif adc_init(); rcin_serial_init(); @@ -436,7 +545,7 @@ void AP_IOMCU_FW::update() // send a response if required if (mask & IOEVENT_TX_BEGIN) { chSysLock(); - setup_tx_dma(&UARTD2); + setup_tx_dma(&HAL_IO_FMU_COMMS); chSysUnlock(); } #endif @@ -447,7 +556,7 @@ void AP_IOMCU_FW::update() loop_counter++; if (do_reboot && (last_ms > reboot_time)) { - hal.scheduler->reboot(true); + hal.scheduler->reboot(false); while (true) {} } if ((mask & IOEVENT_PWM) || @@ -558,6 +667,7 @@ void AP_IOMCU_FW::pwm_out_update() void AP_IOMCU_FW::heater_update() { +#ifdef HAL_GPIO_PIN_HEATER uint32_t now = last_ms; if (!has_heater) { // use blue LED as heartbeat, run it 4x faster when override active @@ -576,6 +686,7 @@ void AP_IOMCU_FW::heater_update() bool heater_on = (get_random16() < uint32_t(reg_setup.heater_duty_cycle) * 0xFFFFU / 100U); HEATER_SET(heater_on? heater_pwm_polarity : !heater_pwm_polarity); } +#endif } void AP_IOMCU_FW::rcin_update() @@ -906,13 +1017,18 @@ bool AP_IOMCU_FW::handle_code_write() // we need to release the JTAG reset pin to be used as a GPIO, otherwise we can't enable // or disable SBUS out +#ifdef AFIO_MAPR_SWJ_CFG_NOJNTRST AFIO->MAPR = AFIO_MAPR_SWJ_CFG_NOJNTRST; - +#endif adc_disable_vrssi(); +#ifdef HAL_GPIO_PIN_SBUS_OUT_EN palClearLine(HAL_GPIO_PIN_SBUS_OUT_EN); +#endif } else { adc_enable_vrssi(); +#ifdef HAL_GPIO_PIN_SBUS_OUT_EN palSetLine(HAL_GPIO_PIN_SBUS_OUT_EN); +#endif } if (reg_setup.features & P_SETUP_FEATURES_HEATER) { has_heater = true; @@ -1067,13 +1183,11 @@ void AP_IOMCU_FW::schedule_reboot(uint32_t time_ms) void AP_IOMCU_FW::calculate_fw_crc(void) { -#define APP_SIZE_MAX 0xf000 -#define APP_LOAD_ADDRESS 0x08001000 // compute CRC of the current firmware uint32_t sum = 0; - for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { - uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); + for (unsigned p = 0; p < HAL_IOMCU_APP_SIZE_MAX; p += 4) { + uint32_t bytes = *(uint32_t *)(p + HAL_IOMCU_APP_LOAD_ADDRESS); sum = crc32_small(sum, (const uint8_t *)&bytes, sizeof(bytes)); } diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h index df67719f1ab58..4f4923736283e 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.h +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h @@ -185,7 +185,9 @@ class AP_IOMCU_FW { bool update_default_rate; bool update_rcout_freq; bool has_heater; +#ifdef IOMCU_IMU_HEATER_POLARITY const bool heater_pwm_polarity = IOMCU_IMU_HEATER_POLARITY; +#endif uint32_t last_blue_led_ms; uint32_t safety_update_ms; uint32_t safety_button_counter; @@ -203,8 +205,14 @@ class AP_IOMCU_FW { }; // GPIO macros +#ifdef HAL_GPIO_PIN_HEATER #define HEATER_SET(on) palWriteLine(HAL_GPIO_PIN_HEATER, (on)); #define BLUE_TOGGLE() palToggleLine(HAL_GPIO_PIN_HEATER); +#else +#define HEATER_SET(on) +#define BLUE_TOGGLE() +#endif + #define AMBER_SET(on) palWriteLine(HAL_GPIO_PIN_AMBER_LED, !(on)); #define SPEKTRUM_POWER(on) palWriteLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, on); #define SPEKTRUM_SET(on) palWriteLine(HAL_GPIO_PIN_SPEKTRUM_OUT, on); diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h index cc43005f042a1..c752c4fab3373 100644 --- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h +++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h @@ -21,7 +21,13 @@ //#define IOMCU_DEBUG -struct PACKED IOPacket { +struct +#if defined(STM32H7) +__attribute__((aligned(32), packed)) +#else +PACKED +#endif +IOPacket { uint8_t count:6; uint8_t code:2; uint8_t crc; diff --git a/libraries/AP_IOMCU/iofirmware/rc.cpp b/libraries/AP_IOMCU/iofirmware/rc.cpp index d654a910330ce..da611224afda0 100644 --- a/libraries/AP_IOMCU/iofirmware/rc.cpp +++ b/libraries/AP_IOMCU/iofirmware/rc.cpp @@ -27,27 +27,57 @@ extern const AP_HAL::HAL& hal; +#ifndef HAL_IOMCU_DSM_SERIAL_DRIVER +#define HAL_IOMCU_DSM_SERIAL_DRIVER SD1 +#endif + +#ifndef HAL_IOMCU_RCIN_SERIAL_DRIVER +#define HAL_IOMCU_RCIN_SERIAL_DRIVER SD3 +#endif + +#ifndef HAL_IOMCU_SBUS_OUT_SERIAL_DRIVER +#define HAL_IOMCU_SBUS_OUT_SERIAL_DRIVER SD3 +#endif + +#ifndef HAL_IOMCU_SEPARATE_SBUS_OUT_RCIN +#define HAL_IOMCU_SEPARATE_SBUS_OUT_RCIN 0 +#endif + // usart3 is for SBUS input and output static const SerialConfig sbus_cfg = { 100000, // speed USART_CR1_PCE | USART_CR1_M, // cr1, enable even parity - USART_CR2_STOP_1, // cr2, two stop bits + USART_CR2_STOP_1 // cr2, two stop bits +#if defined(HAL_SERIAL_SBUS_SWAPPED) && HAL_SERIAL_SBUS_SWAPPED + | USART_CR2_SWAP +#endif + , 0, // cr3 nullptr, // irq_cb nullptr, // ctx }; -// listen for parity errors on sd3 input -static event_listener_t sd3_listener; +// listen for parity errors on sbus out input +static event_listener_t serial_rcin_listener; -static uint8_t sd3_config; +static uint8_t sbus_out_config; + +#define STR(x) XSTR(x) +#define XSTR(x) #x + + +#if HAL_IOMCU_SEPARATE_SBUS_OUT_RCIN +static uint8_t serial_rcin_config; +#else +#define serial_rcin_config sbus_out_config +#endif void sbus_out_write(uint16_t *channels, uint8_t nchannels) { - if (sd3_config == 0) { + if (sbus_out_config == 0) { uint8_t buffer[25]; AP_SBusOut::sbus_format_frame(channels, nchannels, buffer); - chnWrite(&SD3, buffer, sizeof(buffer)); + chnWrite(&HAL_IOMCU_SBUS_OUT_SERIAL_DRIVER, buffer, sizeof(buffer)); } } @@ -72,10 +102,15 @@ static enum { */ void AP_IOMCU_FW::rcin_serial_init(void) { - sdStart(&SD1, &dsm_cfg); - sdStart(&SD3, &sbus_cfg); - chEvtRegisterMaskWithFlags(chnGetEventSource(&SD3), - &sd3_listener, + sdStart(&HAL_IOMCU_DSM_SERIAL_DRIVER, &dsm_cfg); + sdStart(&HAL_IOMCU_SBUS_OUT_SERIAL_DRIVER, &sbus_cfg); + +#if HAL_IOMCU_SEPARATE_SBUS_OUT_RCIN + sdStart(&HAL_IOMCU_RCIN_SERIAL_DRIVER, &sbus_cfg); +#endif + + chEvtRegisterMaskWithFlags(chnGetEventSource(&HAL_IOMCU_RCIN_SERIAL_DRIVER), + &serial_rcin_listener, EVENT_MASK(1), SD_PARITY_ERROR); // disable input for SBUS with pulses, we will use the UART for @@ -115,10 +150,10 @@ void AP_IOMCU_FW::rcin_serial_update(void) // read from DSM port if ((rc_state == RC_SEARCHING || rc_state == RC_DSM_PORT) && - (n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) { + (n = chnReadTimeout(&HAL_IOMCU_DSM_SERIAL_DRIVER, b, sizeof(b), TIME_IMMEDIATE)) > 0) { n = MIN(n, sizeof(b)); // don't mix two 115200 uarts - if (sd3_config == 0) { + if (serial_rcin_config == 0) { rc_stats.num_dsm_bytes += n; for (uint8_t i=0; i 0) { + (n = chnReadTimeout(&HAL_IOMCU_RCIN_SERIAL_DRIVER, b, sizeof(b), TIME_IMMEDIATE)) > 0) { eventflags_t flags; - if (sd3_config == 0 && ((flags = chEvtGetAndClearFlags(&sd3_listener)) & SD_PARITY_ERROR)) { + if (serial_rcin_config == 0 && ((flags = chEvtGetAndClearFlags(&serial_rcin_listener)) & SD_PARITY_ERROR)) { rc_stats.sbus_error = flags; rc_stats.num_sbus_errors++; } else { n = MIN(n, sizeof(b)); rc_stats.num_sbus_bytes += n; for (uint8_t i=0; i 2000 && (sd3_config==1 || !sbus_out_enabled)) { + now - rc_stats.last_good_ms > 2000 && (sbus_out_config==1 || !sbus_out_enabled)) { rc_stats.last_good_ms = now; - sd3_config ^= 1; - sdStop(&SD3); - sdStart(&SD3, sd3_config==0?&sbus_cfg:&dsm_cfg); + sbus_out_config ^= 1; + sdStop(&HAL_IOMCU_SBUS_OUT_SERIAL_DRIVER); + sdStart(&HAL_IOMCU_SBUS_OUT_SERIAL_DRIVER, sbus_out_config==0?&sbus_cfg:&dsm_cfg); } +#endif } /* diff --git a/libraries/AP_IOMCU/iofirmware/wscript b/libraries/AP_IOMCU/iofirmware/wscript index 445b95c0e1ee0..74e49ffc19cb0 100644 --- a/libraries/AP_IOMCU/iofirmware/wscript +++ b/libraries/AP_IOMCU/iofirmware/wscript @@ -20,16 +20,23 @@ def build(bld): ] ) - bld.ap_program( - program_name='iofirmware_lowpolh', - use='iofirmware_libs', - program_groups=['bin','iofirmware'], - defines=['IOMCU_IMU_HEATER_POLARITY=0'] - ) + if bld.env.IOMCU_FW_WITH_HEATER: + bld.ap_program( + program_name='iofirmware_lowpolh', + use='iofirmware_libs', + program_groups=['bin','iofirmware'], + defines=['IOMCU_IMU_HEATER_POLARITY=0'] + ) - bld.ap_program( - program_name='iofirmware_highpolh', - use='iofirmware_libs', - program_groups=['bin','iofirmware'], - defines=['IOMCU_IMU_HEATER_POLARITY=1'] - ) + bld.ap_program( + program_name='iofirmware_highpolh', + use='iofirmware_libs', + program_groups=['bin','iofirmware'], + defines=['IOMCU_IMU_HEATER_POLARITY=1'] + ) + else: + bld.ap_program( + program_name='iofirmware', + use='iofirmware_libs', + program_groups=['bin','iofirmware'] + ) diff --git a/libraries/AP_OSD/AP_OSD_ParamSetting.cpp b/libraries/AP_OSD/AP_OSD_ParamSetting.cpp index 7ec03159e4c59..985c1fd3762e0 100644 --- a/libraries/AP_OSD/AP_OSD_ParamSetting.cpp +++ b/libraries/AP_OSD/AP_OSD_ParamSetting.cpp @@ -139,7 +139,7 @@ static const char* SERIAL_PROTOCOL_VALUES[] = { "FSKY_TX", "LID360", "", "BEACN", "VOLZ", "SBUS", "ESC_TLM", "DEV_TLM", "OPTFLW", "RBTSRV", "NMEA", "WNDVNE", "SLCAN", "RCIN", "MGSQRT", "LTM", "RUNCAM", "HOT_TLM", "SCRIPT", "CRSF", "GEN", "WNCH", "MSP", "DJI", "AIRSPD", "ADSB", "AHRS", "AUDIO", "FETTEC", "TORQ", - "AIS", "CD_ESC", "MSP_DP", "MAV_HL", "TRAMP", "DDS", "IMUOUT", "IQ", "PPP", "IBUS_TLM" + "AIS", "CD_ESC", "MSP_DP", "MAV_HL", "TRAMP", "DDS", "IMUOUT", "IQ", "PPP", "IBUS_TLM", "IOMCU" }; static_assert(AP_SerialManager::SerialProtocol_NumProtocols == ARRAY_SIZE(SERIAL_PROTOCOL_VALUES), "Wrong size SerialProtocol_NumProtocols"); diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index 6f9f1541832c8..5f267784b2666 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -585,7 +585,9 @@ void AP_SerialManager::init() AP_SERIALMANAGER_PPP_BUFSIZE_TX); break; #endif - + case SerialProtocol_IOMCU: + // nothing to do, AP_IOMCU handles this + break; default: uart->begin(state[i].baudrate()); } diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h index e093e75825e2d..1be91ba2f9bed 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.h +++ b/libraries/AP_SerialManager/AP_SerialManager.h @@ -86,6 +86,7 @@ class AP_SerialManager { // Reserving Serial Protocol 47 for SerialProtocol_IQ SerialProtocol_PPP = 48, SerialProtocol_IBUS_Telem = 49, // i-BUS telemetry data, ie via sensor port of FS-iA6B + SerialProtocol_IOMCU = 50, // IOMCU SerialProtocol_NumProtocols // must be the last value }; diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index b4f002d52e6fa..48a8013551386 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -356,6 +356,13 @@ void AP_Vehicle::setup() #endif #if AP_SERIALMANAGER_ENABLED +#if HAL_WITH_IO_MCU + if (BoardConfig.io_enabled()) { + // If IO is enabled, we need to set the protocol to None + // as the IO MCU UART is handled by the AP_IOMCU class + serial_manager.set_protocol_and_baud(HAL_UART_IOMCU_IDX, AP_SerialManager::SerialProtocol_IOMCU, 0); + } +#endif // initialise serial ports serial_manager.init(); #endif diff --git a/modules/ChibiOS b/modules/ChibiOS index 88b84600b59d2..c6d0293e32d09 160000 --- a/modules/ChibiOS +++ b/modules/ChibiOS @@ -1 +1 @@ -Subproject commit 88b84600b59d2f39c9d9137aacdc9f098b374b4a +Subproject commit c6d0293e32d096dc88da861a0db40bfd1f6f8d9d