diff --git a/src/modules/src/serial_4way.c b/src/modules/src/serial_4way.c index 109f109e3..1668840c2 100644 --- a/src/modules/src/serial_4way.c +++ b/src/modules/src/serial_4way.c @@ -33,6 +33,10 @@ #define USE_SERIAL_4WAY_BLHELI_BOOTLOADER #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +//FreeRTOS includes +#include "FreeRTOS.h" +#include "task.h" + #include "motors.h" #include "usec_time.h" #include "led.h" @@ -153,6 +157,18 @@ uint8_t esc4wayInit(void) escCount++; } + // Let ESCs enter bootloader mode + vTaskDelay(M2T(200)); + + // Make them stay in bootloader mode. + for (volatile uint8_t i = 0; i < NBR_OF_MOTORS; i++) { + motorsESCSetOutput(i); + motorsESCSetLo(i); + vTaskDelay(M2T(1)); + motorsESCSetInput(i); + motorsESCSetHi(i); + } + return escCount; }