rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rebased onto #3469

This commit is contained in:
jflyper 2017-07-09 20:53:05 +09:00
parent 870f10e316 1ab83ee750
commit e30d9e4a29
10 changed files with 1520 additions and 515 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

825
src/main/drivers/serial_escserial.c
View File

@ -21,12 +21,6 @@
#include "platform.h"
typedef enum {
BAUDRATE_NORMAL = 19200,
BAUDRATE_KISS = 38400,
BAUDRATE_CASTLE = 18880
} escBaudRate_e;
#if defined(USE_ESCSERIAL)
#include "build/build_config.h"
@ -38,17 +32,25 @@ typedef enum {
#include "config/parameter_group_ids.h"
#include "drivers/io.h"
#include "drivers/nvic.h"
#include "drivers/time.h"
#include "timer.h"
#include "serial.h"
#include "serial_escserial.h"
#include "drivers/light_led.h"
#include "drivers/nvic.h"
#include "drivers/pwm_output.h"
#include "io/serial.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"
typedef enum {
BAUDRATE_NORMAL = 19200,
BAUDRATE_KISS = 38400,
BAUDRATE_CASTLE = 18880
} escBaudRate_e;
#define RX_TOTAL_BITS 10
#define TX_TOTAL_BITS 10
@ -107,9 +109,8 @@ typedef struct {
escOutputs_t escOutputs[MAX_SUPPORTED_MOTORS];
extern timerHardware_t* serialTimerHardware;
extern escSerial_t escSerialPorts[];
extern const struct serialPortVTable escSerialVTable[];
const struct serialPortVTable escSerialVTable[];
escSerial_t escSerialPorts[MAX_ESCSERIAL_PORTS];
@ -123,10 +124,13 @@ PG_RESET_TEMPLATE(escSerialConfig_t, escSerialConfig,
.ioTag = IO_TAG(ESCSERIAL_TIMER_TX_PIN),
);
void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
enum {
TRAILING,
LEADING
};
#define STOP_BIT_MASK (1 << 0)
#define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1))
// XXX No TIM_DeInit equivalent in HAL driver???
#ifdef USE_HAL_DRIVER
@ -136,7 +140,7 @@ static void TIM_DeInit(TIM_TypeDef *tim)
}
#endif
void setTxSignalEsc(escSerial_t *escSerial, uint8_t state)
static void setTxSignalEsc(escSerial_t *escSerial, uint8_t state)
{
if (escSerial->mode == PROTOCOL_KISSALL)
{
@ -183,7 +187,7 @@ static void escSerialGPIOConfig(const timerHardware_t *timhw, ioConfig_t cfg)
#endif
}
void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr)
static void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr)
{
#ifdef STM32F10X
escSerialGPIOConfig(timerHardwarePtr, IOCFG_IPU);
@ -200,6 +204,150 @@ 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);
} 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 reference, uint32_t baud)
{
uint32_t clock = SystemCoreClock/2;
@ -222,6 +370,55 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
}
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.
#ifdef USE_HAL_DRIVER
__HAL_TIM_SetCounter(escSerial->txTimerHandle, __HAL_TIM_GetAutoreload(escSerial->txTimerHandle) / 2);
#else
TIM_SetCounter(escSerial->txTimerHardware->tim, escSerial->txTimerHardware->tim->ARR / 2);
#endif
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_t options)
{
// start bit is usually a FALLING signal
@ -232,164 +429,7 @@ static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
}
static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
{
uint32_t timerPeriod = 34;
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, timerPeriod, MHZ_TO_HZ(1));
timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
}
static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
{
// start bit is usually a FALLING signal
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1));
timerChConfigIC(timerHardwarePtr, ICPOLARITY_FALLING, 0);
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
}
static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
{
escSerialGPIOConfig(timerHardwarePtr, IOCFG_OUT_PP);
timerChITConfig(timerHardwarePtr,DISABLE);
}
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;
}
serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode)
{
escSerial_t *escSerial = &(escSerialPorts[portIndex]);
if (mode != PROTOCOL_KISSALL) {
escSerial->rxTimerHardware = &(timerHardware[output]);
#ifdef USE_HAL_DRIVER
escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim);
#endif
}
escSerial->mode = mode;
escSerial->txTimerHardware = timerGetByTag(escSerialConfig()->ioTag, TIM_USE_ANY);
if (!escSerial->txTimerHardware) {
return NULL;
}
#ifdef USE_HAL_DRIVER
escSerial->txTimerHandle = timerFindTimerHandle(escSerial->txTimerHardware->tim);
#endif
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);
if (mode==PROTOCOL_SIMONK) {
escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex);
escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex);
}
else 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) {
if (pwmMotors[i].io != IO_NONE) {
for (volatile uint8_t j = 0; j < USABLE_TIMER_CHANNEL_COUNT; j++) {
if (pwmMotors[i].io == IOGetByTag(timerHardware[j].tag))
{
escSerialOutputPortConfig(&timerHardware[j]);
if (timerHardware[j].output & TIMER_OUTPUT_INVERTED) {
escOutputs[escSerial->outputCount].inverted = 1;
}
break;
}
}
escOutputs[escSerial->outputCount].io = pwmMotors[i].io;
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;
}
void escSerialInputPortDeConfig(const timerHardware_t *timerHardwarePtr)
{
timerChClearCCFlag(timerHardwarePtr);
timerChITConfig(timerHardwarePtr,DISABLE);
escSerialGPIOConfig(timerHardwarePtr, IOCFG_IPU);
}
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);
TIM_DeInit(escSerial->rxTimerHardware->tim);
}
timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL);
TIM_DeInit(escSerial->txTimerHardware->tim);
}
/*********************************************/
void processTxStateEsc(escSerial_t *escSerial)
static void processTxStateEsc(escSerial_t *escSerial)
{
uint8_t mask;
static uint8_t bitq=0, transmitStart=0;
@ -483,218 +523,7 @@ reload:
}
}
/*-----------------------BL*/
/*********************************************/
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);
}
}
}
enum {
TRAILING,
LEADING
};
void applyChangedBitsBL(escSerial_t *escSerial)
{
if (escSerial->rxEdge == TRAILING) {
uint8_t bitToSet;
for (bitToSet = escSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < escSerial->rxBitIndex; bitToSet++) {
escSerial->internalRxBuffer |= 1 << bitToSet;
}
}
}
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
);
}
}
#define STOP_BIT_MASK (1 << 0)
#define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1))
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);
} else {
escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
}
}
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);
}
}
void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
{
UNUSED(capture);
escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
processTxStateBL(escSerial);
processRxStateBL(escSerial);
}
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.
#ifdef USE_HAL_DRIVER
__HAL_TIM_SetCounter(escSerial->txTimerHandle, __HAL_TIM_GetAutoreload(escSerial->txTimerHandle) / 2);
#else
TIM_SetCounter(escSerial->txTimerHardware->tim, escSerial->txTimerHardware->tim->ARR / 2);
#endif
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);
}
}
/*-------------------------BL*/
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);
} else {
escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
}
}
void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
static void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
{
UNUSED(capture);
escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
@ -710,11 +539,35 @@ void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
}
}
processTxStateEsc(escSerial);
}
void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
{
uint32_t timerPeriod = 34;
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, timerPeriod, MHZ_TO_HZ(1));
timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
}
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);
} 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;
@ -778,7 +631,143 @@ void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture
}
uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance)
static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
{
// start bit is usually a FALLING signal
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1));
timerChConfigIC(timerHardwarePtr, ICPOLARITY_FALLING, 0);
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
}
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(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode)
{
escSerial_t *escSerial = &(escSerialPorts[portIndex]);
if (mode != PROTOCOL_KISSALL) {
escSerial->rxTimerHardware = &(timerHardware[output]);
#ifdef USE_HAL_DRIVER
escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim);
#endif
}
escSerial->mode = mode;
escSerial->txTimerHardware = timerGetByTag(escSerialConfig()->ioTag, TIM_USE_ANY);
#ifdef USE_HAL_DRIVER
escSerial->txTimerHandle = timerFindTimerHandle(escSerial->txTimerHardware->tim);
#endif
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);
if (mode==PROTOCOL_SIMONK) {
escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex);
escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex);
}
else 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) {
if (pwmMotors[i].io != IO_NONE) {
for (volatile uint8_t j = 0; j < USABLE_TIMER_CHANNEL_COUNT; j++) {
if (pwmMotors[i].io == IOGetByTag(timerHardware[j].tag))
{
escSerialOutputPortConfig(&timerHardware[j]);
if (timerHardware[j].output & TIMER_OUTPUT_INVERTED) {
escOutputs[escSerial->outputCount].inverted = 1;
}
break;
}
}
escOutputs[escSerial->outputCount].io = pwmMotors[i].io;
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);
TIM_DeInit(escSerial->rxTimerHardware->tim);
}
timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL);
TIM_DeInit(escSerial->txTimerHardware->tim);
}
static uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance)
{
if ((instance->mode & MODE_RX) == 0) {
return 0;
@ -789,7 +778,7 @@ uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance)
return (s->port.rxBufferHead - s->port.rxBufferTail) & (s->port.rxBufferSize - 1);
}
uint8_t escSerialReadByte(serialPort_t *instance)
static uint8_t escSerialReadByte(serialPort_t *instance)
{
uint8_t ch;
@ -806,7 +795,7 @@ uint8_t escSerialReadByte(serialPort_t *instance)
return ch;
}
void escSerialWriteByte(serialPort_t *s, uint8_t ch)
static void escSerialWriteByte(serialPort_t *s, uint8_t ch)
{
if ((s->mode & MODE_TX) == 0) {
return;
@ -816,24 +805,18 @@ void escSerialWriteByte(serialPort_t *s, uint8_t ch)
s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
}
void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
static void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
{
UNUSED(s);
UNUSED(baudRate);
}
void escSerialSetMode(serialPort_t *instance, portMode_t mode)
static void escSerialSetMode(serialPort_t *instance, portMode_t mode)
{
instance->mode = mode;
}
bool isEscSerialTransmitBufferEmpty(const serialPort_t *instance)
{
// start listening
return instance->txBufferHead == instance->txBufferTail;
}
uint32_t escSerialTxBytesFree(const serialPort_t *instance)
static uint32_t escSerialTxBytesFree(const serialPort_t *instance)
{
if ((instance->mode & MODE_TX) == 0) {
return 0;
@ -861,20 +844,6 @@ const struct serialPortVTable escSerialVTable[] = {
}
};
void escSerialInitialize()
{
//StopPwmAllMotors();
pwmDisableMotors();
for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
// set outputs to pullup
if (timerHardware[i].output & TIMER_OUTPUT_ENABLED)
{
escSerialGPIOConfig(&timerHardware[i], IOCFG_IPU); //GPIO_Mode_IPU
}
}
}
typedef enum {
IDLE,
HEADER_START,
@ -897,7 +866,7 @@ typedef struct mspPort_s {
static mspPort_t currentPort;
static bool ProcessExitCommand(uint8_t c)
static bool processExitCommand(uint8_t c)
{
if (currentPort.c_state == IDLE) {
if (c == '$') {
@ -945,7 +914,6 @@ static bool ProcessExitCommand(uint8_t c)
}
// mode 0=sk, 1=bl, 2=ki output=timerHardware PWM channel.
void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode)
{
bool exitEsc = false;
@ -975,18 +943,18 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin
}
else {
uint8_t first_output = 0;
for (volatile uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
if (timerHardware[i].output & TIMER_OUTPUT_ENABLED)
{
first_output=i;
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
if (timerHardware[i].output & TIMER_OUTPUT_ENABLED) {
first_output = i;
break;
}
}
//doesn't work with messy timertable
motor_output=first_output+output-1;
if (motor_output >=USABLE_TIMER_CHANNEL_COUNT)
motor_output = first_output + output;
if (motor_output >= USABLE_TIMER_CHANNEL_COUNT) {
return;
}
}
escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode);
@ -1014,7 +982,7 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin
while (serialRxBytesWaiting(escPassthroughPort))
{
ch = serialRead(escPassthroughPort);
exitEsc = ProcessExitCommand(ch);
exitEsc = processExitCommand(ch);
if (exitEsc)
{
serialWrite(escPassthroughPort, 0x24);
@ -1039,5 +1007,4 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin
}
}
#endif

1028
src/main/drivers/serial_escserial.c.orig Normal file
View File

File diff suppressed because it is too large Load Diff

12
src/main/drivers/serial_escserial.h
View File

@ -33,19 +33,7 @@ typedef enum {
PROTOCOL_COUNT
} escProtocol_e;
#define ALL_ESCS 255
serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode);
// serialPort API
void escSerialWriteByte(serialPort_t *instance, uint8_t ch);
uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance);
uint32_t escSerialTxBytesFree(const serialPort_t *instance);
uint8_t escSerialReadByte(serialPort_t *instance);
void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate);
bool isEscSerialTransmitBufferEmpty(const serialPort_t *s);
void escSerialInitialize();
void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode);
typedef struct escSerialConfig_s {

138
src/main/fc/cli.c
View File

@ -2097,31 +2097,41 @@ static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfi
cliDumpPrintLinef(dumpMask, equalsDefault, formatMap, buf);
}
static void cliMap(char *cmdline)
{
uint32_t len;
char out[9];
uint32_t i;
char buf[RX_MAPPABLE_CHANNEL_COUNT + 1];
len = strlen(cmdline);
uint32_t len = strlen(cmdline);
if (len == RX_MAPPABLE_CHANNEL_COUNT) {
if (len == 8) {
// uppercase it
for (uint32_t i = 0; i < 8; i++)
cmdline[i] = toupper((unsigned char)cmdline[i]);
for (uint32_t i = 0; i < 8; i++) {
if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i]))
for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
buf[i] = toupper((unsigned char)cmdline[i]);
}
buf[i] = '\0';
for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
buf[i] = toupper((unsigned char)cmdline[i]);
if (strchr(rcChannelLetters, buf[i]) && !strchr(buf + i + 1, buf[i]))
continue;
cliShowParseError();
return;
}
parseRcChannels(cmdline, rxConfigMutable());
parseRcChannels(buf, rxConfigMutable());
} else if (len > 0) {
cliShowParseError();
return;
}
cliPrint("Map: ");
uint32_t i;
for (i = 0; i < 8; i++)
out[rxConfig()->rcmap[i]] = rcChannelLetters[i];
out[i] = '\0';
cliPrintLine(out);
for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
buf[rxConfig()->rcmap[i]] = rcChannelLetters[i];
}
buf[i] = '\0';
cliPrintLinef("map %s", buf);
}
static char *checkCommand(char *cmdLine, const char *command)
@ -2186,27 +2196,20 @@ static void cliGpsPassthrough(char *cmdline)
}
#endif
#if defined(USE_ESCSERIAL) || defined(USE_DSHOT)
#ifndef ALL_ESCS
#define ALL_ESCS 255
#endif
static int parseEscNumber(char *pch, bool allowAllEscs) {
int escNumber = atoi(pch);
if ((escNumber >= 0) && (escNumber < getMotorCount())) {
tfp_printf("Programming on ESC %d.\r\n", escNumber);
} else if (allowAllEscs && escNumber == ALL_ESCS) {
tfp_printf("Programming on all ESCs.\r\n");
static int parseOutputIndex(char *pch, bool allowAllEscs) {
int outputIndex = atoi(pch);
if ((outputIndex >= 0) && (outputIndex < getMotorCount())) {
tfp_printf("Using output %d.\r\n", outputIndex);
} else if (allowAllEscs && outputIndex == ALL_MOTORS) {
tfp_printf("Using all outputs.\r\n");
} else {
tfp_printf("Invalid ESC number, range: 0 to %d.\r\n", getMotorCount() - 1);
tfp_printf("Invalid output number, range: 0 to %d.\r\n", getMotorCount() - 1);
return -1;
}
return escNumber;
return outputIndex;
}
#endif
#ifdef USE_DSHOT
static void cliDshotProg(char *cmdline)
@ -2220,12 +2223,12 @@ static void cliDshotProg(char *cmdline)
char *saveptr;
char *pch = strtok_r(cmdline, " ", &saveptr);
int pos = 0;
int escNumber = 0;
int escIndex = 0;
while (pch != NULL) {
switch (pos) {
case 0:
escNumber = parseEscNumber(pch, true);
if (escNumber == -1) {
escIndex = parseOutputIndex(pch, true);
if (escIndex == -1) {
return;
}
@ -2235,12 +2238,12 @@ static void cliDshotProg(char *cmdline)
int command = atoi(pch);
if (command >= 0 && command < DSHOT_MIN_THROTTLE) {
if (escNumber == ALL_ESCS) {
if (escIndex == ALL_MOTORS) {
for (unsigned i = 0; i < getMotorCount(); i++) {
pwmWriteDshotCommand(i, command);
}
} else {
pwmWriteDshotCommand(escNumber, command);
pwmWriteDshotCommand(escIndex, command);
}
if (command <= 5) {
@ -2276,7 +2279,7 @@ static void cliEscPassthrough(char *cmdline)
char *pch = strtok_r(cmdline, " ", &saveptr);
int pos = 0;
uint8_t mode = 0;
int escNumber = 0;
int escIndex = 0;
while (pch != NULL) {
switch (pos) {
case 0:
@ -2295,8 +2298,8 @@ static void cliEscPassthrough(char *cmdline)
}
break;
case 1:
escNumber = parseEscNumber(pch, mode == PROTOCOL_KISS);
if (escNumber == -1) {
escIndex = parseOutputIndex(pch, mode == PROTOCOL_KISS);
if (escIndex == -1) {
return;
}
@ -2313,7 +2316,7 @@ static void cliEscPassthrough(char *cmdline)
pch = strtok_r(NULL, " ", &saveptr);
}
escEnablePassthrough(cliPort, escNumber, mode);
escEnablePassthrough(cliPort, escIndex, mode);
}
#endif
@ -2355,46 +2358,57 @@ static void cliMixer(char *cmdline)
static void cliMotor(char *cmdline)
{
int motor_index = 0;
int motor_value = 0;
int index = 0;
char *pch = NULL;
char *saveptr;
if (isEmpty(cmdline)) {
cliShowParseError();
return;
}
pch = strtok_r(cmdline, " ", &saveptr);
int motorIndex;
int motorValue;
char *saveptr;
char *pch = strtok_r(cmdline, " ", &saveptr);
int index = 0;
while (pch != NULL) {
switch (index) {
case 0:
motor_index = atoi(pch);
break;
case 1:
motor_value = atoi(pch);
break;
case 0:
motorIndex = parseOutputIndex(pch, true);
if (motorIndex == -1) {
return;
}
break;
case 1:
motorValue = atoi(pch);
break;
}
index++;
pch = strtok_r(NULL, " ", &saveptr);
}
if (motor_index < 0 || motor_index >= MAX_SUPPORTED_MOTORS) {
cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS - 1);
return;
}
if (index == 2) {
if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) {
if (motorValue < PWM_RANGE_MIN || motorValue > PWM_RANGE_MAX) {
cliShowArgumentRangeError("value", 1000, 2000);
} else {
motor_disarmed[motor_index] = convertExternalToMotor(motor_value);
uint32_t motorOutputValue = convertExternalToMotor(motorValue);
cliPrintLinef("motor %d: %d", motor_index, convertMotorToExternal(motor_disarmed[motor_index]));
if (motorIndex != ALL_MOTORS) {
motor_disarmed[motorIndex] = motorOutputValue;
cliPrintLinef("motor %d: %d", motorIndex, motorOutputValue);
} else {
for (int i = 0; i < getMotorCount(); i++) {
motor_disarmed[i] = motorOutputValue;
}
cliPrintLinef("all motors: %d", motorOutputValue);
}
}
} else {
cliShowParseError();
}
}
#ifndef MINIMAL_CLI

5
src/main/fc/fc_core.c
View File

@ -220,9 +220,8 @@ void tryArm(void)
return;
}
#ifdef USE_DSHOT
if (!feature(FEATURE_3D)) {
//TODO: Use BOXDSHOTREVERSE here
if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
if (isMotorProtocolDshot()) {
if (!IS_RC_MODE_ACTIVE(BOXDSHOTREVERSE)) {
reverseMotors = false;
for (unsigned index = 0; index < getMotorCount(); index++) {
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL);

18
src/main/fc/fc_msp.c
View File

@ -150,12 +150,13 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
{ BOXBLACKBOX, "BLACKBOX", 26 },
{ BOXFAILSAFE, "FAILSAFE", 27 },
{ BOXAIRMODE, "AIR MODE", 28 },
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
{ BOX3DDISABLE, "DISABLE 3D", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
{ BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 },
{ BOXCAMERA1, "CAMERA CONTROL 1", 32},
{ BOXCAMERA2, "CAMERA CONTROL 2", 33},
{ BOXCAMERA3, "CAMERA CONTROL 3", 34 },
{ BOXDSHOTREVERSE, "DSHOT REVERSE MOTORS", 35 },
};
// mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@ -193,7 +194,7 @@ typedef enum {
#define ESC_4WAY 0xff
uint8_t escMode;
uint8_t escPortIndex = 0;
uint8_t escPortIndex;
#ifdef USE_ESCSERIAL
static void mspEscPassthroughFn(serialPort_t *serialPort)
@ -232,7 +233,7 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr
case PROTOCOL_KISS:
case PROTOCOL_KISSALL:
case PROTOCOL_CASTLE:
if (escPortIndex < getMotorCount() || (escMode == PROTOCOL_KISS && escPortIndex == ALL_ESCS)) {
if (escPortIndex < getMotorCount() || (escMode == PROTOCOL_KISS && escPortIndex == ALL_MOTORS)) {
sbufWriteU8(dst, 1);
if (mspPostProcessFn) {
@ -396,8 +397,13 @@ void initActiveBoxIds(void)
BME(BOXFPVANGLEMIX);
//TODO: Split this into BOX3DDISABLESWITCH and BOXDSHOTREVERSE
BME(BOX3DDISABLESWITCH);
if (feature(FEATURE_3D)) {
BME(BOX3DDISABLE);
}
if (isMotorProtocolDshot()) {
BME(BOXDSHOTREVERSE);
}
if (feature(FEATURE_SERVO_TILT)) {
BME(BOXCAMSTAB);
@ -469,7 +475,7 @@ static int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags)
const uint64_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON)
| BM(BOXLEDMAX) | BM(BOXLEDLOW) | BM(BOXLLIGHTS) | BM(BOXCALIB) | BM(BOXGOV) | BM(BOXOSD)
| BM(BOXTELEMETRY) | BM(BOXGTUNE) | BM(BOXBLACKBOX) | BM(BOXBLACKBOXERASE) | BM(BOXAIRMODE)
| BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX);
| BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX) | BM(BOXDSHOTREVERSE) | BM(BOX3DDISABLE);
STATIC_ASSERT(sizeof(rcModeCopyMask) * 8 >= CHECKBOX_ITEM_COUNT, copy_mask_too_small_for_boxes);
for (unsigned i = 0; i < CHECKBOX_ITEM_COUNT; i++) {
if ((rcModeCopyMask & BM(i)) // mode copy is enabled

2
src/main/fc/fc_rc.c
View File

@ -306,7 +306,7 @@ void updateRcCommands(void)
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLE) && !failsafeIsActive()) {
fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
}

2
src/main/fc/rc_controls.c
View File

@ -103,7 +103,7 @@ bool areSticksInApModePosition(uint16_t ap_mode)
throttleStatus_e calculateThrottleStatus(void)
{
if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLE)) {
if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)))
return THROTTLE_LOW;
} else {

3
src/main/fc/rc_modes.h
View File

@ -51,12 +51,13 @@ typedef enum {
BOXBLACKBOX,
BOXFAILSAFE,
BOXAIRMODE,
BOX3DDISABLESWITCH,
BOX3DDISABLE,
BOXFPVANGLEMIX,
BOXBLACKBOXERASE,
BOXCAMERA1,
BOXCAMERA2,
BOXCAMERA3,
BOXDSHOTREVERSE,
CHECKBOX_ITEM_COUNT
} boxId_e;

2
src/main/flight/mixer.h
View File

@ -99,6 +99,8 @@ PG_DECLARE(motorConfig_t, motorConfig);
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
#define ALL_MOTORS 255
extern const mixer_t mixers[];
extern float motor[MAX_SUPPORTED_MOTORS];
extern float motor_disarmed[MAX_SUPPORTED_MOTORS];