diff --git a/Makefile b/Makefile
index fab52754f4..b5ff167038 100644
--- a/Makefile
+++ b/Makefile
@@ -306,6 +306,9 @@ COMMON_SRC = build_config.c \
io/serial.c \
io/serial_1wire.c \
io/serial_1wire_vcp.c \
+ io/serial_4way.c \
+ io/serial_4way_avrootloader.c \
+ io/serial_4way_stk500v2.c \
io/serial_cli.c \
io/serial_msp.c \
io/statusindicator.c \
diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c
new file mode 100644
index 0000000000..4be6c1364a
--- /dev/null
+++ b/src/main/io/serial_4way.c
@@ -0,0 +1,775 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ * Author: 4712
+*/
+#include "platform.h"
+#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
+#include
+#include
+#include
+#include
+#include
+#include "drivers/system.h"
+#include "drivers/pwm_output.h"
+#include "flight/mixer.h"
+#include "io/beeper.h"
+#include "io/serial_4way.h"
+#include "io/serial_msp.h"
+#include "drivers/buf_writer.h"
+
+#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#include "io/serial_4way_avrootloader.h"
+#endif
+#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
+#include "io/serial_4way_stk500v2.h"
+#endif
+
+#define SERIAL_4WAY_INTERFACE_NAME_STR "m4wFCIntf"
+// *** change to adapt Revision
+#define SERIAL_4WAY_VER_MAIN 14
+#define SERIAL_4WAY_VER_SUB_1 (uint8_t) 4
+#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 03
+
+#define SERIAL_4WAY_PROTOCOL_VER 106
+// *** end
+
+#if (SERIAL_4WAY_VER_MAIN > 24)
+#error "beware of SERIAL_4WAY_VER_SUB_1 is uint8_t"
+#endif
+
+
+#define SERIAL_4WAY_VERSION (uint16_t) ((SERIAL_4WAY_VER_MAIN * 1000) + (SERIAL_4WAY_VER_SUB_1 * 100) + SERIAL_4WAY_VER_SUB_2)
+
+#define SERIAL_4WAY_VERSION_HI (uint8_t) (SERIAL_4WAY_VERSION / 100)
+#define SERIAL_4WAY_VERSION_LO (uint8_t) (SERIAL_4WAY_VERSION % 100)
+
+static uint8_t escCount;
+
+static uint32_t GetPinPos(uint32_t pin)
+{
+ uint32_t pinPos;
+ for (pinPos = 0; pinPos < 16; pinPos++) {
+ uint32_t pinMask = (0x1 << pinPos);
+ if (pin & pinMask) {
+ return pinPos;
+ }
+ }
+ return 0;
+}
+
+uint8_t Initialize4WayInterface(void)
+{
+ // StopPwmAllMotors();
+ pwmDisableMotors();
+ selected_esc = 0;
+ memset(&escHardware, 0, sizeof(escHardware));
+ pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration();
+ for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) {
+ if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) {
+ if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) {
+ escHardware[selected_esc].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio;
+ escHardware[selected_esc].pin = pwmOutputConfiguration->portConfigurations[i].timerHardware->pin;
+ escHardware[selected_esc].pinpos = GetPinPos(escHardware[selected_esc].pin);
+ escHardware[selected_esc].gpio_config_INPUT.pin = escHardware[selected_esc].pin;
+ escHardware[selected_esc].gpio_config_INPUT.speed = Speed_2MHz; // see pwmOutConfig()
+ escHardware[selected_esc].gpio_config_INPUT.mode = Mode_IPU;
+ escHardware[selected_esc].gpio_config_OUTPUT = escHardware[selected_esc].gpio_config_INPUT;
+ escHardware[selected_esc].gpio_config_OUTPUT.mode = Mode_Out_PP;
+ ESC_INPUT;
+ ESC_SET_HI;
+ selected_esc++;
+ }
+ }
+ }
+ escCount = selected_esc;
+ return escCount;
+}
+
+void DeInitialize4WayInterface(void)
+{
+ for (selected_esc = 0; selected_esc < (escCount); selected_esc++) {
+ escHardware[selected_esc].gpio_config_OUTPUT.mode = Mode_AF_PP; // see pwmOutConfig()
+ ESC_OUTPUT;
+ ESC_SET_LO;
+ }
+ escCount = 0;
+ pwmEnableMotors();
+}
+
+
+#define SET_DISCONNECTED DeviceInfo.words[0] = 0
+
+#define INTF_MODE_IDX 3 // index for DeviceInfostate
+
+// Interface related only
+// establish and test connection to the Interface
+
+// Send Structure
+// ESC + CMD PARAM_LEN [PARAM (if len > 0)] CRC16_Hi CRC16_Lo
+// Return
+// ESC CMD PARAM_LEN [PARAM (if len > 0)] + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo
+
+#define cmd_Remote_Escape 0x2E // '.'
+#define cmd_Local_Escape 0x2F // '/'
+
+// Test Interface still present
+#define cmd_InterfaceTestAlive 0x30 // '0' alive
+// RETURN: ACK
+
+// get Protocol Version Number 01..255
+#define cmd_ProtocolGetVersion 0x31 // '1' version
+// RETURN: uint8_t VersionNumber + ACK
+
+// get Version String
+#define cmd_InterfaceGetName 0x32 // '2' name
+// RETURN: String + ACK
+
+//get Version Number 01..255
+#define cmd_InterfaceGetVersion 0x33 // '3' version
+// RETURN: uint8_t AVersionNumber + ACK
+
+
+// Exit / Restart Interface - can be used to switch to Box Mode
+#define cmd_InterfaceExit 0x34 // '4' exit
+// RETURN: ACK
+
+// Reset the Device connected to the Interface
+#define cmd_DeviceReset 0x35 // '5' reset
+// RETURN: ACK
+
+// Get the Device ID connected
+// #define cmd_DeviceGetID 0x36 //'6' device id removed since 06/106
+// RETURN: uint8_t DeviceID + ACK
+
+// Initialize Flash Access for Device connected
+#define cmd_DeviceInitFlash 0x37 // '7' init flash access
+// RETURN: ACK
+
+// Erase the whole Device Memory of connected Device
+#define cmd_DeviceEraseAll 0x38 // '8' erase all
+// RETURN: ACK
+
+// Erase one Page of Device Memory of connected Device
+#define cmd_DevicePageErase 0x39 // '9' page erase
+// PARAM: uint8_t APageNumber
+// RETURN: ACK
+
+// Read to Buffer from Device Memory of connected Device // Buffer Len is Max 256 Bytes
+// BuffLen = 0 means 256 Bytes
+#define cmd_DeviceRead 0x3A // ':' read Device
+// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255]
+// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK
+
+// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes
+// BuffLen = 0 means 256 Bytes
+#define cmd_DeviceWrite 0x3B // ';' write
+// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255]
+// RETURN: ACK
+
+// Set C2CK low infinite ) permanent Reset state
+#define cmd_DeviceC2CK_LOW 0x3C // '<'
+// RETURN: ACK
+
+// Read to Buffer from Device Memory of connected Device //Buffer Len is Max 256 Bytes
+// BuffLen = 0 means 256 Bytes
+#define cmd_DeviceReadEEprom 0x3D // '=' read Device
+// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255]
+// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK
+
+// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes
+// BuffLen = 0 means 256 Bytes
+#define cmd_DeviceWriteEEprom 0x3E // '>' write
+// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255]
+// RETURN: ACK
+
+// Set Interface Mode
+#define cmd_InterfaceSetMode 0x3F // '?'
+// #define imC2 0
+// #define imSIL_BLB 1
+// #define imATM_BLB 2
+// #define imSK 3
+// PARAM: uint8_t Mode
+// RETURN: ACK or ACK_I_INVALID_CHANNEL
+
+// responses
+#define ACK_OK 0x00
+// #define ACK_I_UNKNOWN_ERROR 0x01
+#define ACK_I_INVALID_CMD 0x02
+#define ACK_I_INVALID_CRC 0x03
+#define ACK_I_VERIFY_ERROR 0x04
+// #define ACK_D_INVALID_COMMAND 0x05
+// #define ACK_D_COMMAND_FAILED 0x06
+// #define ACK_D_UNKNOWN_ERROR 0x07
+
+#define ACK_I_INVALID_CHANNEL 0x08
+#define ACK_I_INVALID_PARAM 0x09
+#define ACK_D_GENERAL_ERROR 0x0F
+
+/* Copyright (c) 2002, 2003, 2004 Marek Michalkiewicz
+ Copyright (c) 2005, 2007 Joerg Wunsch
+ Copyright (c) 2013 Dave Hylands
+ Copyright (c) 2013 Frederic Nadeau
+ All rights reserved.
+
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in
+ the documentation and/or other materials provided with the
+ distribution.
+
+ * Neither the name of the copyright holders nor the names of
+ contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ POSSIBILITY OF SUCH DAMAGE. */
+uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) {
+ int i;
+
+ crc = crc ^ ((uint16_t)data << 8);
+ for (i=0; i < 8; i++){
+ if (crc & 0x8000)
+ crc = (crc << 1) ^ 0x1021;
+ else
+ crc <<= 1;
+ }
+ return crc;
+}
+// * End copyright
+
+
+#define ATMEL_DEVICE_MATCH ((DeviceInfo.words[0] == 0x9307) || (DeviceInfo.words[0] == 0x930A) || \
+ (DeviceInfo.words[0] == 0x930F) || (DeviceInfo.words[0] == 0x940B))
+
+#define SILABS_DEVICE_MATCH ((DeviceInfo.words[0] == 0xF310)||(DeviceInfo.words[0] ==0xF330) || \
+ (DeviceInfo.words[0] == 0xF410) || (DeviceInfo.words[0] == 0xF390) || \
+ (DeviceInfo.words[0] == 0xF850) || (DeviceInfo.words[0] == 0xE8B1) || \
+ (DeviceInfo.words[0] == 0xE8B2))
+
+static uint8_t CurrentInterfaceMode;
+
+static uint8_t Connect(void)
+{
+ for (uint8_t I = 0; I < 3; ++I) {
+ #if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
+ if (Stk_ConnectEx() && ATMEL_DEVICE_MATCH) {
+ CurrentInterfaceMode = imSK;
+ return 1;
+ } else {
+ if (BL_ConnectEx()) {
+ if SILABS_DEVICE_MATCH {
+ CurrentInterfaceMode = imSIL_BLB;
+ return 1;
+ } else if ATMEL_DEVICE_MATCH {
+ CurrentInterfaceMode = imATM_BLB;
+ return 1;
+ }
+ }
+ }
+ #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
+ if (BL_ConnectEx()) {
+ if SILABS_DEVICE_MATCH {
+ CurrentInterfaceMode = imSIL_BLB;
+ return 1;
+ } else if ATMEL_DEVICE_MATCH {
+ CurrentInterfaceMode = imATM_BLB;
+ return 1;
+ }
+ }
+ #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
+ if (Stk_ConnectEx()) {
+ CurrentInterfaceMode = imSK;
+ if ATMEL_DEVICE_MATCH return 1;
+ }
+ #endif
+ }
+ return 0;
+}
+
+static mspPort_t *_mspPort;
+static bufWriter_t *_writer;
+
+static uint8_t ReadByte(void) {
+ // need timeout?
+ while (!serialRxBytesWaiting(_mspPort->port));
+ return serialRead(_mspPort->port);
+}
+
+static union uint8_16u CRC_in;
+static uint8_t ReadByteCrc(void) {
+ uint8_t b = ReadByte();
+ CRC_in.word = _crc_xmodem_update(CRC_in.word, b);
+ return b;
+}
+static void WriteByte(uint8_t b){
+ bufWriterAppend(_writer, b);
+}
+
+static union uint8_16u CRCout;
+static void WriteByteCrc(uint8_t b){
+ WriteByte(b);
+ CRCout.word = _crc_xmodem_update(CRCout.word, b);
+}
+
+void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter) {
+
+ uint8_t ParamBuf[256];
+ uint8_t ESC;
+ uint8_t I_PARAM_LEN;
+ uint8_t CMD;
+ uint8_t ACK_OUT;
+ union uint8_16u CRC_check;
+ union uint8_16u Dummy;
+ uint8_t O_PARAM_LEN;
+ uint8_t *O_PARAM;
+ uint8_t *InBuff;
+
+ _mspPort = mspPort;
+ _writer = bufwriter;
+
+ // Start here with UART Main loop
+ #ifdef BEEPER
+ // fix for buzzer often starts beeping continuously when the ESCs are read
+ // switch beeper silent here
+ // TODO (4712) check: is beeperSilence useful here?
+ beeperSilence();
+ #endif
+ bool isExitScheduled = false;
+
+ while(1) {
+ // restart looking for new sequence from host
+ do {
+ CRC_in.word = 0;
+ ESC = ReadByteCrc();
+ } while (ESC != cmd_Local_Escape);
+
+ RX_LED_ON;
+
+ Dummy.word = 0;
+ O_PARAM = &Dummy.bytes[0];
+ O_PARAM_LEN = 1;
+ CMD = ReadByteCrc();
+ D_FLASH_ADDR_H = ReadByteCrc();
+ D_FLASH_ADDR_L = ReadByteCrc();
+ I_PARAM_LEN = ReadByteCrc();
+
+ InBuff = ParamBuf;
+ uint8_t i = I_PARAM_LEN;
+ do {
+ *InBuff = ReadByteCrc();
+ InBuff++;
+ i--;
+ } while (i != 0);
+
+ CRC_check.bytes[1] = ReadByte();
+ CRC_check.bytes[0] = ReadByte();
+
+ RX_LED_OFF;
+
+ if(CRC_check.word == CRC_in.word) {
+ ACK_OUT = ACK_OK;
+ } else {
+ ACK_OUT = ACK_I_INVALID_CRC;
+ }
+
+ if (ACK_OUT == ACK_OK)
+ {
+ // D_FLASH_ADDR_H=Adress_H;
+ // D_FLASH_ADDR_L=Adress_L;
+ D_PTR_I = ParamBuf;
+
+ switch(CMD) {
+ // ******* Interface related stuff *******
+ case cmd_InterfaceTestAlive:
+ {
+ break;
+ if (IsMcuConnected){
+ switch(CurrentInterfaceMode)
+ {
+ #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+ case imATM_BLB:
+ case imSIL_BLB:
+ {
+ if (!BL_SendCMDKeepAlive()) { // SetStateDisconnected() included
+ ACK_OUT = ACK_D_GENERAL_ERROR;
+ }
+ break;
+ }
+ #endif
+ #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
+ case imSK:
+ {
+ if (!Stk_SignOn()) { // SetStateDisconnected();
+ ACK_OUT = ACK_D_GENERAL_ERROR;
+ }
+ break;
+ }
+ #endif
+ default:
+ ACK_OUT = ACK_D_GENERAL_ERROR;
+ }
+ if ( ACK_OUT != ACK_OK) SET_DISCONNECTED;
+ }
+ break;
+ }
+ case cmd_ProtocolGetVersion:
+ {
+ // Only interface itself, no matter what Device
+ Dummy.bytes[0] = SERIAL_4WAY_PROTOCOL_VER;
+ break;
+ }
+
+ case cmd_InterfaceGetName:
+ {
+ // Only interface itself, no matter what Device
+ // O_PARAM_LEN=16;
+ O_PARAM_LEN = strlen(SERIAL_4WAY_INTERFACE_NAME_STR);
+ O_PARAM = (uint8_t *)SERIAL_4WAY_INTERFACE_NAME_STR;
+ break;
+ }
+
+ case cmd_InterfaceGetVersion:
+ {
+ // Only interface itself, no matter what Device
+ // Dummy = iUart_res_InterfVersion;
+ O_PARAM_LEN = 2;
+ Dummy.bytes[0] = SERIAL_4WAY_VERSION_HI;
+ Dummy.bytes[1] = SERIAL_4WAY_VERSION_LO;
+ break;
+ }
+ case cmd_InterfaceExit:
+ {
+ isExitScheduled = true;
+ break;
+ }
+ case cmd_InterfaceSetMode:
+ {
+ #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
+ if ((ParamBuf[0] <= imSK) && (ParamBuf[0] >= imSIL_BLB)) {
+ #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
+ if ((ParamBuf[0] <= imATM_BLB) && (ParamBuf[0] >= imSIL_BLB)) {
+ #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
+ if (ParamBuf[0] == imSK) {
+ #endif
+ CurrentInterfaceMode = ParamBuf[0];
+ } else {
+ ACK_OUT = ACK_I_INVALID_PARAM;
+ }
+ break;
+ }
+
+ case cmd_DeviceReset:
+ {
+ if (ParamBuf[0] < escCount) {
+ // Channel may change here
+ selected_esc = ParamBuf[0];
+ }
+ else {
+ ACK_OUT = ACK_I_INVALID_CHANNEL;
+ break;
+ }
+ switch (CurrentInterfaceMode)
+ {
+ case imSIL_BLB:
+ #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+ case imATM_BLB:
+ {
+ BL_SendCMDRunRestartBootloader();
+ break;
+ }
+ #endif
+ #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
+ case imSK:
+ {
+ break;
+ }
+ #endif
+ }
+ SET_DISCONNECTED;
+ break;
+ }
+ case cmd_DeviceInitFlash:
+ {
+ SET_DISCONNECTED;
+ if (ParamBuf[0] < escCount) {
+ //Channel may change here
+ //ESC_LO or ESC_HI; Halt state for prev channel
+ selected_esc = ParamBuf[0];
+ } else {
+ ACK_OUT = ACK_I_INVALID_CHANNEL;
+ break;
+ }
+ O_PARAM_LEN = DeviceInfoSize; //4
+ O_PARAM = (uint8_t *)&DeviceInfo;
+ if(Connect()) {
+ DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode;
+ } else {
+ SET_DISCONNECTED;
+ ACK_OUT = ACK_D_GENERAL_ERROR;
+ }
+ break;
+ }
+
+ #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
+ case cmd_DeviceEraseAll:
+ {
+ switch(CurrentInterfaceMode)
+ {
+ case imSK:
+ {
+ if (!Stk_Chip_Erase()) ACK_OUT=ACK_D_GENERAL_ERROR;
+ break;
+ }
+ default:
+ ACK_OUT = ACK_I_INVALID_CMD;
+ }
+ break;
+ }
+ #endif
+
+ #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+ case cmd_DevicePageErase:
+ {
+ switch (CurrentInterfaceMode)
+ {
+ case imSIL_BLB:
+ {
+ Dummy.bytes[0] = ParamBuf[0];
+ //Address = Page * 512
+ D_FLASH_ADDR_H = (Dummy.bytes[0] << 1);
+ D_FLASH_ADDR_L = 0;
+ if (!BL_PageErase()) ACK_OUT = ACK_D_GENERAL_ERROR;
+ break;
+ }
+ default:
+ ACK_OUT = ACK_I_INVALID_CMD;
+ }
+ break;
+ }
+ #endif
+
+ //*** Device Memory Read Ops ***
+ case cmd_DeviceRead:
+ {
+ D_NUM_BYTES = ParamBuf[0];
+ /*
+ D_FLASH_ADDR_H=Adress_H;
+ D_FLASH_ADDR_L=Adress_L;
+ D_PTR_I = BUF_I;
+ */
+ switch(CurrentInterfaceMode)
+ {
+ #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+ case imSIL_BLB:
+ case imATM_BLB:
+ {
+ if(!BL_ReadFlash(CurrentInterfaceMode))
+ {
+ ACK_OUT = ACK_D_GENERAL_ERROR;
+ }
+ break;
+ }
+ #endif
+ #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
+ case imSK:
+ {
+ if(!Stk_ReadFlash())
+ {
+ ACK_OUT = ACK_D_GENERAL_ERROR;
+ }
+ break;
+ }
+ #endif
+ default:
+ ACK_OUT = ACK_I_INVALID_CMD;
+ }
+ if (ACK_OUT == ACK_OK)
+ {
+ O_PARAM_LEN = D_NUM_BYTES;
+ O_PARAM = (uint8_t *)&ParamBuf;
+ }
+ break;
+ }
+
+ case cmd_DeviceReadEEprom:
+ {
+ D_NUM_BYTES = ParamBuf[0];
+ /*
+ D_FLASH_ADDR_H = Adress_H;
+ D_FLASH_ADDR_L = Adress_L;
+ D_PTR_I = BUF_I;
+ */
+ switch (CurrentInterfaceMode)
+ {
+ #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+ case imATM_BLB:
+ {
+ if (!BL_ReadEEprom())
+ {
+ ACK_OUT = ACK_D_GENERAL_ERROR;
+ }
+ break;
+ }
+ #endif
+ #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
+ case imSK:
+ {
+ if (!Stk_ReadEEprom())
+ {
+ ACK_OUT = ACK_D_GENERAL_ERROR;
+ }
+ break;
+ }
+ #endif
+ default:
+ ACK_OUT = ACK_I_INVALID_CMD;
+ }
+ if(ACK_OUT == ACK_OK)
+ {
+ O_PARAM_LEN = D_NUM_BYTES;
+ O_PARAM = (uint8_t *)&ParamBuf;
+ }
+ break;
+ }
+
+ //*** Device Memory Write Ops ***
+ case cmd_DeviceWrite:
+ {
+ D_NUM_BYTES = I_PARAM_LEN;
+ /*
+ D_FLASH_ADDR_H=Adress_H;
+ D_FLASH_ADDR_L=Adress_L;
+ D_PTR_I = BUF_I;
+ */
+ switch (CurrentInterfaceMode)
+ {
+ #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+ case imSIL_BLB:
+ case imATM_BLB:
+ {
+ if (!BL_WriteFlash()) {
+ ACK_OUT = ACK_D_GENERAL_ERROR;
+ }
+ break;
+ }
+ #endif
+ #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
+ case imSK:
+ {
+ if (!Stk_WriteFlash())
+ {
+ ACK_OUT = ACK_D_GENERAL_ERROR;
+ }
+ break;
+ }
+ #endif
+ }
+ break;
+ }
+
+ case cmd_DeviceWriteEEprom:
+ {
+ D_NUM_BYTES = I_PARAM_LEN;
+ ACK_OUT = ACK_D_GENERAL_ERROR;
+ /*
+ D_FLASH_ADDR_H=Adress_H;
+ D_FLASH_ADDR_L=Adress_L;
+ D_PTR_I = BUF_I;
+ */
+ switch (CurrentInterfaceMode)
+ {
+ #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+ case imSIL_BLB:
+ {
+ ACK_OUT = ACK_I_INVALID_CMD;
+ break;
+ }
+ case imATM_BLB:
+ {
+ if (BL_WriteEEprom())
+ {
+ ACK_OUT = ACK_OK;
+ }
+ break;
+ }
+ #endif
+ #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
+ case imSK:
+ {
+ if (Stk_WriteEEprom())
+ {
+ ACK_OUT = ACK_OK;
+ }
+ break;
+ }
+ #endif
+ }
+ break;
+ }
+ default:
+ {
+ ACK_OUT = ACK_I_INVALID_CMD;
+ }
+ }
+ }
+
+ CRCout.word = 0;
+
+ TX_LED_ON;
+ serialBeginWrite(_mspPort->port);
+ WriteByteCrc(cmd_Remote_Escape);
+ WriteByteCrc(CMD);
+ WriteByteCrc(D_FLASH_ADDR_H);
+ WriteByteCrc(D_FLASH_ADDR_L);
+ WriteByteCrc(O_PARAM_LEN);
+
+ i=O_PARAM_LEN;
+ do {
+ WriteByteCrc(*O_PARAM);
+ O_PARAM++;
+ i--;
+ } while (i > 0);
+
+ WriteByteCrc(ACK_OUT);
+ WriteByte(CRCout.bytes[1]);
+ WriteByte(CRCout.bytes[0]);
+ serialEndWrite(_mspPort->port);
+ bufWriterFlush(_writer);
+ TX_LED_OFF;
+ if (isExitScheduled) {
+ DeInitialize4WayInterface();
+ return;
+ }
+ };
+}
+
+
+
+#endif
diff --git a/src/main/io/serial_4way.h b/src/main/io/serial_4way.h
new file mode 100644
index 0000000000..853b120a29
--- /dev/null
+++ b/src/main/io/serial_4way.h
@@ -0,0 +1,95 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ * Author: 4712
+*/
+
+#include
+#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
+#include "drivers/serial.h"
+#include "drivers/buf_writer.h"
+#include "drivers/gpio.h"
+#include "drivers/timer.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/light_led.h"
+#include "io/serial_msp.h"
+
+
+#define imC2 0
+#define imSIL_BLB 1
+#define imATM_BLB 2
+#define imSK 3
+
+#define RX_LED_OFF LED0_OFF
+#define RX_LED_ON LED0_ON
+#define TX_LED_OFF LED1_OFF
+#define TX_LED_ON LED1_ON
+
+typedef struct {
+ GPIO_TypeDef* gpio;
+ uint16_t pinpos;
+ uint16_t pin;
+ gpio_config_t gpio_config_INPUT;
+ gpio_config_t gpio_config_OUTPUT;
+} escHardware_t;
+
+uint8_t selected_esc;
+
+escHardware_t escHardware[MAX_PWM_MOTORS];
+
+#define ESC_IS_HI digitalIn(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) != Bit_RESET
+#define ESC_IS_LO digitalIn(escHardware[selected_esc].gpio, escHardware[selected_esc].pin) == Bit_RESET
+#define ESC_SET_HI digitalHi(escHardware[selected_esc].gpio, escHardware[selected_esc].pin)
+#define ESC_SET_LO digitalLo(escHardware[selected_esc].gpio, escHardware[selected_esc].pin)
+
+#define ESC_INPUT gpioInit(escHardware[selected_esc].gpio, &escHardware[selected_esc].gpio_config_INPUT)
+#define ESC_OUTPUT gpioInit(escHardware[selected_esc].gpio, &escHardware[selected_esc].gpio_config_OUTPUT)
+
+
+
+void delay_us(uint32_t delay);
+
+union __attribute__ ((packed)) uint8_16u
+{
+ uint8_t bytes[2];
+ uint16_t word;
+};
+
+union __attribute__ ((packed)) uint8_32u
+{
+ uint8_t bytes[4];
+ uint16_t words[2];
+ uint32_t dword;
+};
+
+//-----------------------------------------------------------------------------------
+// Global VARIABLES
+//-----------------------------------------------------------------------------------
+uint8_t D_NUM_BYTES;
+uint8_t D_FLASH_ADDR_H;
+uint8_t D_FLASH_ADDR_L;
+uint8_t *D_PTR_I;
+
+#define DeviceInfoSize 4
+
+union uint8_32u DeviceInfo;
+
+#define IsMcuConnected (DeviceInfo.bytes[0] > 0)
+
+uint8_t Initialize4WayInterface(void);
+void Process4WayInterface(mspPort_t *mspPort, bufWriter_t *bufwriter);
+void DeInitialize4WayInterface(void);
+
+#endif
diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c
new file mode 100644
index 0000000000..a2c2f68905
--- /dev/null
+++ b/src/main/io/serial_4way_avrootloader.c
@@ -0,0 +1,363 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ * Author: 4712
+ * for info about Hagens AVRootloader:
+ * http://www.mikrocontroller.net/topic/avr-bootloader-mit-verschluesselung
+*/
+
+#include
+#include
+#include
+#include
+#include
+
+#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#include "drivers/system.h"
+#include "io/serial_4way_avrootloader.h"
+#include "io/serial_4way.h"
+
+// Bootloader commands
+// RunCmd
+#define RestartBootloader 0
+#define ExitBootloader 1
+
+#define CMD_RUN 0x00
+#define CMD_PROG_FLASH 0x01
+#define CMD_ERASE_FLASH 0x02
+#define CMD_READ_FLASH_SIL 0x03
+#define CMD_VERIFY_FLASH 0x03
+#define CMD_READ_EEPROM 0x04
+#define CMD_PROG_EEPROM 0x05
+#define CMD_READ_SRAM 0x06
+#define CMD_READ_FLASH_ATM 0x07
+#define CMD_KEEP_ALIVE 0xFD
+#define CMD_SET_ADDRESS 0xFF
+#define CMD_SET_BUFFER 0xFE
+
+#define CMD_BOOTINIT 0x07
+#define CMD_BOOTSIGN 0x08
+
+// Bootloader result codes
+
+#define brSUCCESS 0x30
+#define brERRORCOMMAND 0xC1
+#define brERRORCRC 0xC2
+#define brNONE 0xFF
+
+static union uint8_16u CRC_16;
+
+static uint8_t cb;
+static uint8_t suart_timeout;
+
+#define WaitStartBitTimeoutms 2
+
+#define BitTime (52) //52uS
+#define BitHalfTime (BitTime >> 1) //26uS
+#define StartBitTime (BitHalfTime + 1)
+#define StopBitTime ((BitTime * 9) + BitHalfTime)
+
+
+
+static uint8_t suart_getc_(void)
+{
+ uint8_t bt=0;
+ uint32_t btime;
+ uint32_t bstop;
+ uint32_t start_time;
+
+ suart_timeout = 1;
+ uint32_t wait_time = millis() + WaitStartBitTimeoutms;
+ while (ESC_IS_HI) {
+ // check for Startbit begin
+ if (millis() >= wait_time) {
+ return 0;
+ }
+ }
+ // Startbit
+ start_time = micros();
+
+ btime = start_time + StartBitTime;
+ bstop= start_time + StopBitTime;
+
+ while (micros() < btime);
+ if (ESC_IS_HI) {
+ return 0;
+ }
+ for (uint8_t bit = 0; bit < 8; bit++)
+ {
+ btime = btime + BitTime;
+ while (micros() < btime);
+ if (ESC_IS_HI)
+ {
+ bt |= (1 << bit);
+ }
+ }
+ while (micros() < bstop);
+ // check Stoppbit
+ if (ESC_IS_LO) {
+ return 0;
+ }
+ suart_timeout = 0;
+ return (bt);
+}
+
+static void suart_putc_(uint8_t TXbyte)
+{
+ uint32_t btime;
+ ESC_SET_LO; // Set low = StartBit
+ btime = BitTime + micros();
+ while (micros() < btime);
+ for(uint8_t bit = 0; bit < 8; bit++)
+ {
+ if(TXbyte & 1)
+ {
+ ESC_SET_HI; // 1
+ }
+ else
+ {
+ ESC_SET_LO; // 0
+ }
+ btime = btime + BitTime;
+ TXbyte = (TXbyte >> 1);
+ while (micros() < btime);
+ }
+ ESC_SET_HI; //Set high = Stoppbit
+ btime = btime + BitTime;
+ while (micros() < btime);
+}
+
+static union uint8_16u LastCRC_16;
+
+static void ByteCrc(void)
+{
+ for (uint8_t i = 0; i < 8; i++)
+ {
+ if (((cb & 0x01) ^ (CRC_16.word & 0x0001)) !=0 )
+ {
+ CRC_16.word = CRC_16.word >> 1;
+ CRC_16.word = CRC_16.word ^ 0xA001;
+ }
+ else
+ {
+ CRC_16.word = CRC_16.word >> 1;
+ }
+ cb = cb >> 1;
+ }
+}
+
+static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len)
+{
+ //Todo CRC in case of timeout?
+ //len 0 means 256
+ CRC_16.word = 0;
+ LastCRC_16.word = 0;
+ uint8_t LastACK = brNONE;
+ do {
+ cb = suart_getc_();
+ if(suart_timeout) goto timeout;
+ *pstring = cb;
+ ByteCrc();
+ pstring++;
+ len--;
+ } while(len > 0);
+
+ if(IsMcuConnected) {
+ //With CRC read 3 more
+ LastCRC_16.bytes[0] = suart_getc_();
+ if(suart_timeout) goto timeout;
+ LastCRC_16.bytes[1] = suart_getc_();
+ if(suart_timeout) goto timeout;
+ LastACK = suart_getc_();
+ if (CRC_16.word != LastCRC_16.word) {
+ LastACK = brERRORCRC;
+ }
+ } else {
+ //TODO check here LastACK
+ LastACK = suart_getc_();
+ }
+timeout:
+ return (LastACK == brSUCCESS);
+}
+
+static void BL_SendBuf(uint8_t *pstring, uint8_t len)
+{
+ ESC_OUTPUT;
+ // wait some us
+ delayMicroseconds(50);
+ CRC_16.word=0;
+ do {
+ cb = *pstring;
+ pstring++;
+ suart_putc_(cb);
+ ByteCrc();
+ len--;
+ } while (len > 0);
+
+ if (IsMcuConnected) {
+ suart_putc_(CRC_16.bytes[0]);
+ suart_putc_(CRC_16.bytes[1]);
+ }
+ ESC_INPUT;
+}
+
+uint8_t BL_ConnectEx(void)
+{
+ #define BootMsgLen 4
+ #define DevSignHi (BootMsgLen)
+ #define DevSignLo (BootMsgLen+1)
+
+ //DeviceInfo.dword=0; is set before
+ uint8_t BootInfo[9];
+ uint8_t BootMsg[BootMsgLen-1] = "471";
+ // x * 0 + 9
+#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
+ uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
+ BL_SendBuf(BootInit, 21);
+#else
+ uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
+ BL_SendBuf(BootInit, 17);
+#endif
+ if (!BL_ReadBuf(BootInfo, BootMsgLen + 4)) {
+ return 0;
+ }
+ // BootInfo has no CRC (ACK byte already analyzed... )
+ // Format = BootMsg("471c") SIGNATURE_001, SIGNATURE_002, BootVersion (always 6), BootPages (,ACK)
+ for (uint8_t i = 0; i < (BootMsgLen - 1); i++) { // Check only the first 3 letters -> 471x OK
+ if (BootInfo[i] != BootMsg[i]) {
+ return (0);
+ }
+ }
+
+ //only 2 bytes used $1E9307 -> 0x9307
+ DeviceInfo.bytes[2] = BootInfo[BootMsgLen - 1];
+ DeviceInfo.bytes[1] = BootInfo[DevSignHi];
+ DeviceInfo.bytes[0] = BootInfo[DevSignLo];
+ return (1);
+}
+
+static uint8_t BL_GetACK(uint32_t Timeout)
+{
+ uint8_t LastACK;
+ do {
+ LastACK = suart_getc_();
+ Timeout--;
+ } while ((suart_timeout) && (Timeout));
+
+ if(suart_timeout) {
+ LastACK = brNONE;
+ }
+ return (LastACK);
+}
+
+
+uint8_t BL_SendCMDKeepAlive(void)
+{
+ uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0};
+ BL_SendBuf(sCMD, 2);
+ if (BL_GetACK(1) != brERRORCOMMAND) {
+ return 0;
+ }
+ return 1;
+}
+
+void BL_SendCMDRunRestartBootloader(void)
+{
+ uint8_t sCMD[] = {RestartBootloader, 0};
+ DeviceInfo.bytes[0] = 1;
+ BL_SendBuf(sCMD, 2); //sends simply 4 x 0x00 (CRC =00)
+ return;
+}
+
+static uint8_t BL_SendCMDSetAddress(void) //supports only 16 bit Adr
+{
+ // skip if adr == 0xFFFF
+ if((D_FLASH_ADDR_H == 0xFF) && (D_FLASH_ADDR_L == 0xFF)) return 1;
+ uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, D_FLASH_ADDR_H, D_FLASH_ADDR_L };
+ BL_SendBuf(sCMD, 4);
+ return (BL_GetACK(2) == brSUCCESS);
+}
+
+static uint8_t BL_SendCMDSetBuffer(void)
+{
+ uint8_t sCMD[] = {CMD_SET_BUFFER, 0, 0, D_NUM_BYTES};
+ if (D_NUM_BYTES == 0) {
+ // set high byte
+ sCMD[2] = 1;
+ }
+ BL_SendBuf(sCMD, 4);
+ if (BL_GetACK(2) != brNONE) return 0;
+ BL_SendBuf(D_PTR_I, D_NUM_BYTES);
+ return (BL_GetACK(40) == brSUCCESS);
+}
+
+static uint8_t BL_ReadA(uint8_t cmd)
+{
+ if (BL_SendCMDSetAddress()) {
+ uint8_t sCMD[] = {cmd, D_NUM_BYTES};
+ BL_SendBuf(sCMD, 2);
+ return (BL_ReadBuf(D_PTR_I, D_NUM_BYTES ));
+ }
+ return 0;
+}
+
+static uint8_t BL_WriteA(uint8_t cmd, uint32_t timeout)
+{
+ if (BL_SendCMDSetAddress()) {
+ if (!BL_SendCMDSetBuffer()) return 0;
+ uint8_t sCMD[] = {cmd, 0x01};
+ BL_SendBuf(sCMD, 2);
+ return (BL_GetACK(timeout) == brSUCCESS);
+ }
+ return 0;
+}
+
+
+uint8_t BL_ReadFlash(uint8_t interface_mode)
+{
+ if(interface_mode == imATM_BLB) {
+ return BL_ReadA(CMD_READ_FLASH_ATM);
+ } else {
+ return BL_ReadA(CMD_READ_FLASH_SIL);
+ }
+}
+
+
+uint8_t BL_ReadEEprom(void)
+{
+ return BL_ReadA(CMD_READ_EEPROM);
+}
+
+uint8_t BL_PageErase(void)
+{
+ if (BL_SendCMDSetAddress()) {
+ uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01};
+ BL_SendBuf(sCMD, 2);
+ return (BL_GetACK((40 / WaitStartBitTimeoutms)) == brSUCCESS);
+ }
+ return 0;
+}
+
+uint8_t BL_WriteEEprom(void)
+{
+ return BL_WriteA(CMD_PROG_EEPROM, (3000 / WaitStartBitTimeoutms));
+}
+
+uint8_t BL_WriteFlash(void)
+{
+ return BL_WriteA(CMD_PROG_FLASH, (40 / WaitStartBitTimeoutms));
+}
+
+#endif
diff --git a/src/main/io/serial_4way_avrootloader.h b/src/main/io/serial_4way_avrootloader.h
new file mode 100644
index 0000000000..6c4f4d1436
--- /dev/null
+++ b/src/main/io/serial_4way_avrootloader.h
@@ -0,0 +1,34 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ * Author: 4712
+ * for info about Hagens AVRootloader:
+ * http://www.mikrocontroller.net/topic/avr-bootloader-mit-verschluesselung
+*/
+
+#pragma once
+
+#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
+
+void BL_SendBootInit(void);
+uint8_t BL_ConnectEx(void);
+uint8_t BL_SendCMDKeepAlive(void);
+uint8_t BL_PageErase(void);
+uint8_t BL_ReadEEprom(void);
+uint8_t BL_WriteEEprom(void);
+uint8_t BL_WriteFlash(void);
+uint8_t BL_ReadFlash(uint8_t interface_mode);
+void BL_SendCMDRunRestartBootloader(void);
+#endif
diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c
new file mode 100644
index 0000000000..2bd0c6f7cb
--- /dev/null
+++ b/src/main/io/serial_4way_stk500v2.c
@@ -0,0 +1,430 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ * Author: 4712
+ * have a look at https://github.com/sim-/tgy/blob/master/boot.inc
+ * for info about the stk500v2 implementation
+ */
+#include
+#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
+#include
+#include
+#include
+#include
+#include "io/serial_4way_stk500v2.h"
+#include "drivers/system.h"
+#include "io/serial_4way.h"
+#include "config/config.h"
+
+#define BIT_LO_US (32) //32uS
+#define BIT_HI_US (2*BIT_LO_US)
+
+static uint8_t StkInBuf[16];
+
+#define STK_BIT_TIMEOUT 250 // micro seconds
+#define STK_WAIT_TICKS (1000 / STK_BIT_TIMEOUT) // per ms
+#define STK_WAITCYLCES (STK_WAIT_TICKS * 35) // 35ms
+#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5 ms
+#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) //5 s
+
+#define WaitPinLo while (ESC_IS_HI) {if (micros() > timeout_timer) goto timeout;}
+#define WaitPinHi while (ESC_IS_LO) {if (micros() > timeout_timer) goto timeout;}
+
+static uint32_t LastBitTime;
+static uint32_t HiLoTsh;
+
+static uint8_t SeqNumber;
+static uint8_t StkCmd;
+static uint8_t ckSumIn;
+static uint8_t ckSumOut;
+
+// used STK message constants from ATMEL AVR - Application note
+#define MESSAGE_START 0x1B
+#define TOKEN 0x0E
+
+#define CMD_SIGN_ON 0x01
+#define CMD_LOAD_ADDRESS 0x06
+#define CMD_CHIP_ERASE_ISP 0x12
+#define CMD_PROGRAM_FLASH_ISP 0x13
+#define CMD_READ_FLASH_ISP 0x14
+#define CMD_PROGRAM_EEPROM_ISP 0x15
+#define CMD_READ_EEPROM_ISP 0x16
+#define CMD_READ_SIGNATURE_ISP 0x1B
+#define CMD_SPI_MULTI 0x1D
+
+#define STATUS_CMD_OK 0x00
+
+#define CmdFlashEepromRead 0xA0
+#define EnterIspCmd1 0xAC
+#define EnterIspCmd2 0x53
+#define signature_r 0x30
+
+#define delay_us(x) delayMicroseconds(x)
+#define IRQ_OFF // dummy
+#define IRQ_ON // dummy
+
+static void StkSendByte(uint8_t dat)
+{
+ ckSumOut ^= dat;
+ for (uint8_t i = 0; i < 8; i++)
+ {
+ if (dat & 0x01)
+ {
+ // 1-bits are encoded as 64.0us high, 72.8us low (135.8us total).
+ ESC_SET_HI;
+ delay_us(BIT_HI_US);
+ ESC_SET_LO;
+ delay_us(BIT_HI_US);
+ }
+ else
+ {
+ // 0-bits are encoded as 27.8us high, 34.5us low, 34.4us high, 37.9 low (134.6us total)
+ ESC_SET_HI;
+ delay_us(BIT_LO_US);
+ ESC_SET_LO;
+ delay_us(BIT_LO_US);
+ ESC_SET_HI;
+ delay_us(BIT_LO_US);
+ ESC_SET_LO;
+ delay_us(BIT_LO_US);
+ }
+ dat >>= 1;
+ }
+}
+
+static void StkSendPacketHeader(void)
+{
+ IRQ_OFF;
+ ESC_OUTPUT;
+ StkSendByte(0xFF);
+ StkSendByte(0xFF);
+ StkSendByte(0x7F);
+ ckSumOut=0;
+ StkSendByte(MESSAGE_START);
+ StkSendByte(++SeqNumber);
+}
+
+static void StkSendPacketFooter(void)
+{
+ StkSendByte(ckSumOut);
+ ESC_SET_HI;
+ delay_us(BIT_LO_US);
+ ESC_INPUT;
+ IRQ_ON;
+}
+
+
+
+static int8_t ReadBit(void)
+{
+ uint32_t btimer = micros();
+ uint32_t timeout_timer = btimer + STK_BIT_TIMEOUT;
+ WaitPinLo;
+ WaitPinHi;
+ LastBitTime = micros() - btimer;
+ if (LastBitTime <= HiLoTsh)
+ {
+ timeout_timer = timeout_timer + STK_BIT_TIMEOUT;
+ WaitPinLo;
+ WaitPinHi;
+ //lo-bit
+ return 0;
+ }
+ else return 1;
+timeout:
+ return -1;
+}
+
+static uint8_t ReadByte(uint8_t *bt)
+{
+ *bt = 0;
+ for (uint8_t i = 0; i < 8; i++)
+ {
+ int8_t bit = ReadBit();
+ if (bit == -1) goto timeout;
+ if (bit == 1) {
+ *bt |= (1 << i);
+ }
+ }
+ ckSumIn ^=*bt;
+ return 1;
+timeout:
+ return 0;
+}
+
+
+
+static uint8_t StkReadLeader(void)
+{
+
+ // Reset learned timing
+ HiLoTsh = BIT_HI_US + BIT_LO_US;
+
+ // Wait for the first bit
+ uint32_t waitcycl; //250uS each
+
+ if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP))
+ {
+ waitcycl = STK_WAITCYLCES_EXT;
+ }
+ else if(StkCmd == CMD_SIGN_ON)
+ {
+ waitcycl = STK_WAITCYLCES_START;
+ }
+ else
+ {
+ waitcycl= STK_WAITCYLCES;
+ }
+ for ( ; waitcycl >0 ; waitcycl--)
+ {
+ //check is not timeout
+ if (ReadBit() >- 1) break;
+ }
+
+ //Skip the first bits
+ if (waitcycl == 0)
+ {
+ goto timeout;
+ }
+
+ for (uint8_t i = 0; i < 10; i++)
+ {
+ if (ReadBit() == -1) goto timeout;
+ }
+
+ // learn timing
+ HiLoTsh = (LastBitTime >> 1) + (LastBitTime >> 2);
+
+ // Read until we get a 0 bit
+ int8_t bit;
+ do
+ {
+ bit = ReadBit();
+ if (bit == -1) goto timeout;
+ } while (bit > 0);
+ return 1;
+timeout:
+ return 0;
+}
+
+static uint8_t StkRcvPacket(uint8_t *pstring)
+{
+ uint8_t bt = 0;
+ union uint8_16u Len;
+
+ IRQ_OFF;
+ if (!StkReadLeader()) goto Err;
+ ckSumIn=0;
+ if (!ReadByte(&bt) || (bt != MESSAGE_START)) goto Err;
+ if (!ReadByte(&bt) || (bt != SeqNumber)) goto Err;
+ ReadByte(&Len.bytes[1]);
+ if (Len.bytes[1] > 1) goto Err;
+ ReadByte(&Len.bytes[0]);
+ if (Len.bytes[0] < 1) goto Err;
+ if (!ReadByte(&bt) || (bt != TOKEN)) goto Err;
+ if (!ReadByte(&bt) || (bt != StkCmd)) goto Err;
+ if (!ReadByte(&bt) || (bt != STATUS_CMD_OK)) goto Err;
+ for (uint16_t i = 0; i < (Len.word - 2); i++)
+ {
+ if (!ReadByte(pstring)) goto Err;
+ pstring++;
+ }
+ ReadByte(&bt);
+ if (ckSumIn != 0) goto Err;
+ IRQ_ON;
+ return 1;
+Err:
+ IRQ_ON;
+ return 0;
+}
+
+static uint8_t _CMD_SPI_MULTI_EX(volatile uint8_t * ResByte,uint8_t Cmd,uint8_t AdrHi,uint8_t AdrLo)
+{
+ StkCmd= CMD_SPI_MULTI;
+ StkSendPacketHeader();
+ StkSendByte(0); // hi byte Msg len
+ StkSendByte(8); // lo byte Msg len
+ StkSendByte(TOKEN);
+ StkSendByte(CMD_SPI_MULTI);
+ StkSendByte(4); // NumTX
+ StkSendByte(4); // NumRX
+ StkSendByte(0); // RxStartAdr
+ StkSendByte(Cmd); // {TxData} Cmd
+ StkSendByte(AdrHi); // {TxData} AdrHi
+ StkSendByte(AdrLo); // {TxData} AdrLoch
+ StkSendByte(0); // {TxData} 0
+ StkSendPacketFooter();
+ if (StkRcvPacket((void *)StkInBuf)) { // NumRX + 3
+ if ((StkInBuf[0] == 0x00) && ((StkInBuf[1] == Cmd)||(StkInBuf[1] == 0x00)/* ignore zero returns */) &&(StkInBuf[2] == 0x00)) {
+ *ResByte = StkInBuf[3];
+ }
+ return 1;
+ }
+ return 0;
+}
+
+static uint8_t _CMD_LOAD_ADDRESS(void)
+{
+ // ignore 0xFFFF
+ // assume address is set before and we read or write the immediately following package
+ if((D_FLASH_ADDR_H == 0xFF) && (D_FLASH_ADDR_L == 0xFF)) return 1;
+ StkCmd = CMD_LOAD_ADDRESS;
+ StkSendPacketHeader();
+ StkSendByte(0); // hi byte Msg len
+ StkSendByte(5); // lo byte Msg len
+ StkSendByte(TOKEN);
+ StkSendByte(CMD_LOAD_ADDRESS);
+ StkSendByte(0);
+ StkSendByte(0);
+ StkSendByte(D_FLASH_ADDR_H);
+ StkSendByte(D_FLASH_ADDR_L);
+ StkSendPacketFooter();
+ return (StkRcvPacket((void *)StkInBuf));
+}
+
+static uint8_t _CMD_READ_MEM_ISP()
+{
+ uint8_t LenHi;
+ if (D_NUM_BYTES>0) {
+ LenHi=0;
+ } else {
+ LenHi=1;
+ }
+ StkSendPacketHeader();
+ StkSendByte(0); // hi byte Msg len
+ StkSendByte(4); // lo byte Msg len
+ StkSendByte(TOKEN);
+ StkSendByte(StkCmd);
+ StkSendByte(LenHi);
+ StkSendByte(D_NUM_BYTES);
+ StkSendByte(CmdFlashEepromRead);
+ StkSendPacketFooter();
+ return (StkRcvPacket(D_PTR_I));
+}
+
+static uint8_t _CMD_PROGRAM_MEM_ISP(void)
+{
+ union uint8_16u Len;
+ uint8_t LenLo=D_NUM_BYTES;
+ uint8_t LenHi;
+ if (LenLo) {
+ LenHi=0;
+ Len.word=LenLo+10;
+ } else {
+ LenHi=1;
+ Len.word=256+10;
+ }
+ StkSendPacketHeader();
+ StkSendByte(Len.bytes[1]); // high byte Msg len
+ StkSendByte(Len.bytes[0]); // low byte Msg len
+ StkSendByte(TOKEN);
+ StkSendByte(StkCmd);
+ StkSendByte(LenHi);
+ StkSendByte(LenLo);
+ StkSendByte(0); // mode
+ StkSendByte(0); // delay
+ StkSendByte(0); // cmd1
+ StkSendByte(0); // cmd2
+ StkSendByte(0); // cmd3
+ StkSendByte(0); // poll1
+ StkSendByte(0); // poll2
+ do {
+ StkSendByte(*D_PTR_I);
+ D_PTR_I++;
+ LenLo--;
+ } while (LenLo);
+ StkSendPacketFooter();
+ return StkRcvPacket((void *)StkInBuf);
+}
+
+uint8_t Stk_SignOn(void)
+{
+ StkCmd=CMD_SIGN_ON;
+ StkSendPacketHeader();
+ StkSendByte(0); // hi byte Msg len
+ StkSendByte(1); // lo byte Msg len
+ StkSendByte(TOKEN);
+ StkSendByte(CMD_SIGN_ON);
+ StkSendPacketFooter();
+ return (StkRcvPacket((void *) StkInBuf));
+}
+
+uint8_t Stk_ConnectEx(void)
+{
+ if (Stk_SignOn()) {
+ if (_CMD_SPI_MULTI_EX(&DeviceInfo.bytes[1],signature_r,0,1)) {
+ if (_CMD_SPI_MULTI_EX(&DeviceInfo.bytes[0],signature_r,0,2)) {
+ return 1;
+ }
+ }
+ }
+ return 0;
+}
+
+uint8_t Stk_Chip_Erase(void)
+{
+ StkCmd = CMD_CHIP_ERASE_ISP;
+ StkSendPacketHeader();
+ StkSendByte(0); // high byte Msg len
+ StkSendByte(7); // low byte Msg len
+ StkSendByte(TOKEN);
+ StkSendByte(CMD_CHIP_ERASE_ISP);
+ StkSendByte(20); // ChipErase_eraseDelay atmega8
+ StkSendByte(0); // ChipErase_pollMethod atmega8
+ StkSendByte(0xAC);
+ StkSendByte(0x88);
+ StkSendByte(0x13);
+ StkSendByte(0x76);
+ StkSendPacketFooter();
+ return (StkRcvPacket((void *)StkInBuf));
+}
+
+uint8_t Stk_ReadFlash(void)
+{
+ if (_CMD_LOAD_ADDRESS()) {
+ StkCmd = CMD_READ_FLASH_ISP;
+ return (_CMD_READ_MEM_ISP());
+ }
+ return 0;
+}
+
+
+uint8_t Stk_ReadEEprom(void)
+{
+ if (_CMD_LOAD_ADDRESS()) {
+ StkCmd = CMD_READ_EEPROM_ISP;
+ return (_CMD_READ_MEM_ISP());
+ }
+ return 0;
+}
+
+uint8_t Stk_WriteFlash(void)
+{
+ if (_CMD_LOAD_ADDRESS()) {
+ StkCmd = CMD_PROGRAM_FLASH_ISP;
+ return (_CMD_PROGRAM_MEM_ISP());
+ }
+ return 0;
+}
+
+uint8_t Stk_WriteEEprom(void)
+{
+ if (_CMD_LOAD_ADDRESS()) {
+ StkCmd = CMD_PROGRAM_EEPROM_ISP;
+ return (_CMD_PROGRAM_MEM_ISP());
+ }
+ return 0;
+}
+#endif
diff --git a/src/main/io/serial_4way_stk500v2.h b/src/main/io/serial_4way_stk500v2.h
new file mode 100644
index 0000000000..f24b60ae7a
--- /dev/null
+++ b/src/main/io/serial_4way_stk500v2.h
@@ -0,0 +1,31 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ * Author: 4712
+*/
+#include
+#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#pragma once
+
+uint8_t Stk_SignOn(void);
+uint8_t Stk_ConnectEx(void);
+uint8_t Stk_ReadEEprom(void);
+uint8_t Stk_WriteEEprom(void);
+uint8_t Stk_ReadFlash(void);
+uint8_t Stk_WriteFlash(void);
+uint8_t Stk_Chip_Erase(void);
+
+#endif
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 3bc83fd93e..2b2c6269f0 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -99,6 +99,10 @@
#ifdef USE_SERIAL_1WIRE_VCP
#include "io/serial_1wire_vcp.h"
#endif
+#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
+#include "io/serial_4way.h"
+#endif
+
#ifdef USE_ESCSERIAL
#include "drivers/serial_escserial.h"
#endif
@@ -1854,6 +1858,28 @@ static bool processInCommand(void)
}
break;
#endif
+#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
+ case MSP_SET_4WAY_IF:
+ // get channel number
+ // switch all motor lines HI
+ // reply the count of ESC found
+ headSerialReply(1);
+ serialize8(Initialize4WayInterface());
+ // because we do not come back after calling Process4WayInterface
+ // proceed with a success reply first
+ tailSerialReply();
+ // flush the transmit buffer
+ bufWriterFlush(writer);
+ // wait for all data to send
+ waitForSerialPortToFinishTransmitting(currentPort->port);
+ // rem: App: Wait at least appx. 500 ms for BLHeli to jump into
+ // bootloader mode before try to connect any ESC
+ // Start to activate here
+ Process4WayInterface(currentPort, writer);
+ // former used MSP uart is still active
+ // proceed as usual with MSP commands
+ break;
+#endif
#ifdef USE_ESCSERIAL
case MSP_SET_ESCSERIAL:
diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h
index 053a186916..fe9d9b9e1b 100644
--- a/src/main/io/serial_msp.h
+++ b/src/main/io/serial_msp.h
@@ -256,6 +256,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
#define MSP_SET_1WIRE 243 //in message Sets 1Wire paththrough
#define MSP_SET_ESCSERIAL 244 //in message Sets escserial passthrough
+#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface
// Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
#define MAX_MSP_PORT_COUNT 2
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index cc6dc885cd..715c588f5e 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -126,11 +126,16 @@
#define USE_SERVOS
#define USE_CLI
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
#ifdef USE_SERIAL_1WIRE
// FlexPort (pin 21/22, TX/RX respectively):
diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h
index add19d9d70..87758ab4ad 100755
--- a/src/main/target/COLIBRI_RACE/target.h
+++ b/src/main/target/COLIBRI_RACE/target.h
@@ -184,11 +184,17 @@
#define USE_SERVOS
#define USE_CLI
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
+
#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOB
#define S1W_TX_PIN GPIO_Pin_10
diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h
index d7ca4e76b6..9a2077af08 100644
--- a/src/main/target/IRCFUSIONF3/target.h
+++ b/src/main/target/IRCFUSIONF3/target.h
@@ -165,11 +165,17 @@
#define BIND_PORT GPIOB
#define BIND_PIN Pin_11
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
+
#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOA
#define S1W_TX_PIN GPIO_Pin_9
diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h
index f8c87d3e51..13aeb63c16 100644
--- a/src/main/target/LUX_RACE/target.h
+++ b/src/main/target/LUX_RACE/target.h
@@ -165,11 +165,17 @@
#define BIND_PORT GPIOC
#define BIND_PIN Pin_5
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
+
#ifdef USE_SERIAL_1WIRE
// Untested
#define S1W_TX_GPIO GPIOB
diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h
index a123d53253..97bd1d8054 100644
--- a/src/main/target/MOTOLAB/target.h
+++ b/src/main/target/MOTOLAB/target.h
@@ -191,11 +191,16 @@
#define BIND_PORT GPIOB
#define BIND_PIN Pin_4
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOB
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index de468e0b6b..701d9e2da4 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -191,11 +191,16 @@
#define BIND_PORT GPIOA
#define BIND_PIN Pin_3
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
#ifdef USE_SERIAL_1WIRE
// STM32F103CBT6-LQFP48 Pin30 (PA9) TX - PC3 connects to onboard CP2102 RX
diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h
index 64436374f1..7ed70db57a 100644
--- a/src/main/target/PORT103R/target.h
+++ b/src/main/target/PORT103R/target.h
@@ -156,11 +156,17 @@
#define TELEMETRY
#define USE_SERVOS
#define USE_CLI
+
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOA
diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h
index f8b72d1d8d..6fbd16a36d 100644
--- a/src/main/target/RMDO/target.h
+++ b/src/main/target/RMDO/target.h
@@ -157,11 +157,16 @@
#define BIND_PORT GPIOB
#define BIND_PIN Pin_11
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOA
diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h
index d727f8c5b9..7a2ba579a0 100644
--- a/src/main/target/SPARKY/target.h
+++ b/src/main/target/SPARKY/target.h
@@ -166,11 +166,16 @@
#define WS2811_IRQ DMA1_Channel7_IRQn
#endif
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOB
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index 08937aac70..b83f65e7e1 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/src/main/target/SPRACINGF3/target.h
@@ -165,11 +165,16 @@
#define BIND_PORT GPIOB
#define BIND_PIN Pin_11
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOA
diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h
index 08d6bf7c95..e843edd03d 100644
--- a/src/main/target/SPRACINGF3MINI/target.h
+++ b/src/main/target/SPRACINGF3MINI/target.h
@@ -234,11 +234,16 @@
#define BINDPLUG_PORT BUTTON_B_PORT
#define BINDPLUG_PIN BUTTON_B_PIN
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO UART1_GPIO
diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h
index 0b8e1bb84b..e474ef0733 100644
--- a/src/main/target/STM32F3DISCOVERY/target.h
+++ b/src/main/target/STM32F3DISCOVERY/target.h
@@ -162,11 +162,16 @@
#define USE_SERVOS
#define USE_CLI
+#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
+#define USE_SERIAL_4WAY_SK_BOOTLOADER
+
+#if !(defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) || defined(USE_SERIAL_4WAY_SK_BOOTLOADER))
#ifdef USE_VCP
#define USE_SERIAL_1WIRE_VCP
#else
#define USE_SERIAL_1WIRE
#endif
+#endif
#ifdef USE_SERIAL_1WIRE
// STM32F3DISCOVERY TX - PD5 connects to UART RX