mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-13 19:40:27 +03:00
serial 1wire passthrough for blheli 14.0.0.3.2+
tested on NAZE and CC3D (OPBL)
This commit is contained in:
parent
c49bd407bf
commit
67f74121d6
14 changed files with 496 additions and 0 deletions
1
Makefile
1
Makefile
|
@ -238,6 +238,7 @@ COMMON_SRC = build_config.c \
|
||||||
io/rc_controls.c \
|
io/rc_controls.c \
|
||||||
io/rc_curves.c \
|
io/rc_curves.c \
|
||||||
io/serial.c \
|
io/serial.c \
|
||||||
|
io/serial_1wire.c \
|
||||||
io/serial_cli.c \
|
io/serial_cli.c \
|
||||||
io/serial_msp.c \
|
io/serial_msp.c \
|
||||||
io/statusindicator.c \
|
io/statusindicator.c \
|
||||||
|
|
86
docs/1wire.md
Normal file
86
docs/1wire.md
Normal file
|
@ -0,0 +1,86 @@
|
||||||
|
# 1-wire passthrough esc programming
|
||||||
|
|
||||||
|
### ESCs must have the BlHeli Bootloader.
|
||||||
|
|
||||||
|
If your ESCs didn't come with BlHeli Bootloader, you'll need to flash them with an ArduinoISP programmer first. [Here's a guide](http://bit.ly/blheli-f20).
|
||||||
|
|
||||||
|
This is the option you need to select for the bootloader:
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
Currently supported on the SPRACINGF3, STM32F3DISCOVERY, NAZE32 (including clones such as the FLIP32) and CC3D.
|
||||||
|
|
||||||
|
## Wiring
|
||||||
|
|
||||||
|
- For the NAZE, no external wiring is necessary. Simply plugin the board via USB cable.
|
||||||
|
|
||||||
|
- For the CC3D, connect [a USB to UART adapter](http://bit.ly/cf-cp2102) to the main port. If you need one, I prefer the [CP2102](http://bit.ly/cf-cp2102) as it is cheap and [the driver](https://www.silabs.com/products/mcu/Pages/USBtoUARTBridgeVCPDrivers.aspx) is readily available.
|
||||||
|
|
||||||
|
- In the case that your board does not power on fully without a battery attached, it is OK to attach the battery before following the steps below. However, it may not be necessary in all cases.
|
||||||
|
|
||||||
|
## Usage
|
||||||
|
|
||||||
|
- Plug in the USB cable and connect to your board with the CleanFlight configurator.
|
||||||
|
|
||||||
|
- For boards without a built in USB/UART adapter, you'll need to plug an external one in. Here is how you wire up the CC3D. Plug your USB/UART adapter into the Flexi port:
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
- Open the BlHeli Suite.
|
||||||
|
|
||||||
|
- Ensure you have selected the correct Atmel or SILABS "Cleanflight" option under the "Select ATMEL / SILABS Interface" menu option.
|
||||||
|
|
||||||
|
- Ensure you have port for your external USB/UART adapter selected, if you're using one, otherwise pick the same COM port that you normally use for Cleanflight.
|
||||||
|
|
||||||
|
- Click "Connect" and wait for the connection to complete. If you get a COM error, hit connect again. It will probably work.
|
||||||
|
|
||||||
|
- Use the boxes at the bottom to select the ESCs you have connected. Note that the boxes correspond directly to the ports on your flight controller. For example if you have motors on ports 1-4, pick boxes 1-4 or in the case of a tri-copter that uses motors on ports 3, 4 and 5, select those ports in BlHeli.
|
||||||
|
|
||||||
|
- Click "Read Setup"
|
||||||
|
|
||||||
|
- Use BlHeli suite as normal.
|
||||||
|
|
||||||
|
- When you're finished with one ESC, click "Disconnect"
|
||||||
|
|
||||||
|
## Implementing and Configuring targets
|
||||||
|
|
||||||
|
The following parameters can be used to enable and configure this in the related target.h file:
|
||||||
|
|
||||||
|
USE_SERIAL_1WIRE Enables the 1wire code, defined in target.h
|
||||||
|
|
||||||
|
|
||||||
|
- For new targets
|
||||||
|
|
||||||
|
- in `target.h`
|
||||||
|
|
||||||
|
```
|
||||||
|
// Turn on serial 1wire passthrough
|
||||||
|
#define USE_SERIAL_1WIRE
|
||||||
|
// How many escs does this board support?
|
||||||
|
#define ESC_COUNT 6
|
||||||
|
// STM32F3DISCOVERY TX - PC3 connects to UART RX
|
||||||
|
#define S1W_TX_GPIO GPIOC
|
||||||
|
#define S1W_TX_PIN GPIO_Pin_3
|
||||||
|
// STM32F3DISCOVERY RX - PC1 connects to UART TX
|
||||||
|
#define S1W_RX_GPIO GPIOC
|
||||||
|
#define S1W_RX_PIN GPIO_Pin_1
|
||||||
|
```
|
||||||
|
|
||||||
|
- in `serial_1wire.c`
|
||||||
|
|
||||||
|
```
|
||||||
|
// Define your esc hardware
|
||||||
|
#if defined(STM32F3DISCOVERY) && !(defined(CHEBUZZF3))
|
||||||
|
const escHardware_t escHardware[ESC_COUNT] = {
|
||||||
|
{ GPIOD, 12 },
|
||||||
|
{ GPIOD, 13 },
|
||||||
|
{ GPIOD, 14 },
|
||||||
|
{ GPIOD, 15 },
|
||||||
|
{ GPIOA, 1 },
|
||||||
|
{ GPIOA, 2 }
|
||||||
|
};
|
||||||
|
```
|
||||||
|
|
||||||
|
## Development Notes
|
||||||
|
|
||||||
|
On the STM32F3DISCOVERY, an external pullup on the ESC line may be necessary. I needed a 3v, 4.7k pullup.
|
|
@ -110,6 +110,19 @@ sending this message for all adjustment range slots.
|
||||||
| adjustmentFunction | uint8 | See below |
|
| adjustmentFunction | uint8 | See below |
|
||||||
| auxSwitchChannelIndex | uint8 | The Aux channel number used to perform the function (indexed from 0) |
|
| auxSwitchChannelIndex | uint8 | The Aux channel number used to perform the function (indexed from 0) |
|
||||||
|
|
||||||
|
### MSP\_SET\_1WIRE
|
||||||
|
|
||||||
|
The MSP\_SET\_1WIRE is used to enable serial1wire passthrough
|
||||||
|
note: it would be ideal to disable this when armed
|
||||||
|
|
||||||
|
| Command | Msg Id | Direction |
|
||||||
|
|---------|--------|-----------|
|
||||||
|
| MSP\_SET\_1WIRE | 243 | to FC |
|
||||||
|
|
||||||
|
| Data | Type | Notes |
|
||||||
|
|------|------|-------|
|
||||||
|
| esc id | uint8 | A monotonically increasing ID, from 0 to the number of escs -1 |
|
||||||
|
|
||||||
#### AdjustmentIndex
|
#### AdjustmentIndex
|
||||||
|
|
||||||
The FC maintains internal state for each adjustmentStateIndex, currently 4 simultaneous adjustment states are maintained. Multiple adjustment ranges
|
The FC maintains internal state for each adjustmentStateIndex, currently 4 simultaneous adjustment states are maintained. Multiple adjustment ranges
|
||||||
|
|
|
@ -73,6 +73,7 @@ Re-apply any new defaults as desired.
|
||||||
|
|
||||||
| `Command` | Description |
|
| `Command` | Description |
|
||||||
|------------------|------------------------------------------------|
|
|------------------|------------------------------------------------|
|
||||||
|
| `1wire <esc>` | passthrough 1wire to the specified esc |
|
||||||
| `adjrange` | show/set adjustment ranges settings |
|
| `adjrange` | show/set adjustment ranges settings |
|
||||||
| `aux` | show/set aux settings |
|
| `aux` | show/set aux settings |
|
||||||
| `mmix` | design custom motor mixer |
|
| `mmix` | design custom motor mixer |
|
||||||
|
|
BIN
docs/assets/images/blheli-bootloader.png
Normal file
BIN
docs/assets/images/blheli-bootloader.png
Normal file
Binary file not shown.
After Width: | Height: | Size: 17 KiB |
BIN
docs/assets/images/serial1wire-cc3d-wiring.jpg
Normal file
BIN
docs/assets/images/serial1wire-cc3d-wiring.jpg
Normal file
Binary file not shown.
After Width: | Height: | Size: 265 KiB |
242
src/main/io/serial_1wire.c
Normal file
242
src/main/io/serial_1wire.c
Normal file
|
@ -0,0 +1,242 @@
|
||||||
|
/*
|
||||||
|
* 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/>.
|
||||||
|
*
|
||||||
|
* Ported from https://github.com/4712/BLHeliSuite/blob/master/Interfaces/Arduino1Wire/Source/Arduino1Wire_C/Arduino1Wire.c
|
||||||
|
* by Nathan Tsoi <nathan@vertile.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_SERIAL_1WIRE
|
||||||
|
|
||||||
|
#include "drivers/gpio.h"
|
||||||
|
#include "drivers/light_led.h"
|
||||||
|
#include "drivers/system.h"
|
||||||
|
#include "io/serial_1wire.h"
|
||||||
|
|
||||||
|
const escHardware_t escHardware[ESC_COUNT] = {
|
||||||
|
// Figure out esc clocks and pins, extrapolated from timer.c
|
||||||
|
// Periphs could be pulled progmatically... but I'll leave that for another exercise
|
||||||
|
#if defined(STM32F3DISCOVERY) && !(defined(CHEBUZZF3))
|
||||||
|
{ GPIOD, 12 },
|
||||||
|
{ GPIOD, 13 },
|
||||||
|
{ GPIOD, 14 },
|
||||||
|
{ GPIOD, 15 },
|
||||||
|
{ GPIOA, 1 },
|
||||||
|
{ GPIOA, 2 }
|
||||||
|
#elif defined(CJMCU) || defined(EUSTM32F103RC) || defined(NAZE) || defined(OLIMEXINO) || defined(PORT103R)
|
||||||
|
{ GPIOA, 8 },
|
||||||
|
{ GPIOA, 11 },
|
||||||
|
{ GPIOB, 6 },
|
||||||
|
{ GPIOB, 7 },
|
||||||
|
{ GPIOB, 8 },
|
||||||
|
{ GPIOB, 9 }
|
||||||
|
#elif CC3D
|
||||||
|
{ GPIOB, 9 },
|
||||||
|
{ GPIOB, 8 },
|
||||||
|
{ GPIOB, 7 },
|
||||||
|
{ GPIOA, 8 },
|
||||||
|
{ GPIOB, 4 },
|
||||||
|
{ GPIOA, 2 }
|
||||||
|
#elif SPRACINGF3
|
||||||
|
{ GPIOA, 6 },
|
||||||
|
{ GPIOA, 7 },
|
||||||
|
{ GPIOA, 11 },
|
||||||
|
{ GPIOA, 12 },
|
||||||
|
{ GPIOB, 8 },
|
||||||
|
{ GPIOB, 9 },
|
||||||
|
{ GPIOA, 2 },
|
||||||
|
{ GPIOA, 3 }
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
|
||||||
|
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;
|
||||||
|
gpioInit(gpio, &cfg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void usb1WireInitialize()
|
||||||
|
{
|
||||||
|
for (volatile uint8_t i = 0; i<ESC_COUNT ; i++){
|
||||||
|
gpio_set_mode(escHardware[i].gpio, (1U << escHardware[i].pinpos), Mode_IPU); //GPIO_Mode_IPU
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef STM32F10X
|
||||||
|
static volatile uint32_t in_cr_mask, out_cr_mask;
|
||||||
|
|
||||||
|
static __IO uint32_t *cr;
|
||||||
|
static void gpio_prep_vars(uint16_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;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void gpioSetOne(uint16_t escIndex, GPIO_Mode mode) {
|
||||||
|
// reference CRL or CRH, depending whether pin number is 0..7 or 8..15
|
||||||
|
if (mode == Mode_IPU) {
|
||||||
|
*cr = in_cr_mask;
|
||||||
|
escHardware[escIndex].gpio->ODR |= (1U << escHardware[escIndex].pinpos);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
*cr = out_cr_mask;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define disable_hardware_uart __disable_irq()
|
||||||
|
#define enable_hardware_uart __enable_irq()
|
||||||
|
#define ESC_HI(escIndex) ((escHardware[escIndex].gpio->IDR & (1U << escHardware[escIndex].pinpos)) != (uint32_t)Bit_RESET)
|
||||||
|
#define RX_HI ((S1W_RX_GPIO->IDR & S1W_RX_PIN) != (uint32_t)Bit_RESET)
|
||||||
|
#define ESC_SET_HI(escIndex) escHardware[escIndex].gpio->BSRR = (1U << escHardware[escIndex].pinpos)
|
||||||
|
#define ESC_SET_LO(escIndex) escHardware[escIndex].gpio->BRR = (1U << escHardware[escIndex].pinpos)
|
||||||
|
#define TX_SET_HIGH S1W_TX_GPIO->BSRR = S1W_TX_PIN
|
||||||
|
#define TX_SET_LO S1W_TX_GPIO->BRR = S1W_TX_PIN
|
||||||
|
|
||||||
|
#ifdef STM32F303xC
|
||||||
|
#define ESC_INPUT(escIndex) escHardware[escIndex].gpio->MODER &= ~(GPIO_MODER_MODER0 << (escHardware[escIndex].pinpos * 2))
|
||||||
|
#define ESC_OUTPUT(escIndex) escHardware[escIndex].gpio->MODER |= GPIO_Mode_OUT << (escHardware[escIndex].pinpos * 2)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef STM32F10X
|
||||||
|
#define ESC_INPUT(escIndex) gpioSetOne(escIndex, Mode_IPU)
|
||||||
|
#define ESC_OUTPUT(escIndex) gpioSetOne(escIndex, Mode_Out_PP)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if defined(STM32F3DISCOVERY)
|
||||||
|
#define LED_PRGMR_RX GPIO_Pin_8
|
||||||
|
#define LED_PRGMR_TX GPIO_Pin_10
|
||||||
|
// Top Left LD4, PE8 (blue)-- from programmer (RX)
|
||||||
|
#define RX_LED_OFF GPIOE->BRR = LED_PRGMR_RX
|
||||||
|
#define RX_LED_ON GPIOE->BSRR = LED_PRGMR_RX
|
||||||
|
// Top Right LD5, PE10 (orange) -- to programmer (TX)
|
||||||
|
#define TX_LED_OFF GPIOE->BRR = LED_PRGMR_TX
|
||||||
|
#define TX_LED_ON GPIOE->BSRR = LED_PRGMR_TX
|
||||||
|
static void ledInitDebug(void)
|
||||||
|
{
|
||||||
|
uint32_t pinmask = LED_PRGMR_RX|LED_PRGMR_TX;
|
||||||
|
GPIO_DeInit(GPIOE);
|
||||||
|
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOE, ENABLE);
|
||||||
|
gpio_set_mode(GPIOE, pinmask, Mode_Out_PP);
|
||||||
|
GPIOE->BRR = pinmask;
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
#define RX_LED_OFF LED0_OFF
|
||||||
|
#define RX_LED_ON LED0_ON
|
||||||
|
#define TX_LED_OFF LED1_OFF
|
||||||
|
#define TX_LED_ON LED1_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 usb1WirePassthrough(int8_t escIndex)
|
||||||
|
{
|
||||||
|
#ifdef STM32F3DISCOVERY
|
||||||
|
ledInitDebug();
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//Disable all interrupts
|
||||||
|
disable_hardware_uart;
|
||||||
|
|
||||||
|
// reset all the pins
|
||||||
|
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_TX_GPIO, S1W_TX_PIN, Mode_Out_PP);
|
||||||
|
// hey user, turn on your ESC now
|
||||||
|
|
||||||
|
#ifdef STM32F10X
|
||||||
|
// reset our gpio register pointers and bitmask values
|
||||||
|
gpio_prep_vars(escIndex);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
ESC_OUTPUT(escIndex);
|
||||||
|
ESC_SET_HI(escIndex);
|
||||||
|
TX_SET_HIGH;
|
||||||
|
// Wait for programmer to go from 1 -> 0 indicating incoming data
|
||||||
|
while(RX_HI);
|
||||||
|
while(1) {
|
||||||
|
// A new iteration on this loop starts when we have data from the programmer (read_programmer goes low)
|
||||||
|
// Setup escIndex pin to send data, pullup is the default
|
||||||
|
ESC_OUTPUT(escIndex);
|
||||||
|
// Write the first bit
|
||||||
|
ESC_SET_LO(escIndex);
|
||||||
|
// Echo on the programmer tx line
|
||||||
|
TX_SET_LO;
|
||||||
|
//set LEDs
|
||||||
|
RX_LED_OFF;
|
||||||
|
TX_LED_ON;
|
||||||
|
// Wait for programmer to go 0 -> 1
|
||||||
|
uint32_t ct=3333;
|
||||||
|
while(!RX_HI) {
|
||||||
|
ct--;
|
||||||
|
// check for low time ->ct=3333; //~600uS //byte Lo time for 0 @ 19200 baud -> 8*52 uS => 416uS
|
||||||
|
// App must send a 0 at 9600 baud (or lower) which has a LO time of at 104uS (or more)
|
||||||
|
if (ct==0) {
|
||||||
|
// Set ESC line high again //restore Input with pull up
|
||||||
|
ESC_INPUT(escIndex);
|
||||||
|
// Programmer TX
|
||||||
|
gpio_set_mode(S1W_TX_GPIO, S1W_TX_PIN, Mode_AF_PP);
|
||||||
|
// Enable Hardware UART
|
||||||
|
enable_hardware_uart;
|
||||||
|
// Wait a bit more (todo check if necessary...))
|
||||||
|
delay(50);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// Programmer is high, end of bit
|
||||||
|
// At first Echo to the esc, which helps to charge input capacities at ESC
|
||||||
|
ESC_SET_HI(escIndex);
|
||||||
|
// Listen to the escIndex, input mode, pullup resistor is on
|
||||||
|
gpio_set_mode(escHardware[escIndex].gpio, (1U << escHardware[escIndex].pinpos), Mode_IPU);
|
||||||
|
TX_LED_OFF;
|
||||||
|
// Listen to the escIndex while there is no data from the programmer
|
||||||
|
while (RX_HI) {
|
||||||
|
if (ESC_HI(escIndex)) {
|
||||||
|
TX_SET_HIGH;
|
||||||
|
RX_LED_OFF;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
TX_SET_LO;
|
||||||
|
RX_LED_ON;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
32
src/main/io/serial_1wire.h
Normal file
32
src/main/io/serial_1wire.h
Normal file
|
@ -0,0 +1,32 @@
|
||||||
|
/*
|
||||||
|
* 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/>.
|
||||||
|
*
|
||||||
|
* Ported from https://github.com/4712/BLHeliSuite/blob/master/Interfaces/Arduino1Wire/Source/Arduino1Wire_C/Arduino1Wire.c
|
||||||
|
* by Nathan Tsoi <nathan@vertile.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#ifdef USE_SERIAL_1WIRE
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
GPIO_TypeDef* gpio;
|
||||||
|
uint32_t pinpos;
|
||||||
|
} escHardware_t;
|
||||||
|
|
||||||
|
void usb1WireInitialize();
|
||||||
|
void usb1WirePassthrough(int8_t escIndex);
|
||||||
|
#endif
|
|
@ -51,6 +51,7 @@
|
||||||
#include "io/gimbal.h"
|
#include "io/gimbal.h"
|
||||||
#include "io/rc_controls.h"
|
#include "io/rc_controls.h"
|
||||||
#include "io/serial.h"
|
#include "io/serial.h"
|
||||||
|
#include "io/serial_1wire.h"
|
||||||
#include "io/ledstrip.h"
|
#include "io/ledstrip.h"
|
||||||
#include "io/flashfs.h"
|
#include "io/flashfs.h"
|
||||||
#include "io/beeper.h"
|
#include "io/beeper.h"
|
||||||
|
@ -144,6 +145,10 @@ static void cliFlashRead(char *cmdline);
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_SERIAL_1WIRE
|
||||||
|
static void cliUSB1Wire(char *cmdline);
|
||||||
|
#endif
|
||||||
|
|
||||||
// buffer
|
// buffer
|
||||||
static char cliBuffer[48];
|
static char cliBuffer[48];
|
||||||
static uint32_t bufferIndex = 0;
|
static uint32_t bufferIndex = 0;
|
||||||
|
@ -216,6 +221,9 @@ typedef struct {
|
||||||
|
|
||||||
// should be sorted a..z for bsearch()
|
// should be sorted a..z for bsearch()
|
||||||
const clicmd_t cmdTable[] = {
|
const clicmd_t cmdTable[] = {
|
||||||
|
#ifdef USE_SERIAL_1WIRE
|
||||||
|
CLI_COMMAND_DEF("1wire", "1-wire interface to escs", "<esc index>", cliUSB1Wire),
|
||||||
|
#endif
|
||||||
CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange),
|
CLI_COMMAND_DEF("adjrange", "configure adjustment ranges", NULL, cliAdjustmentRange),
|
||||||
CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux),
|
CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux),
|
||||||
#ifdef LED_STRIP
|
#ifdef LED_STRIP
|
||||||
|
@ -2110,6 +2118,30 @@ static void cliStatus(char *cmdline)
|
||||||
printf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t));
|
printf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef USE_SERIAL_1WIRE
|
||||||
|
static void cliUSB1Wire(char *cmdline)
|
||||||
|
{
|
||||||
|
int i;
|
||||||
|
|
||||||
|
if (isEmpty(cmdline)) {
|
||||||
|
cliPrint("Please specify a ouput channel. e.g. `1wire 2` to connect to motor 2\r\n");
|
||||||
|
return;
|
||||||
|
} else {
|
||||||
|
i = atoi(cmdline);
|
||||||
|
if (i >= 0 && i <= ESC_COUNT) {
|
||||||
|
printf("Switching to BlHeli mode on motor port %d\r\n", i);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
printf("Invalid motor port, valid range: 1 to %d\r\n", ESC_COUNT);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
UNUSED(cmdline);
|
||||||
|
StopPwmAllMotors();
|
||||||
|
// motor 1 => index 0
|
||||||
|
usb1WirePassthrough(i-1);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
static void cliVersion(char *cmdline)
|
static void cliVersion(char *cmdline)
|
||||||
{
|
{
|
||||||
UNUSED(cmdline);
|
UNUSED(cmdline);
|
||||||
|
|
|
@ -85,6 +85,9 @@
|
||||||
|
|
||||||
#include "serial_msp.h"
|
#include "serial_msp.h"
|
||||||
|
|
||||||
|
#ifdef USE_SERIAL_1WIRE
|
||||||
|
#include "io/serial_1wire.h"
|
||||||
|
#endif
|
||||||
static serialPort_t *mspSerialPort;
|
static serialPort_t *mspSerialPort;
|
||||||
|
|
||||||
extern uint16_t cycleTime; // FIXME dependency on mw.c
|
extern uint16_t cycleTime; // FIXME dependency on mw.c
|
||||||
|
@ -308,6 +311,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
||||||
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
|
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
|
||||||
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
|
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
|
||||||
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
|
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
|
||||||
|
#define MSP_SET_1WIRE 243 //in message Sets 1Wire paththrough
|
||||||
|
|
||||||
#define INBUF_SIZE 64
|
#define INBUF_SIZE 64
|
||||||
|
|
||||||
|
@ -1721,6 +1725,47 @@ static bool processInCommand(void)
|
||||||
isRebootScheduled = true;
|
isRebootScheduled = true;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
#ifdef USE_SERIAL_1WIRE
|
||||||
|
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
|
||||||
|
usb1WireInitialize();
|
||||||
|
// and come back rigth afterwards
|
||||||
|
// rem: App: Wait at least appx. 500 ms for BLHeli to jump into bootloader mode before try to connect any ESC
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// Check for channel number 0..ESC_COUNT-1
|
||||||
|
if (i < ESC_COUNT) {
|
||||||
|
// because we do not come back after calling usb1WirePassthrough
|
||||||
|
// proceed a success reply first
|
||||||
|
headSerialReply(0);
|
||||||
|
tailSerialReply();
|
||||||
|
//wait for all data is send
|
||||||
|
while (!isSerialTransmitBufferEmpty(mspSerialPort)) {
|
||||||
|
delay(50);
|
||||||
|
}
|
||||||
|
// Start to activate here
|
||||||
|
// motor 1 => index 0
|
||||||
|
usb1WirePassthrough(i);
|
||||||
|
// MPS uart is active again
|
||||||
|
} else {
|
||||||
|
// ESC channel higher than max. allowed
|
||||||
|
// rem: BLHelilSuite will not support more than 8
|
||||||
|
// Client should check active Motors before preinitialize the Passthrough
|
||||||
|
// with MSP_MOTOR and check each value >0 and later call only active channels
|
||||||
|
// rem: atm not allowed channel mapping other than 0..x ascending
|
||||||
|
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
|
||||||
default:
|
default:
|
||||||
// we do not know how to handle the (valid) message, indicate error MSP $M!
|
// we do not know how to handle the (valid) message, indicate error MSP $M!
|
||||||
return false;
|
return false;
|
||||||
|
|
|
@ -116,6 +116,19 @@
|
||||||
#define USE_SERVOS
|
#define USE_SERVOS
|
||||||
#define USE_CLI
|
#define USE_CLI
|
||||||
|
|
||||||
|
#define USE_SERIAL_1WIRE
|
||||||
|
// How many escs does this board support?
|
||||||
|
#define ESC_COUNT 6
|
||||||
|
|
||||||
|
// 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
|
||||||
|
#define S1W_TX_GPIO GPIOB
|
||||||
|
#define S1W_TX_PIN GPIO_Pin_10
|
||||||
|
// JST Pin4 RX - connect to external UART/USB TX
|
||||||
|
#define S1W_RX_GPIO GPIOB
|
||||||
|
#define S1W_RX_PIN GPIO_Pin_11
|
||||||
|
|
||||||
#if defined(OPBL)
|
#if defined(OPBL)
|
||||||
// disabled some features for OPBL build due to code size.
|
// disabled some features for OPBL build due to code size.
|
||||||
#undef AUTOTUNE
|
#undef AUTOTUNE
|
||||||
|
@ -124,6 +137,10 @@
|
||||||
#define SKIP_CLI_COMMAND_HELP
|
#define SKIP_CLI_COMMAND_HELP
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if defined(OPBL) && defined(USE_SERIAL_1WIRE)
|
||||||
|
#undef DISPLAY
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
#define SPEKTRUM_BIND
|
#define SPEKTRUM_BIND
|
||||||
// USART3, PB11 (Flexport)
|
// USART3, PB11 (Flexport)
|
||||||
|
|
|
@ -183,6 +183,16 @@
|
||||||
#define BIND_PORT GPIOA
|
#define BIND_PORT GPIOA
|
||||||
#define BIND_PIN Pin_3
|
#define BIND_PIN Pin_3
|
||||||
|
|
||||||
|
#define USE_SERIAL_1WIRE
|
||||||
|
// How many escs does this board support?
|
||||||
|
#define ESC_COUNT 6
|
||||||
|
// 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
|
||||||
|
|
||||||
// alternative defaults for AlienWii32 F1 target
|
// alternative defaults for AlienWii32 F1 target
|
||||||
#ifdef ALIENWII32
|
#ifdef ALIENWII32
|
||||||
#undef TARGET_BOARD_IDENTIFIER
|
#undef TARGET_BOARD_IDENTIFIER
|
||||||
|
|
|
@ -169,3 +169,10 @@
|
||||||
// USART3,
|
// USART3,
|
||||||
#define BIND_PORT GPIOB
|
#define BIND_PORT GPIOB
|
||||||
#define BIND_PIN Pin_11
|
#define BIND_PIN Pin_11
|
||||||
|
|
||||||
|
#define USE_SERIAL_1WIRE
|
||||||
|
#define ESC_COUNT 8
|
||||||
|
#define S1W_TX_GPIO GPIOA
|
||||||
|
#define S1W_TX_PIN GPIO_Pin_9
|
||||||
|
#define S1W_RX_GPIO GPIOA
|
||||||
|
#define S1W_RX_PIN GPIO_Pin_10
|
||||||
|
|
|
@ -98,3 +98,13 @@
|
||||||
#define AUTOTUNE
|
#define AUTOTUNE
|
||||||
#define USE_SERVOS
|
#define USE_SERVOS
|
||||||
#define USE_CLI
|
#define USE_CLI
|
||||||
|
|
||||||
|
#define USE_SERIAL_1WIRE
|
||||||
|
// How many escs does this board support?
|
||||||
|
#define ESC_COUNT 6
|
||||||
|
// STM32F3DISCOVERY TX - PC3 connects to UART RX
|
||||||
|
#define S1W_TX_GPIO GPIOC
|
||||||
|
#define S1W_TX_PIN GPIO_Pin_3
|
||||||
|
// STM32F3DISCOVERY RX - PC1 connects to UART TX
|
||||||
|
#define S1W_RX_GPIO GPIOC
|
||||||
|
#define S1W_RX_PIN GPIO_Pin_1
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue