diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index eff736d050..837d107bbb 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -33,8 +33,8 @@ #include "drivers/buf_writer.h" #include "drivers/io.h" #include "drivers/serial.h" +#include "drivers/time.h" #include "drivers/timer.h" -#include "drivers/pwm_output.h" #include "drivers/light_led.h" #include "flight/mixer.h" @@ -76,9 +76,9 @@ // *** change to adapt Revision #define SERIAL_4WAY_VER_MAIN 20 #define SERIAL_4WAY_VER_SUB_1 (uint8_t) 0 -#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 03 +#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 04 -#define SERIAL_4WAY_PROTOCOL_VER 107 +#define SERIAL_4WAY_PROTOCOL_VER 108 // *** end #if (SERIAL_4WAY_VER_MAIN > 24) @@ -139,7 +139,6 @@ uint8_t esc4wayInit(void) // StopPwmAllMotors(); // XXX Review effect of motor refactor //pwmDisableMotors(); - motorDisable(); escCount = 0; memset(&escHardware, 0, sizeof(escHardware)); pwmOutputPort_t *pwmMotors = pwmGetMotors(); @@ -153,6 +152,7 @@ uint8_t esc4wayInit(void) } } } + motorDisable(); return escCount; } @@ -566,9 +566,13 @@ void esc4wayProcess(serialPort_t *mspPort) case cmd_DeviceReset: { + bool rebootEsc = false; if (ParamBuf[0] < escCount) { // Channel may change here selected_esc = ParamBuf[0]; + if (ioMem.D_FLASH_ADDR_L == 1) { + rebootEsc = true; + } } else { ACK_OUT = ACK_I_INVALID_CHANNEL; @@ -582,6 +586,14 @@ void esc4wayProcess(serialPort_t *mspPort) case imARM_BLB: { BL_SendCMDRunRestartBootloader(&DeviceInfo); + if (rebootEsc) { + ESC_OUTPUT; + setEscLo(selected_esc); + timeMs_t m = millis(); + while (millis() - m < 300); + setEscHi(selected_esc); + ESC_INPUT; + } break; } #endif