mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 13:25:30 +03:00
1wire-pass-through-vcp
also changed uart 1wire-pass-through to avoid need of FC reboot after exit.
This commit is contained in:
parent
9d737474b8
commit
85dc6b59d2
26 changed files with 533 additions and 69 deletions
1
Makefile
1
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 \
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -183,3 +183,9 @@ serialPort_t *usbVcpOpen(void)
|
|||
|
||||
return (serialPort_t *)s;
|
||||
}
|
||||
uint32_t usbVcpGetBaudRate(serialPort_t *instance)
|
||||
{
|
||||
UNUSED(instance);
|
||||
|
||||
return CDC_BaudRate();
|
||||
}
|
||||
|
|
|
@ -30,3 +30,4 @@ typedef struct {
|
|||
} vcpPort_t;
|
||||
|
||||
serialPort_t *usbVcpOpen(void);
|
||||
uint32_t usbVcpGetBaudRate(serialPort_t *instance);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -33,4 +33,5 @@ typedef struct {
|
|||
|
||||
void usb1WireInitialize();
|
||||
void usb1WirePassthrough(uint8_t escIndex);
|
||||
void usb1WireDeInitialize(void);
|
||||
#endif
|
||||
|
|
237
src/main/io/serial_1wire_vcp.c
Normal file
237
src/main/io/serial_1wire_vcp.c
Normal file
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* Author 4712
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#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
|
||||
|
42
src/main/io/serial_1wire_vcp.h
Normal file
42
src/main/io/serial_1wire_vcp.h
Normal file
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* 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
|
||||
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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****/
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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****/
|
||||
|
||||
|
|
|
@ -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****/
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue