diff --git a/Makefile b/Makefile
index f0234cc2b7..fab52754f4 100644
--- a/Makefile
+++ b/Makefile
@@ -305,6 +305,7 @@ COMMON_SRC = build_config.c \
io/rc_curves.c \
io/serial.c \
io/serial_1wire.c \
+ io/serial_1wire_vcp.c \
io/serial_cli.c \
io/serial_msp.c \
io/statusindicator.c \
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index cce25e3403..d521153bfe 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -50,6 +50,7 @@ static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
static uint8_t allocatedOutputPortCount = 0;
+static bool pwmMotorsEnabled = true;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
{
TIM_OCInitTypeDef TIM_OCInitStructure;
@@ -163,7 +164,7 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value)
void pwmWriteMotor(uint8_t index, uint16_t value)
{
- if (motors[index] && index < MAX_MOTORS)
+ if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled)
motors[index]->pwmWritePtr(index, value);
}
@@ -177,6 +178,16 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
}
}
+void pwmDisableMotors(void)
+{
+ pwmMotorsEnabled = false;
+}
+
+void pwmEnableMotors(void)
+{
+ pwmMotorsEnabled = true;
+}
+
void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{
uint8_t index;
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index 19c5aa8202..11036d5278 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -24,3 +24,5 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount);
void pwmWriteServo(uint8_t index, uint16_t value);
bool isMotorBrushed(uint16_t motorPwmRate);
+void pwmDisableMotors(void);
+void pwmEnableMotors(void);
diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c
index ed1e50de1f..c3f632b1c8 100644
--- a/src/main/drivers/serial_usb_vcp.c
+++ b/src/main/drivers/serial_usb_vcp.c
@@ -183,3 +183,9 @@ serialPort_t *usbVcpOpen(void)
return (serialPort_t *)s;
}
+uint32_t usbVcpGetBaudRate(serialPort_t *instance)
+{
+ UNUSED(instance);
+
+ return CDC_BaudRate();
+}
diff --git a/src/main/drivers/serial_usb_vcp.h b/src/main/drivers/serial_usb_vcp.h
index 068073c2f0..7d1b88be1f 100644
--- a/src/main/drivers/serial_usb_vcp.h
+++ b/src/main/drivers/serial_usb_vcp.h
@@ -30,3 +30,4 @@ typedef struct {
} vcpPort_t;
serialPort_t *usbVcpOpen(void);
+uint32_t usbVcpGetBaudRate(serialPort_t *instance);
diff --git a/src/main/io/serial_1wire.c b/src/main/io/serial_1wire.c
index 98eaa5b88f..6b0a5e89ab 100644
--- a/src/main/io/serial_1wire.c
+++ b/src/main/io/serial_1wire.c
@@ -32,6 +32,7 @@
#include "io/serial_1wire.h"
#include "io/beeper.h"
#include "drivers/pwm_mapping.h"
+#include "drivers/pwm_output.h"
#include "flight/mixer.h"
uint8_t escCount; // we detect the hardware dynamically
@@ -42,7 +43,7 @@ static void gpio_set_mode(GPIO_TypeDef* gpio, uint16_t pin, GPIO_Mode mode) {
gpio_config_t cfg;
cfg.pin = pin;
cfg.mode = mode;
- cfg.speed = Speed_10MHz;
+ cfg.speed = Speed_2MHz;
gpioInit(gpio, &cfg);
}
@@ -51,8 +52,7 @@ static uint32_t GetPinPos(uint32_t pin) {
for (pinPos = 0; pinPos < 16; pinPos++) {
uint32_t pinMask = (0x1 << pinPos);
if (pin & pinMask) {
- // pos found
- return pinPos;
+ return pinPos;
}
}
return 0;
@@ -61,11 +61,12 @@ static uint32_t GetPinPos(uint32_t pin) {
void usb1WireInitialize()
{
escCount = 0;
+ pwmDisableMotors();
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 ) {
+ if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) {
escHardware[escCount].gpio = pwmOutputConfiguration->portConfigurations[i].timerHardware->gpio;
escHardware[escCount].pin = pwmOutputConfiguration->portConfigurations[i].timerHardware->pin;
escHardware[escCount].pinpos = GetPinPos(escHardware[escCount].pin);
@@ -76,29 +77,37 @@ void usb1WireInitialize()
}
}
+void usb1WireDeInitialize(void){
+ for (uint8_t selected_esc = 0; selected_esc < (escCount); selected_esc++) {
+ gpio_set_mode(escHardware[selected_esc].gpio,escHardware[selected_esc].pin, Mode_AF_PP); //GPIO_Mode_IPU
+ }
+ escCount = 0;
+ pwmEnableMotors();
+}
+
#ifdef STM32F10X
static volatile uint32_t in_cr_mask, out_cr_mask;
static __IO uint32_t *cr;
static void gpio_prep_vars(uint32_t escIndex)
{
- GPIO_TypeDef *gpio = escHardware[escIndex].gpio;
- uint32_t pinpos = escHardware[escIndex].pinpos;
- // mask out extra bits from pinmode, leaving just CNF+MODE
- uint32_t inmode = Mode_IPU & 0x0F;
- uint32_t outmode = (Mode_Out_PP & 0x0F) | Speed_10MHz;
- // reference CRL or CRH, depending whether pin number is 0..7 or 8..15
- cr = &gpio->CRL + (pinpos / 8);
- // offset to CNF and MODE portions of CRx register
- uint32_t shift = (pinpos % 8) * 4;
- // Read out current CRx value
- in_cr_mask = out_cr_mask = *cr;
- // Mask out 4 bits
- in_cr_mask &= ~(0xF << shift);
- out_cr_mask &= ~(0xF << shift);
- // save current pinmode
- in_cr_mask |= inmode << shift;
- out_cr_mask |= outmode << shift;
+ GPIO_TypeDef *gpio = escHardware[escIndex].gpio;
+ uint32_t pinpos = escHardware[escIndex].pinpos;
+ // mask out extra bits from pinmode, leaving just CNF+MODE
+ uint32_t inmode = Mode_IPU & 0x0F;
+ uint32_t outmode = (Mode_Out_PP & 0x0F) | Speed_10MHz;
+ // reference CRL or CRH, depending whether pin number is 0..7 or 8..15
+ cr = &gpio->CRL + (pinpos / 8);
+ // offset to CNF and MODE portions of CRx register
+ uint32_t shift = (pinpos % 8) * 4;
+ // Read out current CRx value
+ in_cr_mask = out_cr_mask = *cr;
+ // Mask out 4 bits
+ in_cr_mask &= ~(0xF << shift);
+ out_cr_mask &= ~(0xF << shift);
+ // save current pinmode
+ in_cr_mask |= inmode << shift;
+ out_cr_mask |= outmode << shift;
}
static void gpioSetOne(uint32_t escIndex, GPIO_Mode mode) {
@@ -153,7 +162,7 @@ void usb1WirePassthrough(uint8_t escIndex)
GPIO_ResetBits(S1W_RX_GPIO, S1W_RX_PIN);
GPIO_ResetBits(S1W_TX_GPIO, S1W_TX_PIN);
// configure gpio
- gpio_set_mode(S1W_RX_GPIO, S1W_RX_PIN, Mode_IPU);
+ //gpio_set_mode(S1W_RX_GPIO, S1W_RX_PIN, Mode_IPU);
gpio_set_mode(S1W_TX_GPIO, S1W_TX_PIN, Mode_Out_PP);
#ifdef STM32F10X
@@ -209,6 +218,8 @@ void usb1WirePassthrough(uint8_t escIndex)
// we get here in case ct reached zero
TX_SET_HIGH;
RX_LED_OFF;
+ // reactivate serial port
+ gpio_set_mode(S1W_TX_GPIO, S1W_TX_PIN, Mode_AF_PP);
// Enable all irq (for Hardware UART)
__enable_irq();
return;
diff --git a/src/main/io/serial_1wire.h b/src/main/io/serial_1wire.h
index 1e560ed938..594cdd43a3 100644
--- a/src/main/io/serial_1wire.h
+++ b/src/main/io/serial_1wire.h
@@ -33,4 +33,5 @@ typedef struct {
void usb1WireInitialize();
void usb1WirePassthrough(uint8_t escIndex);
+void usb1WireDeInitialize(void);
#endif
diff --git a/src/main/io/serial_1wire_vcp.c b/src/main/io/serial_1wire_vcp.c
new file mode 100644
index 0000000000..db93b4695a
--- /dev/null
+++ b/src/main/io/serial_1wire_vcp.c
@@ -0,0 +1,237 @@
+/*
+ * 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
+#include
+#include
+#include
+#include "platform.h"
+
+#ifdef USE_SERIAL_1WIRE_VCP
+#include "drivers/gpio.h"
+#include "drivers/light_led.h"
+#include "drivers/system.h"
+#include "io/serial_1wire_vcp.h"
+#include "io/beeper.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/pwm_output.h"
+#include "flight/mixer.h"
+#include "io/serial_msp.h"
+#include "drivers/buf_writer.h"
+#include "drivers/serial_usb_vcp.h"
+
+uint8_t escCount;
+
+static escHardware_t escHardware[MAX_PWM_MOTORS];
+
+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;
+}
+static uint8_t selected_esc;
+
+#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 usb1WireInitializeVcp(void)
+{
+ 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;
+ selected_esc = 0;
+}
+
+void usb1WireDeInitializeVcp(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 START_BIT_TIMEOUT_MS 2
+
+#define BIT_TIME (52) //52uS
+#define BIT_TIME_HALVE (BIT_TIME >> 1) //26uS
+#define START_BIT_TIME (BIT_TIME_HALVE + 1)
+#define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE)
+
+static void suart_putc_(uint8_t tx_b)
+{
+ uint32_t btime;
+ ESC_SET_LO; // 0 = start bit
+ btime = BIT_TIME + micros();
+ while (micros() < btime);
+ for(uint8_t bit = 0; bit <8; bit++)
+ {
+ if(tx_b & 1)
+ {
+ ESC_SET_HI; // 1
+ }
+ else
+ {
+ ESC_SET_LO; // 0
+ }
+ btime = btime + BIT_TIME;
+ tx_b = (tx_b >> 1);
+ while (micros() < btime);
+ }
+ ESC_SET_HI; // 1 = stop bit
+ btime = btime + BIT_TIME;
+ while (micros() < btime);
+}
+
+
+static uint8_t suart_getc_(uint8_t *bt)
+{
+ uint32_t btime;
+ uint32_t start_time;
+ uint32_t stop_time;
+ uint32_t wait_start;
+
+ *bt = 0;
+
+ wait_start = millis() + START_BIT_TIMEOUT_MS;
+ while (ESC_IS_HI) {
+ // check for start bit begin
+ if (millis() > wait_start) {
+ return 0;
+ }
+ }
+ // start bit
+ start_time = micros();
+ btime = start_time + START_BIT_TIME;
+ stop_time = start_time + STOP_BIT_TIME;
+
+ while (micros() < btime);
+
+ if (ESC_IS_HI) return 0; // check start bit
+ for (uint8_t bit=0;bit<8;bit++)
+ {
+ btime = btime + BIT_TIME;
+ while (micros() < btime);
+ if (ESC_IS_HI)
+ {
+ *bt |= (1 << bit); // 1 else 0
+ }
+ }
+ while (micros() < stop_time);
+
+ if (ESC_IS_LO) return 0; // check stop bit
+
+ return 1; // OK
+}
+#define USE_TXRX_LED
+
+#ifdef USE_TXRX_LED
+#define RX_LED_OFF LED0_OFF
+#define RX_LED_ON LED0_ON
+#ifdef LED1
+#define TX_LED_OFF LED1_OFF
+#define TX_LED_ON LED1_ON
+#else
+#define TX_LED_OFF LED0_OFF
+#define TX_LED_ON LED0_ON
+#endif
+#else
+#define RX_LED_OFF
+#define RX_LED_ON
+#define TX_LED_OFF
+#define TX_LED_ON
+#endif
+
+// This method translates 2 wires (a tx and rx line) to 1 wire, by letting the
+// RX line control when data should be read or written from the single line
+void usb1WirePassthroughVcp(mspPort_t *mspPort, bufWriter_t *bufwriter, uint8_t escIndex)
+{
+#ifdef BEEPER
+ // fix for buzzer often starts beeping continuously when the ESCs are read
+ // switch beeper silent here
+ // TODO (4712) do we need beeperSilence()?
+ // beeperSilence();
+#endif
+
+ selected_esc = escIndex;
+ // TODO (4712) check all possible baud rate ok?
+ // uint32_t baudrate = serialGetBaudRate(mspPort->port);
+ // serialSetBaudRate(mspPort->port, 19200);
+
+ while(usbVcpGetBaudRate(mspPort->port) != 4800) {
+ // esc line is in Mode_IPU by default
+ static uint8_t bt;
+
+ if (suart_getc_(&bt)) {
+ RX_LED_ON;
+ serialBeginWrite(mspPort->port);
+ bufWriterAppend(bufwriter, bt);
+ while (suart_getc_(&bt)){
+ bufWriterAppend(bufwriter, bt);
+ }
+ serialEndWrite(mspPort->port);
+ bufWriterFlush(bufwriter);
+ RX_LED_OFF;
+ }
+ if (serialRxBytesWaiting(mspPort->port)) {
+ ESC_OUTPUT;
+ TX_LED_ON;
+ do {
+ bt = serialRead(mspPort->port);
+ suart_putc_(bt);
+ } while(serialRxBytesWaiting(mspPort->port));
+ ESC_INPUT;
+ TX_LED_OFF;
+ }
+ }
+ // serialSetBaudRate(mspPort->port, baudrate);
+ return;
+}
+#endif
+
diff --git a/src/main/io/serial_1wire_vcp.h b/src/main/io/serial_1wire_vcp.h
new file mode 100644
index 0000000000..0f4a6a3dc3
--- /dev/null
+++ b/src/main/io/serial_1wire_vcp.h
@@ -0,0 +1,42 @@
+/*
+ * 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
+ */
+#pragma once
+
+#include "platform.h"
+#ifdef USE_SERIAL_1WIRE_VCP
+#include "drivers/serial.h"
+#include "drivers/buf_writer.h"
+#include "drivers/pwm_mapping.h"
+#include "io/serial_msp.h"
+
+extern uint8_t escCount;
+
+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;
+
+void usb1WireInitializeVcp(void);
+void usb1WireDeInitializeVcp(void);
+void usb1WirePassthroughVcp(mspPort_t *mspPort, bufWriter_t *bufwriter, uint8_t escIndex);
+#endif
+
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index bd8ee07a5b..3bc83fd93e 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -96,6 +96,9 @@
#ifdef USE_SERIAL_1WIRE
#include "io/serial_1wire.h"
#endif
+#ifdef USE_SERIAL_1WIRE_VCP
+#include "io/serial_1wire_vcp.h"
+#endif
#ifdef USE_ESCSERIAL
#include "drivers/serial_escserial.h"
#endif
@@ -1751,61 +1754,105 @@ static bool processInCommand(void)
// switch all motor lines HI
usb1WireInitialize();
// reply the count of ESC found
- headSerialReply(1);
+ headSerialReply(2);
serialize8(escCount);
-
+ serialize8(currentPort->port->identifier);
// and come back right afterwards
// rem: App: Wait at least appx. 500 ms for BLHeli to jump into
// bootloader mode before try to connect any ESC
return true;
- }
- else {
+ } else if (i < escCount) {
// Check for channel number 0..ESC_COUNT-1
- if (i < escCount) {
- // because we do not come back after calling usb1WirePassthrough
- // proceed with a success reply first
- headSerialReply(0);
- tailSerialReply();
- // flush the transmit buffer
- bufWriterFlush(writer);
- // wait for all data to send
- waitForSerialPortToFinishTransmitting(currentPort->port);
- // Start to activate here
- // motor 1 => index 0
+ // because we do not come back after calling usb1WirePassthrough
+ // proceed with a success reply first
+ headSerialReply(0);
+ tailSerialReply();
+ // flush the transmit buffer
+ bufWriterFlush(writer);
+ // wait for all data to send
+ waitForSerialPortToFinishTransmitting(currentPort->port);
+ // Start to activate here
+ // motor 1 => index 0
- // search currentPort portIndex
- /* next lines seems to be unnecessary, because the currentPort always point to the same mspPorts[portIndex]
- uint8_t portIndex;
- for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
- if (currentPort == &mspPorts[portIndex]) {
- break;
- }
- }
- */
- mspReleasePortIfAllocated(mspSerialPort); // CloseSerialPort also marks currentPort as UNUSED_PORT
- usb1WirePassthrough(i);
- // Wait a bit more to let App read the 0 byte and switch baudrate
- // 2ms will most likely do the job, but give some grace time
- delay(10);
- // rebuild/refill currentPort structure, does openSerialPort if marked UNUSED_PORT - used ports are skiped
- mspAllocateSerialPorts(&masterConfig.serialConfig);
- /* restore currentPort and mspSerialPort
- setCurrentPort(&mspPorts[portIndex]); // not needed same index will be restored
- */
- // former used MSP uart is active again
- // restore MSP_SET_1WIRE as current command for correct headSerialReply(0)
- currentPort->cmdMSP = MSP_SET_1WIRE;
- } else {
- // ESC channel higher than max. allowed
- // rem: BLHeliSuite will not support more than 8
- headSerialError(0);
+ // search currentPort portIndex
+ /* next lines seems to be unnecessary, because the currentPort always point to the same mspPorts[portIndex]
+ uint8_t portIndex;
+ for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
+ if (currentPort == &mspPorts[portIndex]) {
+ break;
+ }
}
+ */
+ // *mspReleasePortIfAllocated(mspSerialPort); // CloseSerialPort also marks currentPort as UNUSED_PORT
+ usb1WirePassthrough(i);
+ // Wait a bit more to let App read the 0 byte and/or switch baudrate
+ // 2ms will most likely do the job, but give some grace time
+ delay(10);
+ // rebuild/refill currentPort structure, does openSerialPort if marked UNUSED_PORT - used ports are skiped
+ // *mspAllocateSerialPorts(&masterConfig.serialConfig);
+ /* restore currentPort and mspSerialPort
+ setCurrentPort(&mspPorts[portIndex]); // not needed same index will be restored
+ */
+ // former used MSP uart is active again
+ // restore MSP_SET_1WIRE as current command for correct headSerialReply(0)
+ //currentPort->cmdMSP = MSP_SET_1WIRE;
+ } else if (i == 0xFE) {
+ usb1WireDeInitialize();
+ } else {
+ // ESC channel higher than max. allowed
+ headSerialError(0);
+ }
+ // proceed as usual with MSP commands
+ // and wait to switch to next channel
+ // rem: App needs to call MSP_BOOT to deinitialize Passthrough
+ break;
+#endif
+
+#ifdef USE_SERIAL_1WIRE_VCP
+ case MSP_SET_1WIRE:
+ // get channel number
+ i = read8();
+ // we do not give any data back, assume channel number is transmitted OK
+ if (i == 0xFF) {
+ // 0xFF -> preinitialize the Passthrough
+ // switch all motor lines HI
+ usb1WireInitializeVcp();
+ // reply the count of ESC found
+ headSerialReply(2);
+ serialize8(escCount);
+ serialize8(currentPort->port->identifier);
+ // and come back right afterwards
+ // rem: App: Wait at least appx. 500 ms for BLHeli to jump into
+ // bootloader mode before try to connect any ESC
+
+ return true;
+ } else if (i < escCount) {
+ // Check for channel number 0..ESC_COUNT-1
+ // because we do not come back after calling usb1WirePassthrough
+ // proceed with a success reply first
+ headSerialReply(0);
+ tailSerialReply();
+ // flush the transmit buffer
+ bufWriterFlush(writer);
+ // wait for all data to send
+ waitForSerialPortToFinishTransmitting(currentPort->port);
+ // Start to activate here
+ // motor 1 => index 0
+ usb1WirePassthroughVcp(currentPort, writer, i);
+ // Wait a bit more to let App switch baudrate
+ delay(10);
+ // former used MSP uart is still active
// proceed as usual with MSP commands
// and wait to switch to next channel
- // rem: App needs to call MSP_BOOT to deinitialize Passthrough
+ // rem: App needs to call MSP_SET_1WIRE(0xFE) to deinitialize Passthrough
+ } else if (i == 0xFE) {
+ usb1WireDeInitializeVcp();
+ } else {
+ // ESC channel higher than max. allowed
+ headSerialError(0);
}
- break;
+ break;
#endif
#ifdef USE_ESCSERIAL
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index e57a642709..cc6dc885cd 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -126,8 +126,13 @@
#define USE_SERVOS
#define USE_CLI
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
#define USE_SERIAL_1WIRE
+#endif
+#ifdef USE_SERIAL_1WIRE
// FlexPort (pin 21/22, TX/RX respectively):
// Note, FlexPort has 10k pullups on both TX and RX
// JST Pin3 TX - connect to external UART/USB RX
@@ -136,6 +141,7 @@
// JST Pin4 RX - connect to external UART/USB TX
#define S1W_RX_GPIO GPIOB
#define S1W_RX_PIN GPIO_Pin_11
+#endif
#undef DISPLAY
#undef SONAR
diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h
index d267b553c9..add19d9d70 100755
--- a/src/main/target/COLIBRI_RACE/target.h
+++ b/src/main/target/COLIBRI_RACE/target.h
@@ -184,8 +184,14 @@
#define USE_SERVOS
#define USE_CLI
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
#define USE_SERIAL_1WIRE
+#endif
+#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOB
#define S1W_TX_PIN GPIO_Pin_10
#define S1W_RX_GPIO GPIOB
#define S1W_RX_PIN GPIO_Pin_11
+#endif
diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h
index 4152bbfb05..d7ca4e76b6 100644
--- a/src/main/target/IRCFUSIONF3/target.h
+++ b/src/main/target/IRCFUSIONF3/target.h
@@ -165,8 +165,16 @@
#define BIND_PORT GPIOB
#define BIND_PIN Pin_11
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
#define USE_SERIAL_1WIRE
+#endif
+#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOA
#define S1W_TX_PIN GPIO_Pin_9
#define S1W_RX_GPIO GPIOA
#define S1W_RX_PIN GPIO_Pin_10
+#endif
+
+
diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h
index df441c3198..f8c87d3e51 100644
--- a/src/main/target/LUX_RACE/target.h
+++ b/src/main/target/LUX_RACE/target.h
@@ -165,9 +165,15 @@
#define BIND_PORT GPIOC
#define BIND_PIN Pin_5
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
#define USE_SERIAL_1WIRE
+#endif
+#ifdef USE_SERIAL_1WIRE
// Untested
#define S1W_TX_GPIO GPIOB
#define S1W_TX_PIN GPIO_Pin_10
#define S1W_RX_GPIO GPIOB
#define S1W_RX_PIN GPIO_Pin_11
+#endif
diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h
index f49661f856..a123d53253 100644
--- a/src/main/target/MOTOLAB/target.h
+++ b/src/main/target/MOTOLAB/target.h
@@ -191,10 +191,15 @@
#define BIND_PORT GPIOB
#define BIND_PIN Pin_4
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
#define USE_SERIAL_1WIRE
+#endif
+#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOB
#define S1W_TX_PIN GPIO_Pin_6
#define S1W_RX_GPIO GPIOB
#define S1W_RX_PIN GPIO_Pin_7
-
+#endif
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index 7ec9848ae6..de468e0b6b 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -191,14 +191,20 @@
#define BIND_PORT GPIOA
#define BIND_PIN Pin_3
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
#define USE_SERIAL_1WIRE
+#endif
+#ifdef USE_SERIAL_1WIRE
// STM32F103CBT6-LQFP48 Pin30 (PA9) TX - PC3 connects to onboard CP2102 RX
#define S1W_TX_GPIO GPIOA
#define S1W_TX_PIN GPIO_Pin_9
// STM32F103CBT6-LQFP48 Pin31 (PA10) RX - PC1 to onboard CP2102 TX
#define S1W_RX_GPIO GPIOA
#define S1W_RX_PIN GPIO_Pin_10
+#endif
// alternative defaults for AlienWii32 F1 target
#ifdef ALIENWII32
diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h
index 07ce1e2b2c..64436374f1 100644
--- a/src/main/target/PORT103R/target.h
+++ b/src/main/target/PORT103R/target.h
@@ -156,3 +156,15 @@
#define TELEMETRY
#define USE_SERVOS
#define USE_CLI
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
+#define USE_SERIAL_1WIRE
+#endif
+
+#ifdef USE_SERIAL_1WIRE
+#define S1W_TX_GPIO GPIOA
+#define S1W_TX_PIN GPIO_Pin_9
+#define S1W_RX_GPIO GPIOA
+#define S1W_RX_PIN GPIO_Pin_10
+#endif
diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h
index 64e5f12468..f8b72d1d8d 100644
--- a/src/main/target/RMDO/target.h
+++ b/src/main/target/RMDO/target.h
@@ -157,9 +157,15 @@
#define BIND_PORT GPIOB
#define BIND_PIN Pin_11
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
#define USE_SERIAL_1WIRE
+#endif
+#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOA
#define S1W_TX_PIN GPIO_Pin_9
#define S1W_RX_GPIO GPIOA
#define S1W_RX_PIN GPIO_Pin_10
+#endif
diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h
index 907a3cb093..d727f8c5b9 100644
--- a/src/main/target/SPARKY/target.h
+++ b/src/main/target/SPARKY/target.h
@@ -166,12 +166,18 @@
#define WS2811_IRQ DMA1_Channel7_IRQn
#endif
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
#define USE_SERIAL_1WIRE
+#endif
+#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOB
#define S1W_TX_PIN GPIO_Pin_6
#define S1W_RX_GPIO GPIOB
#define S1W_RX_PIN GPIO_Pin_7
+#endif
#define SPEKTRUM_BIND
// USART2, PA3
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index bd4c60bba5..08937aac70 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/src/main/target/SPRACINGF3/target.h
@@ -165,9 +165,15 @@
#define BIND_PORT GPIOB
#define BIND_PIN Pin_11
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
#define USE_SERIAL_1WIRE
+#endif
+#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO GPIOA
#define S1W_TX_PIN GPIO_Pin_9
#define S1W_RX_GPIO GPIOA
#define S1W_RX_PIN GPIO_Pin_10
+#endif
diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h
index 0fd8b22327..08d6bf7c95 100644
--- a/src/main/target/SPRACINGF3MINI/target.h
+++ b/src/main/target/SPRACINGF3MINI/target.h
@@ -234,9 +234,15 @@
#define BINDPLUG_PORT BUTTON_B_PORT
#define BINDPLUG_PIN BUTTON_B_PIN
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
#define USE_SERIAL_1WIRE
+#endif
+#ifdef USE_SERIAL_1WIRE
#define S1W_TX_GPIO UART1_GPIO
#define S1W_TX_PIN UART1_TX_PIN
#define S1W_RX_GPIO UART1_GPIO
#define S1W_RX_PIN UART1_RX_PIN
+#endif
diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h
index 5796239f4a..0b8e1bb84b 100644
--- a/src/main/target/STM32F3DISCOVERY/target.h
+++ b/src/main/target/STM32F3DISCOVERY/target.h
@@ -162,12 +162,17 @@
#define USE_SERVOS
#define USE_CLI
+#ifdef USE_VCP
+#define USE_SERIAL_1WIRE_VCP
+#else
#define USE_SERIAL_1WIRE
-// How many escs does this board support?
-#define ESC_COUNT 6
+#endif
+
+#ifdef USE_SERIAL_1WIRE
// STM32F3DISCOVERY TX - PD5 connects to UART RX
#define S1W_TX_GPIO GPIOD
#define S1W_TX_PIN GPIO_Pin_5
// STM32F3DISCOVERY RX - PD6 connects to UART TX
#define S1W_RX_GPIO GPIOD
#define S1W_RX_PIN GPIO_Pin_6
+#endif
diff --git a/src/main/vcp/hw_config.c b/src/main/vcp/hw_config.c
index bad9674a1e..7384b1e0b4 100644
--- a/src/main/vcp/hw_config.c
+++ b/src/main/vcp/hw_config.c
@@ -362,4 +362,17 @@ uint8_t usbIsConnected(void)
return (bDeviceState != UNCONNECTED);
}
+
+/*******************************************************************************
+ * Function Name : CDC_BaudRate.
+ * Description : Get the current baud rate
+ * Input : None.
+ * Output : None.
+ * Return : Baud rate in bps
+ *******************************************************************************/
+uint32_t CDC_BaudRate(void)
+{
+ return Virtual_Com_Port_GetBaudRate();
+}
+
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/src/main/vcp/hw_config.h b/src/main/vcp/hw_config.h
index ee40ed7085..de454859f3 100644
--- a/src/main/vcp/hw_config.h
+++ b/src/main/vcp/hw_config.h
@@ -59,6 +59,7 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength); // HJI
uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len); // HJI
uint8_t usbIsConfigured(void); // HJI
uint8_t usbIsConnected(void); // HJI
+uint32_t CDC_BaudRate(void);
/* External variables --------------------------------------------------------*/
extern __IO uint32_t receiveLength; // HJI
diff --git a/src/main/vcp/usb_prop.c b/src/main/vcp/usb_prop.c
index 0130ecd714..b070ce9cca 100644
--- a/src/main/vcp/usb_prop.c
+++ b/src/main/vcp/usb_prop.c
@@ -358,5 +358,17 @@ uint8_t *Virtual_Com_Port_SetLineCoding(uint16_t Length)
return (uint8_t *)&linecoding;
}
+/*******************************************************************************
+ * Function Name : Virtual_Com_Port_GetBaudRate.
+ * Description : Get the current baudrate
+ * Input : None.
+ * Output : None.
+ * Return : baudrate in bps
+ *******************************************************************************/
+uint32_t Virtual_Com_Port_GetBaudRate(void)
+{
+ return linecoding.bitrate;
+}
+
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/src/main/vcp/usb_prop.h b/src/main/vcp/usb_prop.h
index b9b379c453..12d050c38d 100644
--- a/src/main/vcp/usb_prop.h
+++ b/src/main/vcp/usb_prop.h
@@ -79,6 +79,7 @@ uint8_t *Virtual_Com_Port_GetStringDescriptor( uint16_t);
uint8_t *Virtual_Com_Port_GetLineCoding(uint16_t Length);
uint8_t *Virtual_Com_Port_SetLineCoding(uint16_t Length);
+uint32_t Virtual_Com_Port_GetBaudRate(void);
#endif /* __usb_prop_H */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/