mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 11:29:58 +03:00
1008 lines
30 KiB
C
1008 lines
30 KiB
C
/*
|
|
* This file is part of Cleanflight and Betaflight.
|
|
*
|
|
* Cleanflight and Betaflight are free software. You can redistribute
|
|
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
|
|
* 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 this software.
|
|
*
|
|
* If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#include "platform.h"
|
|
|
|
#if defined(USE_ESCSERIAL)
|
|
|
|
#include "build/build_config.h"
|
|
#include "build/atomic.h"
|
|
|
|
#include "common/utils.h"
|
|
|
|
#include "drivers/io.h"
|
|
#include "drivers/light_led.h"
|
|
#include "drivers/nvic.h"
|
|
#include "drivers/pwm_output.h"
|
|
#include "drivers/serial.h"
|
|
#include "drivers/serial_escserial.h"
|
|
#include "drivers/time.h"
|
|
#include "drivers/timer.h"
|
|
|
|
#include "flight/mixer.h"
|
|
|
|
#include "io/serial.h"
|
|
|
|
#include "pg/motor.h"
|
|
|
|
typedef enum {
|
|
BAUDRATE_NORMAL = 19200,
|
|
BAUDRATE_SIMONK = 28800, // = 9600 * 3
|
|
BAUDRATE_KISS = 38400,
|
|
BAUDRATE_CASTLE = 18880
|
|
} escBaudRate_e;
|
|
|
|
#define RX_TOTAL_BITS 10
|
|
#define TX_TOTAL_BITS 10
|
|
|
|
#define MAX_ESCSERIAL_PORTS 1
|
|
static serialPort_t *escPort = NULL;
|
|
static serialPort_t *passPort = NULL;
|
|
|
|
#define ICPOLARITY_RISING true
|
|
#define ICPOLARITY_FALLING false
|
|
|
|
typedef struct escSerial_s {
|
|
serialPort_t port;
|
|
|
|
IO_t rxIO;
|
|
IO_t txIO;
|
|
|
|
const timerHardware_t *rxTimerHardware;
|
|
volatile uint8_t rxBuffer[ESCSERIAL_BUFFER_SIZE];
|
|
const timerHardware_t *txTimerHardware;
|
|
volatile uint8_t txBuffer[ESCSERIAL_BUFFER_SIZE];
|
|
|
|
#ifdef USE_HAL_DRIVER
|
|
const TIM_HandleTypeDef *txTimerHandle;
|
|
const TIM_HandleTypeDef *rxTimerHandle;
|
|
#endif
|
|
|
|
uint8_t isSearchingForStartBit;
|
|
uint8_t rxBitIndex;
|
|
uint8_t rxLastLeadingEdgeAtBitIndex;
|
|
uint8_t rxEdge;
|
|
|
|
uint8_t isTransmittingData;
|
|
uint8_t isReceivingData;
|
|
int8_t bitsLeftToTransmit;
|
|
|
|
uint16_t internalTxBuffer; // includes start and stop bits
|
|
uint16_t internalRxBuffer; // includes start and stop bits
|
|
|
|
uint16_t receiveTimeout;
|
|
uint16_t transmissionErrors;
|
|
uint16_t receiveErrors;
|
|
|
|
uint8_t escSerialPortIndex;
|
|
uint8_t mode;
|
|
uint8_t outputCount;
|
|
|
|
timerCCHandlerRec_t timerCb;
|
|
timerCCHandlerRec_t edgeCb;
|
|
} escSerial_t;
|
|
|
|
typedef struct {
|
|
IO_t io;
|
|
uint8_t inverted;
|
|
} escOutputs_t;
|
|
|
|
escOutputs_t escOutputs[MAX_SUPPORTED_MOTORS];
|
|
|
|
//extern timerHardware_t* serialTimerHardware;
|
|
|
|
const struct serialPortVTable escSerialVTable[];
|
|
|
|
escSerial_t escSerialPorts[MAX_ESCSERIAL_PORTS];
|
|
|
|
PG_REGISTER_WITH_RESET_TEMPLATE(escSerialConfig_t, escSerialConfig, PG_ESCSERIAL_CONFIG, 0);
|
|
|
|
#ifndef ESCSERIAL_PIN
|
|
#define ESCSERIAL_PIN NONE
|
|
#endif
|
|
|
|
PG_RESET_TEMPLATE(escSerialConfig_t, escSerialConfig,
|
|
.ioTag = IO_TAG(ESCSERIAL_PIN),
|
|
);
|
|
|
|
enum {
|
|
TRAILING,
|
|
LEADING
|
|
};
|
|
|
|
#define STOP_BIT_MASK (1 << 0)
|
|
#define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1))
|
|
|
|
static void setTxSignalEsc(escSerial_t *escSerial, uint8_t state)
|
|
{
|
|
if (escSerial->mode == PROTOCOL_KISSALL)
|
|
{
|
|
for (volatile uint8_t i = 0; i < escSerial->outputCount; i++) {
|
|
uint8_t state_temp = state;
|
|
if (escOutputs[i].inverted) {
|
|
state_temp ^= ENABLE;
|
|
}
|
|
|
|
if (state_temp) {
|
|
IOHi(escOutputs[i].io);
|
|
} else {
|
|
IOLo(escOutputs[i].io);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (escSerial->rxTimerHardware->output & TIMER_OUTPUT_INVERTED) {
|
|
state ^= ENABLE;
|
|
}
|
|
|
|
if (state) {
|
|
IOHi(escSerial->txIO);
|
|
} else {
|
|
IOLo(escSerial->txIO);
|
|
}
|
|
}
|
|
}
|
|
|
|
static void escSerialGPIOConfig(const timerHardware_t *timhw, ioConfig_t cfg)
|
|
{
|
|
ioTag_t tag = timhw->tag;
|
|
|
|
if (!tag) {
|
|
return;
|
|
}
|
|
|
|
IOInit(IOGetByTag(tag), OWNER_MOTOR, 0);
|
|
IOConfigGPIOAF(IOGetByTag(tag), cfg, timhw->alternateFunction);
|
|
}
|
|
|
|
static void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr)
|
|
{
|
|
escSerialGPIOConfig(timerHardwarePtr, IOCFG_AF_PP_UP);
|
|
timerChClearCCFlag(timerHardwarePtr);
|
|
timerChITConfig(timerHardwarePtr, ENABLE);
|
|
}
|
|
|
|
static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
|
|
{
|
|
return timerPeriod > 0xFFFF;
|
|
}
|
|
|
|
static bool isEscSerialTransmitBufferEmpty(const serialPort_t *instance)
|
|
{
|
|
// start listening
|
|
return instance->txBufferHead == instance->txBufferTail;
|
|
}
|
|
|
|
static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
|
|
{
|
|
escSerialGPIOConfig(timerHardwarePtr, IOCFG_OUT_PP);
|
|
timerChITConfig(timerHardwarePtr, DISABLE);
|
|
}
|
|
|
|
static void processTxStateBL(escSerial_t *escSerial)
|
|
{
|
|
uint8_t mask;
|
|
if (escSerial->isReceivingData) {
|
|
return;
|
|
}
|
|
|
|
if (!escSerial->isTransmittingData) {
|
|
char byteToSend;
|
|
if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
|
|
// canreceive
|
|
return;
|
|
}
|
|
|
|
// data to send
|
|
byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++];
|
|
if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) {
|
|
escSerial->port.txBufferTail = 0;
|
|
}
|
|
|
|
// build internal buffer, MSB = Stop Bit (1) + data bits (MSB to LSB) + start bit(0) LSB
|
|
escSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1);
|
|
escSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
|
|
escSerial->isTransmittingData = true;
|
|
|
|
//set output
|
|
if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE) {
|
|
escSerialOutputPortConfig(escSerial->rxTimerHardware);
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (escSerial->bitsLeftToTransmit) {
|
|
mask = escSerial->internalTxBuffer & 1;
|
|
escSerial->internalTxBuffer >>= 1;
|
|
|
|
setTxSignalEsc(escSerial, mask);
|
|
escSerial->bitsLeftToTransmit--;
|
|
return;
|
|
}
|
|
|
|
escSerial->isTransmittingData = false;
|
|
if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
|
|
if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE)
|
|
{
|
|
escSerialInputPortConfig(escSerial->rxTimerHardware);
|
|
}
|
|
}
|
|
}
|
|
|
|
static void extractAndStoreRxByteBL(escSerial_t *escSerial)
|
|
{
|
|
if ((escSerial->port.mode & MODE_RX) == 0) {
|
|
return;
|
|
}
|
|
|
|
uint8_t haveStartBit = (escSerial->internalRxBuffer & START_BIT_MASK) == 0;
|
|
uint8_t haveStopBit = (escSerial->internalRxBuffer & STOP_BIT_MASK) == 1;
|
|
|
|
if (!haveStartBit || !haveStopBit) {
|
|
escSerial->receiveErrors++;
|
|
return;
|
|
}
|
|
|
|
uint8_t rxByte = (escSerial->internalRxBuffer >> 1) & 0xFF;
|
|
|
|
if (escSerial->port.rxCallback) {
|
|
escSerial->port.rxCallback(rxByte, escSerial->port.rxCallbackData);
|
|
} else {
|
|
escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
|
|
escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
|
|
}
|
|
}
|
|
|
|
static void prepareForNextRxByteBL(escSerial_t *escSerial)
|
|
{
|
|
// prepare for next byte
|
|
escSerial->rxBitIndex = 0;
|
|
escSerial->isSearchingForStartBit = true;
|
|
if (escSerial->rxEdge == LEADING) {
|
|
escSerial->rxEdge = TRAILING;
|
|
timerChConfigIC(
|
|
escSerial->rxTimerHardware,
|
|
(escSerial->port.options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0
|
|
);
|
|
}
|
|
}
|
|
|
|
static void applyChangedBitsBL(escSerial_t *escSerial)
|
|
{
|
|
if (escSerial->rxEdge == TRAILING) {
|
|
uint8_t bitToSet;
|
|
for (bitToSet = escSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < escSerial->rxBitIndex; bitToSet++) {
|
|
escSerial->internalRxBuffer |= 1 << bitToSet;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void processRxStateBL(escSerial_t *escSerial)
|
|
{
|
|
if (escSerial->isSearchingForStartBit) {
|
|
return;
|
|
}
|
|
|
|
escSerial->rxBitIndex++;
|
|
|
|
if (escSerial->rxBitIndex == RX_TOTAL_BITS - 1) {
|
|
applyChangedBitsBL(escSerial);
|
|
return;
|
|
}
|
|
|
|
if (escSerial->rxBitIndex == RX_TOTAL_BITS) {
|
|
|
|
if (escSerial->rxEdge == TRAILING) {
|
|
escSerial->internalRxBuffer |= STOP_BIT_MASK;
|
|
}
|
|
|
|
extractAndStoreRxByteBL(escSerial);
|
|
prepareForNextRxByteBL(escSerial);
|
|
}
|
|
}
|
|
|
|
static void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
|
|
{
|
|
UNUSED(capture);
|
|
escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
|
|
|
|
processTxStateBL(escSerial);
|
|
processRxStateBL(escSerial);
|
|
}
|
|
|
|
static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t portIndex, uint32_t baud)
|
|
{
|
|
uint32_t clock = SystemCoreClock/2;
|
|
uint32_t timerPeriod;
|
|
timerReset(timerHardwarePtr->tim);
|
|
do {
|
|
timerPeriod = clock / baud;
|
|
if (isTimerPeriodTooLarge(timerPeriod)) {
|
|
if (clock > 1) {
|
|
clock = clock / 2; // this is wrong - mhz stays the same ... This will double baudrate until ok (but minimum baudrate is < 1200)
|
|
} else {
|
|
// TODO unable to continue, unable to determine clock and timerPeriods for the given baud
|
|
}
|
|
|
|
}
|
|
} while (isTimerPeriodTooLarge(timerPeriod));
|
|
|
|
timerReconfigureTimeBase(timerHardwarePtr->tim, timerPeriod, clock);
|
|
timerChCCHandlerInit(&escSerialPorts[portIndex].timerCb, onSerialTimerBL);
|
|
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[portIndex].timerCb, NULL);
|
|
timerStart(timerHardwarePtr->tim);
|
|
}
|
|
|
|
static void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
|
|
{
|
|
UNUSED(capture);
|
|
|
|
escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb);
|
|
bool inverted = escSerial->port.options & SERIAL_INVERTED;
|
|
|
|
if ((escSerial->port.mode & MODE_RX) == 0) {
|
|
return;
|
|
}
|
|
|
|
if (escSerial->isSearchingForStartBit) {
|
|
// Adjust the timing so it will interrupt on the middle.
|
|
// This is clobbers transmission, but it is okay because we are
|
|
// always half-duplex.
|
|
timerSetCounter(escSerial->txTimerHardware->tim, timerGetPeriod(escSerial->txTimerHardware->tim) / 2);
|
|
|
|
if (escSerial->isTransmittingData) {
|
|
escSerial->transmissionErrors++;
|
|
}
|
|
|
|
timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0);
|
|
escSerial->rxEdge = LEADING;
|
|
|
|
escSerial->rxBitIndex = 0;
|
|
escSerial->rxLastLeadingEdgeAtBitIndex = 0;
|
|
escSerial->internalRxBuffer = 0;
|
|
escSerial->isSearchingForStartBit = false;
|
|
return;
|
|
}
|
|
|
|
if (escSerial->rxEdge == LEADING) {
|
|
escSerial->rxLastLeadingEdgeAtBitIndex = escSerial->rxBitIndex;
|
|
}
|
|
|
|
applyChangedBitsBL(escSerial);
|
|
|
|
if (escSerial->rxEdge == TRAILING) {
|
|
escSerial->rxEdge = LEADING;
|
|
timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0);
|
|
} else {
|
|
escSerial->rxEdge = TRAILING;
|
|
timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0);
|
|
}
|
|
}
|
|
|
|
static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_e options)
|
|
{
|
|
// start bit is usually a FALLING signal
|
|
timerReset(timerHardwarePtr->tim);
|
|
timerReconfigureTimeBase(timerHardwarePtr->tim, 0xFFFF, SystemCoreClock / 2);
|
|
timerChConfigIC(timerHardwarePtr, (options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0);
|
|
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL);
|
|
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
|
|
timerStart(timerHardwarePtr->tim);
|
|
}
|
|
|
|
#ifdef USE_ESCSERIAL_SIMONK
|
|
static void processTxStateEsc(escSerial_t *escSerial)
|
|
{
|
|
uint8_t mask;
|
|
static uint8_t bitq=0, transmitStart=0;
|
|
if (escSerial->isReceivingData) {
|
|
return;
|
|
}
|
|
|
|
if (transmitStart==0)
|
|
{
|
|
setTxSignalEsc(escSerial, 1);
|
|
}
|
|
if (!escSerial->isTransmittingData) {
|
|
char byteToSend;
|
|
reload:
|
|
if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
|
|
// canreceive
|
|
transmitStart=0;
|
|
return;
|
|
}
|
|
|
|
if (transmitStart<3)
|
|
{
|
|
if (transmitStart==0)
|
|
byteToSend = 0xff;
|
|
if (transmitStart==1)
|
|
byteToSend = 0xff;
|
|
if (transmitStart==2)
|
|
byteToSend = 0x7f;
|
|
transmitStart++;
|
|
}
|
|
else{
|
|
// data to send
|
|
byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++];
|
|
if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) {
|
|
escSerial->port.txBufferTail = 0;
|
|
}
|
|
}
|
|
|
|
// build internal buffer, data bits (MSB to LSB)
|
|
escSerial->internalTxBuffer = byteToSend;
|
|
escSerial->bitsLeftToTransmit = 8;
|
|
escSerial->isTransmittingData = true;
|
|
|
|
//set output
|
|
escSerialOutputPortConfig(escSerial->rxTimerHardware);
|
|
return;
|
|
}
|
|
|
|
if (escSerial->bitsLeftToTransmit) {
|
|
mask = escSerial->internalTxBuffer & 1;
|
|
if (mask)
|
|
{
|
|
if (bitq==0 || bitq==1)
|
|
{
|
|
setTxSignalEsc(escSerial, 1);
|
|
}
|
|
if (bitq==2 || bitq==3)
|
|
{
|
|
setTxSignalEsc(escSerial, 0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (bitq==0 || bitq==2)
|
|
{
|
|
setTxSignalEsc(escSerial, 1);
|
|
}
|
|
if (bitq==1 ||bitq==3)
|
|
{
|
|
setTxSignalEsc(escSerial, 0);
|
|
}
|
|
}
|
|
bitq++;
|
|
if (bitq>3)
|
|
{
|
|
escSerial->internalTxBuffer >>= 1;
|
|
escSerial->bitsLeftToTransmit--;
|
|
bitq=0;
|
|
if (escSerial->bitsLeftToTransmit==0)
|
|
{
|
|
goto reload;
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
|
|
escSerial->isTransmittingData = false;
|
|
escSerialInputPortConfig(escSerial->rxTimerHardware);
|
|
}
|
|
}
|
|
|
|
static void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
|
|
{
|
|
UNUSED(capture);
|
|
escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
|
|
|
|
if (escSerial->isReceivingData)
|
|
{
|
|
escSerial->receiveTimeout++;
|
|
if (escSerial->receiveTimeout>8)
|
|
{
|
|
escSerial->isReceivingData=0;
|
|
escSerial->receiveTimeout=0;
|
|
timerChConfigIC(escSerial->rxTimerHardware, ICPOLARITY_FALLING, 0);
|
|
}
|
|
}
|
|
|
|
processTxStateEsc(escSerial);
|
|
}
|
|
|
|
static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
|
|
{
|
|
uint32_t timerPeriod = 34;
|
|
timerReset(timerHardwarePtr->tim);
|
|
timerReconfigureTimeBase(timerHardwarePtr->tim, timerPeriod, MHZ_TO_HZ(1));
|
|
timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
|
|
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
|
|
timerStart(timerHardwarePtr->tim);
|
|
}
|
|
|
|
static void extractAndStoreRxByteEsc(escSerial_t *escSerial)
|
|
{
|
|
if ((escSerial->port.mode & MODE_RX) == 0) {
|
|
return;
|
|
}
|
|
|
|
uint8_t rxByte = (escSerial->internalRxBuffer) & 0xFF;
|
|
|
|
if (escSerial->port.rxCallback) {
|
|
escSerial->port.rxCallback(rxByte, escSerial->port.rxCallbackData);
|
|
} else {
|
|
escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
|
|
escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
|
|
}
|
|
}
|
|
|
|
static void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
|
|
{
|
|
UNUSED(capture);
|
|
static uint8_t zerofirst=0;
|
|
static uint8_t bits=0;
|
|
static uint16_t bytes=0;
|
|
|
|
escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb);
|
|
|
|
timerSetCounter(escSerial->rxTimerHardware->tim, 0);
|
|
|
|
if (capture > 40 && capture < 90)
|
|
{
|
|
zerofirst++;
|
|
if (zerofirst>1)
|
|
{
|
|
zerofirst=0;
|
|
escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1;
|
|
bits++;
|
|
}
|
|
}
|
|
else if (capture>90 && capture < 200)
|
|
{
|
|
zerofirst=0;
|
|
escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1;
|
|
escSerial->internalRxBuffer |= 0x80;
|
|
bits++;
|
|
}
|
|
else
|
|
{
|
|
if (!escSerial->isReceivingData)
|
|
{
|
|
//start
|
|
//lets reset
|
|
|
|
escSerial->isReceivingData = 1;
|
|
zerofirst=0;
|
|
bytes=0;
|
|
bits=1;
|
|
escSerial->internalRxBuffer = 0x80;
|
|
|
|
timerChConfigIC(escSerial->rxTimerHardware, ICPOLARITY_RISING, 0);
|
|
}
|
|
}
|
|
escSerial->receiveTimeout = 0;
|
|
|
|
if (bits==8)
|
|
{
|
|
bits=0;
|
|
bytes++;
|
|
if (bytes>3)
|
|
{
|
|
extractAndStoreRxByteEsc(escSerial);
|
|
}
|
|
escSerial->internalRxBuffer=0;
|
|
}
|
|
|
|
}
|
|
|
|
static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
|
|
{
|
|
// start bit is usually a FALLING signal
|
|
timerReset(timerHardwarePtr->tim);
|
|
timerReconfigureTimeBase(timerHardwarePtr->tim, 0xFFFF, MHZ_TO_HZ(1));
|
|
timerChConfigIC(timerHardwarePtr, ICPOLARITY_FALLING, 0);
|
|
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc);
|
|
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
|
|
timerStart(timerHardwarePtr->tim);
|
|
}
|
|
#endif
|
|
|
|
static void resetBuffers(escSerial_t *escSerial)
|
|
{
|
|
escSerial->port.rxBufferSize = ESCSERIAL_BUFFER_SIZE;
|
|
escSerial->port.rxBuffer = escSerial->rxBuffer;
|
|
escSerial->port.rxBufferTail = 0;
|
|
escSerial->port.rxBufferHead = 0;
|
|
|
|
escSerial->port.txBuffer = escSerial->txBuffer;
|
|
escSerial->port.txBufferSize = ESCSERIAL_BUFFER_SIZE;
|
|
escSerial->port.txBufferTail = 0;
|
|
escSerial->port.txBufferHead = 0;
|
|
}
|
|
|
|
static serialPort_t *openEscSerial(const motorDevConfig_t *motorConfig, escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_e options, uint8_t mode)
|
|
{
|
|
escSerial_t *escSerial = &(escSerialPorts[portIndex]);
|
|
|
|
if (mode != PROTOCOL_KISSALL) {
|
|
|
|
if (escSerialConfig()->ioTag == IO_TAG_NONE) {
|
|
return NULL;
|
|
}
|
|
|
|
const ioTag_t tag = motorConfig->ioTags[output];
|
|
const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, 0);
|
|
|
|
if (timerHardware == NULL) {
|
|
return NULL;
|
|
}
|
|
|
|
escSerial->rxTimerHardware = timerHardware;
|
|
// N-Channels can't be used as RX.
|
|
if (escSerial->rxTimerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
|
|
return NULL;
|
|
}
|
|
|
|
#ifdef USE_HAL_DRIVER
|
|
escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim);
|
|
#endif
|
|
// Workaround to ensure that the timerHandle is configured before use, timer will be reconfigured to a different frequency below
|
|
// this prevents a null-pointer dereference in __HAL_TIM_CLEAR_FLAG called by timerChClearCCFlag and similar accesses of timerHandle without the Instance being configured first.
|
|
timerConfigure(escSerial->rxTimerHardware, 0xffff, 1);
|
|
|
|
escSerial->txTimerHardware = timerAllocate(escSerialConfig()->ioTag, OWNER_MOTOR, 0);
|
|
if (escSerial->txTimerHardware == NULL) {
|
|
return NULL;
|
|
}
|
|
|
|
#ifdef USE_HAL_DRIVER
|
|
escSerial->txTimerHandle = timerFindTimerHandle(escSerial->txTimerHardware->tim);
|
|
#endif
|
|
|
|
// Workaround to ensure that the timerHandle is configured before use, timer will be reconfigured to a different frequency below
|
|
// this prevents a null-pointer dereference in __HAL_TIM_CLEAR_FLAG called by timerChClearCCFlag and similar accesses of timerHandle without the Instance being configured first.
|
|
timerConfigure(escSerial->txTimerHardware, 0xffff, 1);
|
|
}
|
|
|
|
escSerial->mode = mode;
|
|
escSerial->port.vTable = escSerialVTable;
|
|
escSerial->port.baudRate = baud;
|
|
escSerial->port.mode = MODE_RXTX;
|
|
escSerial->port.options = options;
|
|
escSerial->port.rxCallback = callback;
|
|
|
|
resetBuffers(escSerial);
|
|
|
|
escSerial->isTransmittingData = false;
|
|
|
|
escSerial->isSearchingForStartBit = true;
|
|
escSerial->rxBitIndex = 0;
|
|
|
|
escSerial->transmissionErrors = 0;
|
|
escSerial->receiveErrors = 0;
|
|
escSerial->receiveTimeout = 0;
|
|
|
|
escSerial->escSerialPortIndex = portIndex;
|
|
|
|
if (mode != PROTOCOL_KISSALL)
|
|
{
|
|
escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag);
|
|
escSerialInputPortConfig(escSerial->rxTimerHardware);
|
|
setTxSignalEsc(escSerial, ENABLE);
|
|
}
|
|
delay(50);
|
|
|
|
#ifdef USE_ESCSERIAL_SIMONK
|
|
if (mode==PROTOCOL_SIMONK) {
|
|
escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex);
|
|
escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex);
|
|
}
|
|
else
|
|
#endif
|
|
if (mode==PROTOCOL_BLHELI) {
|
|
serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
|
|
serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
|
|
}
|
|
else if (mode==PROTOCOL_KISS) {
|
|
escSerialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used
|
|
serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
|
|
}
|
|
else if (mode==PROTOCOL_KISSALL) {
|
|
escSerial->outputCount = 0;
|
|
memset(&escOutputs, 0, sizeof(escOutputs));
|
|
pwmOutputPort_t *pwmMotors = pwmGetMotors();
|
|
for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
|
if (pwmMotors[i].enabled && pwmMotors[i].io != IO_NONE) {
|
|
const ioTag_t tag = motorConfig->ioTags[i];
|
|
if (tag != IO_TAG_NONE) {
|
|
const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, 0);
|
|
if (timerHardware) {
|
|
// Workaround to ensure that the timerHandle is configured before use, timer will be reconfigured to a different frequency below
|
|
// this prevents a null-pointer dereference in __HAL_TIM_CLEAR_FLAG called by timerChClearCCFlag and similar accesses of timerHandle without the Instance being configured first.
|
|
timerConfigure(timerHardware, 0xffff, 1);
|
|
escSerialOutputPortConfig(timerHardware);
|
|
escOutputs[escSerial->outputCount].io = pwmMotors[i].io;
|
|
if (timerHardware->output & TIMER_OUTPUT_INVERTED) {
|
|
escOutputs[escSerial->outputCount].inverted = 1;
|
|
}
|
|
escSerial->txTimerHardware = timerHardware;
|
|
escSerial->outputCount++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
setTxSignalEsc(escSerial, ENABLE);
|
|
serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
|
|
}
|
|
else if (mode == PROTOCOL_CASTLE) {
|
|
escSerialOutputPortConfig(escSerial->rxTimerHardware);
|
|
serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
|
|
serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
|
|
}
|
|
return &escSerial->port;
|
|
}
|
|
|
|
static void escSerialInputPortDeConfig(const timerHardware_t *timerHardwarePtr)
|
|
{
|
|
timerChClearCCFlag(timerHardwarePtr);
|
|
timerChITConfig(timerHardwarePtr,DISABLE);
|
|
escSerialGPIOConfig(timerHardwarePtr, IOCFG_IPU);
|
|
}
|
|
|
|
static void closeEscSerial(escSerialPortIndex_e portIndex, uint8_t mode)
|
|
{
|
|
escSerial_t *escSerial = &(escSerialPorts[portIndex]);
|
|
|
|
if (mode != PROTOCOL_KISSALL) {
|
|
escSerialInputPortDeConfig(escSerial->rxTimerHardware);
|
|
timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL);
|
|
timerReset(escSerial->rxTimerHardware->tim);
|
|
}
|
|
|
|
timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL);
|
|
timerReset(escSerial->txTimerHardware->tim);
|
|
}
|
|
|
|
static uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance)
|
|
{
|
|
if ((instance->mode & MODE_RX) == 0) {
|
|
return 0;
|
|
}
|
|
|
|
escSerial_t *s = (escSerial_t *)instance;
|
|
|
|
return (s->port.rxBufferHead - s->port.rxBufferTail) & (s->port.rxBufferSize - 1);
|
|
}
|
|
|
|
static uint8_t escSerialReadByte(serialPort_t *instance)
|
|
{
|
|
uint8_t ch;
|
|
|
|
if ((instance->mode & MODE_RX) == 0) {
|
|
return 0;
|
|
}
|
|
|
|
if (escSerialTotalBytesWaiting(instance) == 0) {
|
|
return 0;
|
|
}
|
|
|
|
ch = instance->rxBuffer[instance->rxBufferTail];
|
|
instance->rxBufferTail = (instance->rxBufferTail + 1) % instance->rxBufferSize;
|
|
return ch;
|
|
}
|
|
|
|
static void escSerialWriteByte(serialPort_t *s, uint8_t ch)
|
|
{
|
|
if ((s->mode & MODE_TX) == 0) {
|
|
return;
|
|
}
|
|
|
|
s->txBuffer[s->txBufferHead] = ch;
|
|
s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
|
|
}
|
|
|
|
static void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
|
|
{
|
|
UNUSED(s);
|
|
UNUSED(baudRate);
|
|
}
|
|
|
|
static void escSerialSetMode(serialPort_t *instance, portMode_e mode)
|
|
{
|
|
instance->mode = mode;
|
|
}
|
|
|
|
static uint32_t escSerialTxBytesFree(const serialPort_t *instance)
|
|
{
|
|
if ((instance->mode & MODE_TX) == 0) {
|
|
return 0;
|
|
}
|
|
|
|
escSerial_t *s = (escSerial_t *)instance;
|
|
|
|
uint8_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1);
|
|
|
|
return (s->port.txBufferSize - 1) - bytesUsed;
|
|
}
|
|
|
|
const struct serialPortVTable escSerialVTable[] = {
|
|
{
|
|
.serialWrite = escSerialWriteByte,
|
|
.serialTotalRxWaiting = escSerialTotalBytesWaiting,
|
|
.serialTotalTxFree = escSerialTxBytesFree,
|
|
.serialRead = escSerialReadByte,
|
|
.serialSetBaudRate = escSerialSetBaudRate,
|
|
.isSerialTransmitBufferEmpty = isEscSerialTransmitBufferEmpty,
|
|
.setMode = escSerialSetMode,
|
|
.setCtrlLineStateCb = NULL,
|
|
.setBaudRateCb = NULL,
|
|
.writeBuf = NULL,
|
|
.beginWrite = NULL,
|
|
.endWrite = NULL
|
|
}
|
|
};
|
|
|
|
typedef enum {
|
|
IDLE,
|
|
HEADER_START,
|
|
HEADER_M,
|
|
HEADER_ARROW,
|
|
HEADER_SIZE,
|
|
HEADER_CMD,
|
|
COMMAND_RECEIVED
|
|
} mspState_e;
|
|
|
|
typedef struct mspPort_s {
|
|
uint8_t offset;
|
|
uint8_t dataSize;
|
|
uint8_t checksum;
|
|
uint8_t indRX;
|
|
uint8_t inBuf[10];
|
|
mspState_e c_state;
|
|
uint8_t cmdMSP;
|
|
} mspPort_t;
|
|
|
|
static mspPort_t currentPort;
|
|
|
|
static bool processExitCommand(uint8_t c)
|
|
{
|
|
if (currentPort.c_state == IDLE) {
|
|
if (c == '$') {
|
|
currentPort.c_state = HEADER_START;
|
|
} else {
|
|
return false;
|
|
}
|
|
} else if (currentPort.c_state == HEADER_START) {
|
|
currentPort.c_state = (c == 'M') ? HEADER_M : IDLE;
|
|
} else if (currentPort.c_state == HEADER_M) {
|
|
currentPort.c_state = (c == '<') ? HEADER_ARROW : IDLE;
|
|
} else if (currentPort.c_state == HEADER_ARROW) {
|
|
if (c > 10) {
|
|
currentPort.c_state = IDLE;
|
|
|
|
} else {
|
|
currentPort.dataSize = c;
|
|
currentPort.offset = 0;
|
|
currentPort.checksum = 0;
|
|
currentPort.indRX = 0;
|
|
currentPort.checksum ^= c;
|
|
currentPort.c_state = HEADER_SIZE;
|
|
}
|
|
} else if (currentPort.c_state == HEADER_SIZE) {
|
|
currentPort.cmdMSP = c;
|
|
currentPort.checksum ^= c;
|
|
currentPort.c_state = HEADER_CMD;
|
|
} else if (currentPort.c_state == HEADER_CMD && currentPort.offset < currentPort.dataSize) {
|
|
currentPort.checksum ^= c;
|
|
currentPort.inBuf[currentPort.offset++] = c;
|
|
} else if (currentPort.c_state == HEADER_CMD && currentPort.offset >= currentPort.dataSize) {
|
|
if (currentPort.checksum == c) {
|
|
currentPort.c_state = COMMAND_RECEIVED;
|
|
|
|
if ((currentPort.cmdMSP == 0xF4) && (currentPort.dataSize==0))
|
|
{
|
|
currentPort.c_state = IDLE;
|
|
return true;
|
|
}
|
|
} else {
|
|
currentPort.c_state = IDLE;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool escEnablePassthrough(serialPort_t *escPassthroughPort, const motorDevConfig_t *motorConfig, uint16_t escIndex, uint8_t mode)
|
|
{
|
|
bool exitEsc = false;
|
|
uint8_t motor_output = escIndex;
|
|
LED0_OFF;
|
|
LED1_OFF;
|
|
|
|
motorDisable();
|
|
passPort = escPassthroughPort;
|
|
|
|
uint32_t escBaudrate;
|
|
switch (mode) {
|
|
case PROTOCOL_KISS:
|
|
escBaudrate = BAUDRATE_KISS;
|
|
break;
|
|
case PROTOCOL_CASTLE:
|
|
escBaudrate = BAUDRATE_CASTLE;
|
|
break;
|
|
default:
|
|
escBaudrate = BAUDRATE_NORMAL;
|
|
break;
|
|
}
|
|
|
|
if ((mode == PROTOCOL_KISS) && (motor_output == 255)) {
|
|
mode = PROTOCOL_KISSALL;
|
|
} else if (motor_output >= MAX_SUPPORTED_MOTORS) {
|
|
return false;
|
|
}
|
|
|
|
escPort = openEscSerial(motorConfig, ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode);
|
|
|
|
if (!escPort) {
|
|
return false;
|
|
}
|
|
|
|
uint8_t ch;
|
|
while (1) {
|
|
if (mode!=2)
|
|
{
|
|
if (serialRxBytesWaiting(escPort)) {
|
|
LED0_ON;
|
|
while (serialRxBytesWaiting(escPort))
|
|
{
|
|
ch = serialRead(escPort);
|
|
serialWrite(escPassthroughPort, ch);
|
|
}
|
|
LED0_OFF;
|
|
}
|
|
}
|
|
if (serialRxBytesWaiting(escPassthroughPort)) {
|
|
LED1_ON;
|
|
while (serialRxBytesWaiting(escPassthroughPort))
|
|
{
|
|
ch = serialRead(escPassthroughPort);
|
|
exitEsc = processExitCommand(ch);
|
|
if (exitEsc)
|
|
{
|
|
serialWrite(escPassthroughPort, 0x24);
|
|
serialWrite(escPassthroughPort, 0x4D);
|
|
serialWrite(escPassthroughPort, 0x3E);
|
|
serialWrite(escPassthroughPort, 0x00);
|
|
serialWrite(escPassthroughPort, 0xF4);
|
|
serialWrite(escPassthroughPort, 0xF4);
|
|
closeEscSerial(ESCSERIAL1, mode);
|
|
return true;
|
|
}
|
|
if (mode==PROTOCOL_BLHELI || mode==PROTOCOL_KISS || mode==PROTOCOL_KISSALL) {
|
|
serialWrite(escPassthroughPort, ch); // blheli loopback
|
|
}
|
|
serialWrite(escPort, ch);
|
|
}
|
|
LED1_OFF;
|
|
}
|
|
if (mode != PROTOCOL_CASTLE) {
|
|
delay(5);
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif
|