diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index 121e7a0e1..315560e43 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -40,14 +40,12 @@ typedef uint8_t ioConfig_t; // packed IO configuration #define IO_CONFIG(mode, speed) ((mode) | (speed)) #define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_2MHz) -#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_25MHz) #define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_Out_OD, GPIO_Speed_2MHz) #define IOCFG_AF_PP IO_CONFIG(GPIO_Mode_AF_PP, GPIO_Speed_2MHz) #define IOCFG_AF_OD IO_CONFIG(GPIO_Mode_AF_OD, GPIO_Speed_2MHz) #define IOCFG_IPD IO_CONFIG(GPIO_Mode_IPD, GPIO_Speed_2MHz) #define IOCFG_IPU IO_CONFIG(GPIO_Mode_IPU, GPIO_Speed_2MHz) #define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN_FLOATING, GPIO_Speed_2MHz) -#define IOCFG_IPU_25 IO_CONFIG(GPIO_Mode_IPU, GPIO_Speed_25MHz) #elif defined(STM32F3) || defined(STM32F4) diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 78cfd482a..5883772cf 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -72,6 +72,24 @@ #define UART3_RX_PINSOURCE GPIO_PinSource11 #endif +#ifndef UART4_GPIO +#define UART4_TX_PIN GPIO_Pin_10 // PC10 (AF5) +#define UART4_RX_PIN GPIO_Pin_11 // PC11 (AF5) +#define UART4_GPIO_AF GPIO_AF_5 +#define UART4_GPIO GPIOC +#define UART4_TX_PINSOURCE GPIO_PinSource10 +#define UART4_RX_PINSOURCE GPIO_PinSource11 +#endif + +#ifndef UART5_GPIO // The real UART5_RX is on PD2, no board is using. +#define UART5_TX_PIN GPIO_Pin_12 // PC12 (AF5) +#define UART5_RX_PIN GPIO_Pin_12 // PC12 (AF5) +#define UART5_GPIO_AF GPIO_AF_5 +#define UART5_GPIO GPIOC +#define UART5_TX_PINSOURCE GPIO_PinSource12 +#define UART5_RX_PINSOURCE GPIO_PinSource12 +#endif + #ifdef USE_USART1 static uartPort_t uartPort1; #endif @@ -81,6 +99,12 @@ static uartPort_t uartPort2; #ifdef USE_USART3 static uartPort_t uartPort3; #endif +#ifdef USE_USART4 +static uartPort_t uartPort4; +#endif +#ifdef USE_USART5 +static uartPort_t uartPort5; +#endif static void handleUsartTxDma(dmaChannelDescriptor_t* descriptor) { @@ -328,6 +352,124 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode, portOptions_t optio } #endif +#ifdef USE_USART4 +uartPort_t *serialUSART4(uint32_t baudRate, portMode_t mode, portOptions_t options) +{ + uartPort_t *s; + static volatile uint8_t rx4Buffer[UART4_RX_BUFFER_SIZE]; + static volatile uint8_t tx4Buffer[UART4_TX_BUFFER_SIZE]; + NVIC_InitTypeDef NVIC_InitStructure; + GPIO_InitTypeDef GPIO_InitStructure; + + s = &uartPort4; + s->port.vTable = uartVTable; + + s->port.baudRate = baudRate; + + s->port.rxBufferSize = UART4_RX_BUFFER_SIZE; + s->port.txBufferSize = UART4_TX_BUFFER_SIZE; + s->port.rxBuffer = rx4Buffer; + s->port.txBuffer = tx4Buffer; + + s->USARTx = UART4; + + RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART4, ENABLE); + + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = (options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP; + + if (options & SERIAL_BIDIR) { + GPIO_InitStructure.GPIO_Pin = UART4_TX_PIN; + GPIO_InitStructure.GPIO_OType = (options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD; + GPIO_PinAFConfig(UART4_GPIO, UART4_TX_PINSOURCE, UART4_GPIO_AF); + GPIO_Init(UART4_GPIO, &GPIO_InitStructure); + if(!(options & SERIAL_INVERTED)) + GPIO_SetBits(UART4_GPIO, UART4_TX_PIN); // OpenDrain output should be inactive + } else { + if (mode & MODE_TX) { + GPIO_InitStructure.GPIO_Pin = UART4_TX_PIN; + GPIO_PinAFConfig(UART4_GPIO, UART4_TX_PINSOURCE, UART4_GPIO_AF); + GPIO_Init(UART4_GPIO, &GPIO_InitStructure); + } + + if (mode & MODE_RX) { + GPIO_InitStructure.GPIO_Pin = UART4_RX_PIN; + GPIO_PinAFConfig(UART4_GPIO, UART4_RX_PINSOURCE, UART4_GPIO_AF); + GPIO_Init(UART4_GPIO, &GPIO_InitStructure); + } + } + + NVIC_InitStructure.NVIC_IRQChannel = UART4_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART4); + NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART4); + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + return s; +} +#endif + +#ifdef USE_USART5 +uartPort_t *serialUSART5(uint32_t baudRate, portMode_t mode, portOptions_t options) +{ + uartPort_t *s; + static volatile uint8_t rx5Buffer[UART5_RX_BUFFER_SIZE]; + static volatile uint8_t tx5Buffer[UART5_TX_BUFFER_SIZE]; + NVIC_InitTypeDef NVIC_InitStructure; + GPIO_InitTypeDef GPIO_InitStructure; + + s = &uartPort5; + s->port.vTable = uartVTable; + + s->port.baudRate = baudRate; + + s->port.rxBufferSize = UART5_RX_BUFFER_SIZE; + s->port.txBufferSize = UART5_TX_BUFFER_SIZE; + s->port.rxBuffer = rx5Buffer; + s->port.txBuffer = tx5Buffer; + + s->USARTx = UART5; + + RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE); + + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = (options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP; + + if (options & SERIAL_BIDIR) { + GPIO_InitStructure.GPIO_Pin = UART5_TX_PIN; + GPIO_InitStructure.GPIO_OType = (options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD; + GPIO_PinAFConfig(UART5_GPIO, UART5_TX_PINSOURCE, UART5_GPIO_AF); + GPIO_Init(UART5_GPIO, &GPIO_InitStructure); + if(!(options & SERIAL_INVERTED)) + GPIO_SetBits(UART5_GPIO, UART5_TX_PIN); // OpenDrain output should be inactive + } else { + if (mode & MODE_TX) { + GPIO_InitStructure.GPIO_Pin = UART5_TX_PIN; + GPIO_PinAFConfig(UART5_GPIO, UART5_TX_PINSOURCE, UART5_GPIO_AF); + GPIO_Init(UART5_GPIO, &GPIO_InitStructure); + } + + if (mode & MODE_RX) { + GPIO_InitStructure.GPIO_Pin = UART5_RX_PIN; + GPIO_PinAFConfig(UART5_GPIO, UART5_RX_PINSOURCE, UART5_GPIO_AF); + GPIO_Init(UART5_GPIO, &GPIO_InitStructure); + } + } + + NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART5); + NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART5); + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + return s; +} +#endif + void usartIrqHandler(uartPort_t *s) { uint32_t ISR = s->USARTx->ISR; @@ -386,3 +528,21 @@ void USART3_IRQHandler(void) usartIrqHandler(s); } #endif + +#ifdef USE_USART4 +void UART4_IRQHandler(void) +{ + uartPort_t *s = &uartPort4; + + usartIrqHandler(s); +} +#endif + +#ifdef USE_USART5 +void UART5_IRQHandler(void) +{ + uartPort_t *s = &uartPort5; + + usartIrqHandler(s); +} +#endif diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 92e17f8a8..c6d58aa74 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -42,6 +42,7 @@ void systemReset(void); void systemResetToBootloader(void); bool isMPUSoftReset(void); void cycleCounterInit(void); +void checkForBootLoaderRequest(void); void enableGPIOPowerUsageAndNoiseReductions(void); // current crystal frequency - 8 or 12MHz diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index 4ad526850..3e59fe734 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -37,7 +37,8 @@ void systemReset(void) SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } -void systemResetToBootloader(void) { +void systemResetToBootloader(void) +{ // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC @@ -68,6 +69,8 @@ bool isMPUSoftReset(void) void systemInit(void) { + checkForBootLoaderRequest(); + SetSysClock(false); #ifdef CC3D @@ -110,3 +113,6 @@ void systemInit(void) SysTick_Config(SystemCoreClock / 1000); } +void checkForBootLoaderRequest(void) +{ +} \ No newline at end of file diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index 7e58ab061..ee8aef1a0 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -35,7 +35,8 @@ void systemReset(void) SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } -void systemResetToBootloader(void) { +void systemResetToBootloader(void) +{ // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC @@ -82,6 +83,8 @@ bool isMPUSoftReset(void) void systemInit(void) { + checkForBootLoaderRequest(); + // Enable FPU SCB->CPACR = (0x3 << (10 * 2)) | (0x3 << (11 * 2)); SetSysClock(); @@ -102,3 +105,7 @@ void systemInit(void) // SysTick SysTick_Config(SystemCoreClock / 1000); } + +void checkForBootLoaderRequest(void) +{ +} diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index f0042026f..53f3767a5 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -169,6 +169,8 @@ bool isMPUSoftReset(void) void systemInit(void) { + checkForBootLoaderRequest(); + SetSysClock(); // Configure NVIC preempt/priority groups @@ -194,3 +196,18 @@ void systemInit(void) SysTick_Config(SystemCoreClock / 1000); } +void(*bootJump)(void); +void checkForBootLoaderRequest(void) +{ + if (*((uint32_t *)0x2001FFFC) == 0xDEADBEEF) { + + *((uint32_t *)0x2001FFFC) = 0x0; + + __enable_irq(); + __set_MSP(0x20001000); + + bootJump = (void(*)(void))(*((uint32_t *) 0x1fff0004)); + bootJump(); + while (1); + } +} \ No newline at end of file diff --git a/src/main/io/serial.c b/src/main/io/serial.c index b2effbb4b..6fbdad7c0 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -316,7 +316,7 @@ serialPort_t *openSerialPort( #endif #ifdef USE_USART5 case SERIAL_PORT_USART5: - serialPort = uartOpen(USART5, callback, baudRate, mode, options); + serialPort = uartOpen(UART5, callback, baudRate, mode, options); break; #endif #ifdef USE_USART6 diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 49c580541..bcea2f8a1 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -120,12 +120,12 @@ void setEscLo(uint8_t selEsc) void setEscInput(uint8_t selEsc) { - IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU_25); + IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU); } void setEscOutput(uint8_t selEsc) { - IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP_25); + IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP); } // Initialize 4way ESC interface diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a51f828ed..e160869df 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -112,6 +112,7 @@ static void cliRxFail(char *cmdline); static void cliAdjustmentRange(char *cmdline); static void cliMotorMix(char *cmdline); static void cliDefaults(char *cmdline); +void cliDfu(char *cmdLine); static void cliDump(char *cmdLine); void cliDumpProfile(uint8_t profileIndex); void cliDumpRateProfile(uint8_t rateProfileIndex) ; @@ -122,6 +123,7 @@ static void cliPlaySound(char *cmdline); static void cliProfile(char *cmdline); static void cliRateProfile(char *cmdline); static void cliReboot(void); +static void cliRebootEx(bool bootLoader); static void cliSave(char *cmdline); static void cliSerial(char *cmdline); #ifndef SKIP_SERIAL_PASSTHROUGH @@ -263,8 +265,8 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), - CLI_COMMAND_DEF("dump", "dump configuration", - "[master|profile]", cliDump), + CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), + CLI_COMMAND_DEF("dump", "dump configuration", "[master|profile]", cliDump), CLI_COMMAND_DEF("exit", NULL, NULL, cliExit), CLI_COMMAND_DEF("feature", "configure features", "list\r\n" @@ -2564,10 +2566,19 @@ static void cliRateProfile(char *cmdline) { static void cliReboot(void) { - cliPrint("\r\nRebooting"); + cliRebootEx(false); +} + +static void cliRebootEx(bool bootLoader) +{ + cliPrint("\r\nRebooting"); bufWriterFlush(cliWriter); waitForSerialPortToFinishTransmitting(cliPort); stopMotors(); + if (bootLoader) { + systemResetToBootloader(); + return; + } systemReset(); } @@ -3107,6 +3118,13 @@ static void cliResource(char *cmdline) } } +void cliDfu(char *cmdLine) +{ + UNUSED(cmdLine); + cliPrint("\r\nRestarting in DFU mode"); + cliRebootEx(true); +} + void cliInit(serialConfig_t *serialConfig) { UNUSED(serialConfig); diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 5c5e7795f..98387ee23 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -170,6 +170,7 @@ #define DEFAULT_FEATURES (FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 +#define SERIALRX_UART SERIAL_PORT_USART3 #define USE_SERIAL_4WAY_BLHELI_INTERFACE