Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

First cut of configurable serial port functionality.

Browse source 
Currently port usage is hard-coded to the default port layout, cli
commands are coming in a future commit.

This decouples all code from the global 'serialPorts' structure which
has been removed.  Any code that needs to use a serial port can use
findOpenSerialPort() and openSerialPort() and maintain it's own
reference to the port.

Ports can switch between functions.  e.g. by default
cli/msp/telemetry/gps
passthrough all use USART1.  Each port maintains it's current function.
see begin/endSerialPortFunction.

There are only certain combinations of serial port functions that are
supported, these are listed in serialPortFunctionScenario_e.

This commit also adds a few 'static' keywords to variables that should
have been.

There a a few other minor fixes and tweaks to various bits of code that
this uncovered too.
This commit is contained in:
Dominic Clifton 2014-05-08 21:46:09 +01:00
parent 533a1f9e48
commit 1777d8feda
33 changed files with 787 additions and 394 deletions
Download patch file Download diff file Expand all files Collapse all files

40
src/build_config.c
View file

@ -1,21 +1,14 @@
#include "stdbool.h"
#include "stdint.h"
#include "stdlib.h"
#include "stdarg.h"
#include "platform.h"
#include "build_config.h"
#include "common/printf.h"
#include "drivers/gpio_common.h"
#include "drivers/timer_common.h"
#include "drivers/pwm_mapping.h"
#include "flight_mixer.h"
#include "drivers/serial_common.h"
#include "serial_common.h"
#include "build_config.h"
#if MAX_PWM_MOTORS != MAX_SUPPORTED_MOTORS
#error Motor configuration mismatch
@ -24,34 +17,3 @@
#if MAX_PWM_SERVOS != MAX_SUPPORTED_SERVOS
#error Servo configuration mismatch
#endif
#ifdef REQUIRE_CC_ARM_PRINTF_SUPPORT
// gcc/GNU version
static void _putc(void *p, char c)
{
serialWrite(serialPorts.mainport, c);
}
void initPrintfSupport(void)
{
init_printf(NULL, _putc);
}
#else
// keil/armcc version
int fputc(int c, FILE *f)
{
// let DMA catch up a bit when using set or dump, we're too fast.
while (!isSerialTransmitBufferEmpty(serialPorts.mainport));
serialWrite(serialPorts.mainport, c);
return c;
}
void initPrintfSupport(void)
{
// nothing to do
}
#endif

2
src/build_config.h
View file

@ -6,5 +6,3 @@
#define REQUIRE_CC_ARM_PRINTF_SUPPORT
#define REQUIRE_PRINTF_LONG_SUPPORT
#endif
void initPrintfSupport(void);

2
src/buzzer.c
View file

@ -27,7 +27,7 @@ typedef enum {
FAILSAFE_FIND_ME
} failsafeBuzzerWarnings_e;
failsafe_t* failsafe;
static failsafe_t* failsafe;
void buzzerInit(failsafe_t *initialFailsafe)
{

37
src/common/printf.c
View file

@ -32,18 +32,20 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdarg.h>
#include <stdlib.h>
#include "build_config.h"
#include "drivers/serial_common.h"
#include "serial_common.h"
#include "build_config.h"
#include "printf.h"
#ifdef REQUIRE_PRINTF_LONG_SUPPORT
#include "typeconversion.h"
#endif
serialPort_t *printfSerialPort;
#ifdef REQUIRE_CC_ARM_PRINTF_SUPPORT
@ -154,7 +156,7 @@ void tfp_printf(char *fmt, ...)
va_start(va, fmt);
tfp_format(stdout_putp, stdout_putf, fmt, va);
va_end(va);
while (!isSerialTransmitBufferEmpty(serialPorts.mainport));
while (!isSerialTransmitBufferEmpty(printfSerialPort));
}
static void putcp(void *p, char c)
@ -171,4 +173,35 @@ void tfp_sprintf(char *s, char *fmt, ...)
va_end(va);
}
static void _putc(void *p, char c)
{
serialWrite(printfSerialPort, c);
}
void initPrintfSupport(void)
{
init_printf(NULL, _putc);
}
#else
// keil/armcc version
int fputc(int c, FILE *f)
{
// let DMA catch up a bit when using set or dump, we're too fast.
while (!isSerialTransmitBufferEmpty(serialPorts.mainport));
serialWrite(printfSerialPort, c);
return c;
}
void initPrintfSupport(serialPort_t *serialPort)
{
// Nothing to do
}
#endif
void setPrintfSerialPort(serialPort_t *serialPort)
{
printfSerialPort = serialPort;
}

2
src/common/printf.h
View file

@ -115,4 +115,6 @@ void tfp_format(void *putp, void (*putf) (void *, char), char *fmt, va_list va);
#define printf tfp_printf
#define sprintf tfp_sprintf
void setPrintfSerialPort(serialPort_t *serialPort);
#endif

28
src/config.c
View file

@ -16,10 +16,10 @@
#include "statusindicator.h"
#include "sensors_acceleration.h"
#include "sensors_barometer.h"
#include "telemetry_common.h"
#include "gps_common.h"
#include "drivers/serial_common.h"
#include "serial_common.h"
#include "telemetry_common.h"
#include "flight_mixer.h"
#include "boardalignment.h"
#include "battery.h"
@ -29,7 +29,6 @@
#include "rc_curves.h"
#include "rx_common.h"
#include "gps_common.h"
#include "serial_common.h"
#include "failsafe.h"
#include "runtime_config.h"
@ -51,7 +50,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
master_t masterConfig; // master config struct with data independent from profiles
profile_t currentProfile; // profile config struct
static const uint8_t EEPROM_CONF_VERSION = 64;
static const uint8_t EEPROM_CONF_VERSION = 65;
static void resetConf(void);
static uint8_t calculateChecksum(const uint8_t *data, uint32_t length)
@ -279,10 +278,10 @@ void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
{
telemetryConfig->telemetry_provider = TELEMETRY_PROVIDER_FRSKY;
telemetryConfig->telemetry_port = TELEMETRY_PORT_UART;
telemetryConfig->frsky_inversion = SERIAL_NOT_INVERTED;
telemetryConfig->telemetry_switch = 0;
}
// Default settings
static void resetConf(void)
{
@ -336,14 +335,12 @@ static void resetConf(void)
masterConfig.motor_pwm_rate = 400;
masterConfig.servo_pwm_rate = 50;
// gps/nav stuff
masterConfig.gps_type = GPS_NMEA;
masterConfig.gps_baudrate = GPS_BAUD_115200;
masterConfig.gps_provider = GPS_NMEA;
masterConfig.gps_initial_baudrate_index = GPS_BAUDRATE_115200;
// serial (USART1) baudrate
masterConfig.serialConfig.port1_baudrate = 115200;
masterConfig.serialConfig.softserial_baudrate = 9600;
masterConfig.serialConfig.softserial_1_inverted = 0;
masterConfig.serialConfig.softserial_2_inverted = 0;
masterConfig.serialConfig.msp_baudrate = 115200;
masterConfig.serialConfig.cli_baudrate = 115200;
masterConfig.serialConfig.gps_passthrough_baudrate = 115200;
masterConfig.serialConfig.reboot_character = 'R';
masterConfig.looptime = 3500;
@ -451,6 +448,9 @@ uint32_t featureMask(void)
bool canSoftwareSerialBeUsed(void)
{
#ifdef FY90Q
return false;
#endif
// FIXME this is not ideal because it means you can't disable parallel PWM input even when using spektrum/sbus etc.
// really we want to say 'return !feature(FEATURE_PARALLEL_PWM);'
return feature(FEATURE_SOFTSERIAL) && feature(FEATURE_PPM); // Software serial can only be used in PPM mode because parallel PWM uses the same hardware pins/timers

5
src/config_master.h
View file

@ -51,12 +51,13 @@ typedef struct master_t {
uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable.
// gps-related stuff
uint8_t gps_type; // See GPSHardware enum.
int8_t gps_baudrate; // See GPSBaudRates enum.
gpsProvider_e gps_provider;
gpsBaudRate_e gps_initial_baudrate_index;
serialConfig_t serialConfig;
telemetryConfig_t telemetryConfig;
profile_t profile[3]; // 3 separate profiles
uint8_t current_profile_index; // currently loaded profile

9
src/drivers/serial_common.h
View file

@ -1,5 +1,11 @@
#pragma once
typedef enum {
SERIAL_NOT_INVERTED = 0,
SERIAL_INVERTED
} serialInversion_e;
typedef enum portMode_t {
MODE_RX = 1 << 0,
MODE_TX = 1 << 1,
@ -12,8 +18,9 @@ typedef void (* serialReceiveCallbackPtr)(uint16_t data); // used by serial dr
typedef struct serialPort {
const struct serialPortVTable *vTable;
uint8_t identifier;
portMode_t mode;
serialInversion_e inversion;
uint32_t baudRate;
uint32_t rxBufferSize;

52
src/drivers/serial_softserial.c
View file

@ -37,7 +37,7 @@ void onSerialRxPinChange(uint8_t portIndex, captureCompare_t capture);
void setTxSignal(softSerial_t *softSerial, uint8_t state)
{
if (softSerial->isInverted) {
if (softSerial->port.inversion == SERIAL_INVERTED) {
state = !state;
}
@ -127,10 +127,10 @@ void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
TIM_ICInit(tim, &TIM_ICInitStructure);
}
void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference, uint8_t inverted)
void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference, serialInversion_e inversion)
{
// start bit is usually a FALLING signal
serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, inverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, inversion == SERIAL_INVERTED ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, onSerialRxPinChange);
}
@ -152,10 +152,11 @@ void resetBuffers(softSerial_t *softSerial)
softSerial->port.txBufferHead = 0;
}
void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t baud, uint8_t inverted)
void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t baud, serialInversion_e inversion)
{
softSerial->port.vTable = softSerialVTable;
softSerial->port.mode = MODE_RXTX;
softSerial->port.inversion = inversion;
resetBuffers(softSerial);
@ -163,11 +164,12 @@ void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t
softSerial->isSearchingForStartBit = true;
softSerial->rxBitIndex = 0;
softSerial->isInverted = inverted;
softSerial->transmissionErrors = 0;
softSerial->receiveErrors = 0;
softSerial->softSerialPortIndex = portIndex;
serialOutputPortConfig(softSerial->txTimerHardware);
serialInputPortConfig(softSerial->rxTimerHardware);
@ -175,20 +177,10 @@ void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t
delay(50);
serialTimerTxConfig(softSerial->txTimerHardware, portIndex, baud);
serialTimerRxConfig(softSerial->rxTimerHardware, portIndex, inverted);
serialTimerRxConfig(softSerial->rxTimerHardware, portIndex, inversion);
}
typedef struct softSerialConfiguration_s {
uint32_t sharedBaudRate;
bool primaryPortInitialised;
} softSerialConfiguration_t;
softSerialConfiguration_t softSerialConfiguration = {
0,
false
};
void setupSoftSerialPrimary(uint32_t baud, uint8_t inverted)
serialPort_t *openSoftSerial1(uint32_t baud, serialInversion_e inversion)
{
uint8_t portIndex = 0;
softSerial_t *softSerial = &(softSerialPorts[portIndex]);
@ -196,13 +188,12 @@ void setupSoftSerialPrimary(uint32_t baud, uint8_t inverted)
softSerial->rxTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_RX_HARDWARE]);
softSerial->txTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_TX_HARDWARE]);
initialiseSoftSerial(softSerial, portIndex, baud, inverted);
initialiseSoftSerial(softSerial, portIndex, baud, inversion);
softSerialConfiguration.sharedBaudRate = baud;
softSerialConfiguration.primaryPortInitialised = true;
return &softSerial->port;
}
void setupSoftSerialSecondary(uint8_t inverted)
serialPort_t * openSoftSerial2(uint32_t baud, serialInversion_e inversion)
{
int portIndex = 1;
softSerial_t *softSerial = &(softSerialPorts[portIndex]);
@ -210,7 +201,9 @@ void setupSoftSerialSecondary(uint8_t inverted)
softSerial->rxTimerHardware = &(timerHardware[SOFT_SERIAL_2_TIMER_RX_HARDWARE]);
softSerial->txTimerHardware = &(timerHardware[SOFT_SERIAL_2_TIMER_TX_HARDWARE]);
initialiseSoftSerial(softSerial, portIndex, softSerialConfiguration.sharedBaudRate, inverted);
initialiseSoftSerial(softSerial, portIndex, baud, inversion);
return &softSerial->port;
}
@ -285,7 +278,11 @@ void prepareForNextRxByte(softSerial_t *softSerial)
softSerial->isSearchingForStartBit = true;
if (softSerial->rxEdge == LEADING) {
softSerial->rxEdge = TRAILING;
serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, softSerial->isInverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
serialICConfig(
softSerial->rxTimerHardware->tim,
softSerial->rxTimerHardware->channel,
softSerial->port.inversion == SERIAL_INVERTED ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling
);
}
}
@ -360,7 +357,7 @@ void onSerialRxPinChange(uint8_t portIndex, captureCompare_t capture)
softSerial->transmissionErrors++;
}
serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, softSerial->isInverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising);
serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, softSerial->port.inversion == SERIAL_INVERTED ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising);
softSerial->rxEdge = LEADING;
softSerial->rxBitIndex = 0;
@ -378,10 +375,10 @@ void onSerialRxPinChange(uint8_t portIndex, captureCompare_t capture)
if (softSerial->rxEdge == TRAILING) {
softSerial->rxEdge = LEADING;
serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, softSerial->isInverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising);
serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, softSerial->port.inversion == SERIAL_INVERTED ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising);
} else {
softSerial->rxEdge = TRAILING;
serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, softSerial->isInverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, softSerial->port.inversion == SERIAL_INVERTED ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
}
}
@ -444,7 +441,8 @@ void softSerialWriteByte(serialPort_t *s, uint8_t ch)
void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
{
// not implemented.
softSerial_t *softSerial = (softSerial_t *)s;
initialiseSoftSerial(softSerial, softSerial->softSerialPortIndex, baudRate, softSerial->port.inversion);
}
void softSerialSetMode(serialPort_t *instance, portMode_t mode)

8
src/drivers/serial_softserial.h
View file

@ -29,10 +29,10 @@ typedef struct softSerial_s {
uint16_t internalTxBuffer; // includes start and stop bits
uint16_t internalRxBuffer; // includes start and stop bits
uint8_t isInverted;
uint16_t transmissionErrors;
uint16_t receiveErrors;
uint8_t softSerialPortIndex;
} softSerial_t;
extern timerHardware_t* serialTimerHardware;
@ -40,8 +40,8 @@ extern softSerial_t softSerialPorts[];
extern const struct serialPortVTable softSerialVTable[];
void setupSoftSerialPrimary(uint32_t baud, uint8_t inverted);
void setupSoftSerialSecondary(uint8_t inverted);
serialPort_t *openSoftSerial1(uint32_t baud, uint8_t inverted);
serialPort_t *openSoftSerial2(uint32_t baud, uint8_t inverted);
// serialPort API
void softSerialWriteByte(serialPort_t *instance, uint8_t ch);

84
src/drivers/serial_uart_common.c
View file

@ -10,10 +10,32 @@
uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode);
uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode);
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode)
static void uartReconfigure(uartPort_t *uartPort)
{
USART_InitTypeDef USART_InitStructure;
USART_InitStructure.USART_BaudRate = uartPort->port.baudRate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
if (uartPort->port.mode & MODE_SBUS) {
USART_InitStructure.USART_StopBits = USART_StopBits_2;
USART_InitStructure.USART_Parity = USART_Parity_Even;
} else {
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
}
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = 0;
if (uartPort->port.mode & MODE_RX)
USART_InitStructure.USART_Mode |= USART_Mode_Rx;
if (uartPort->port.mode & MODE_TX)
USART_InitStructure.USART_Mode |= USART_Mode_Tx;
USART_Init(uartPort->USARTx, &USART_InitStructure);
}
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion)
{
DMA_InitTypeDef DMA_InitStructure;
USART_InitTypeDef USART_InitStructure;
uartPort_t *s = NULL;
@ -32,22 +54,14 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
s->port.mode = mode;
s->port.baudRate = baudRate;
USART_InitStructure.USART_BaudRate = baudRate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
if (mode & MODE_SBUS) {
USART_InitStructure.USART_StopBits = USART_StopBits_2;
USART_InitStructure.USART_Parity = USART_Parity_Even;
} else {
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
}
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = 0;
if (mode & MODE_RX)
USART_InitStructure.USART_Mode |= USART_Mode_Rx;
if (mode & MODE_TX)
USART_InitStructure.USART_Mode |= USART_Mode_Tx;
USART_Init(s->USARTx, &USART_InitStructure);
#if 1 // FIXME use inversion on STM32F3
s->port.inversion = SERIAL_NOT_INVERTED;
#else
s->port.inversion = inversion;
#endif
uartReconfigure(s);
DMA_StructInit(&DMA_InitStructure);
DMA_InitStructure.DMA_PeripheralBaseAddr = s->rxDMAPeripheralBaseAddr;
@ -109,30 +123,26 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
void uartSetBaudRate(serialPort_t *instance, uint32_t baudRate)
{
USART_InitTypeDef USART_InitStructure;
uartPort_t *s = (uartPort_t *)instance;
USART_InitStructure.USART_BaudRate = baudRate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = 0;
if (s->port.mode & MODE_RX)
USART_InitStructure.USART_Mode |= USART_Mode_Rx;
if (s->port.mode & MODE_TX)
USART_InitStructure.USART_Mode |= USART_Mode_Tx;
USART_Init(s->USARTx, &USART_InitStructure);
s->port.baudRate = baudRate;
uartPort_t *uartPort = (uartPort_t *)instance;
uartPort->port.baudRate = baudRate;
#ifndef STM32F303xC // FIXME this doesnt seem to work, for now re-open the port from scratch, perhaps clearing some uart flags may help?
uartReconfigure(uartPort);
#else
uartOpen(uartPort->USARTx, uartPort->port.callback, uartPort->port.baudRate, uartPort->port.mode, uartPort->port.inversion);
#endif
}
void uartSetMode(serialPort_t *s, portMode_t mode)
void uartSetMode(serialPort_t *instance, portMode_t mode)
{
// not implemented.
uartPort_t *uartPort = (uartPort_t *)instance;
uartPort->port.mode = mode;
#ifndef STM32F303xC // FIXME this doesnt seem to work, for now re-open the port from scratch, perhaps clearing some uart flags may help?
uartReconfigure(uartPort);
#else
uartOpen(uartPort->USARTx, uartPort->port.callback, uartPort->port.baudRate, uartPort->port.mode, uartPort->port.inversion);
#endif
}
void uartStartTxDMA(uartPort_t *s)
{
s->txDMAChannel->CMAR = (uint32_t)&s->port.txBuffer[s->port.txBufferTail];

2
src/drivers/serial_uart_common.h
View file

@ -27,7 +27,7 @@ typedef struct {
extern const struct serialPortVTable uartVTable[];
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode);
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion);
// serialPort API
void uartWrite(serialPort_t *instance, uint8_t ch);

6
src/drivers/serial_uart_stm32f30x.c
View file

@ -184,6 +184,7 @@ void DMA1_Channel4_IRQHandler(void)
{
uartPort_t *s = &uartPort1;
DMA_ClearITPendingBit(DMA1_IT_TC4);
DMA_Cmd(DMA1_Channel4, DISABLE);
handleUsartTxDma(s);
}
@ -209,6 +210,11 @@ void usartIrqHandler(uartPort_t *s)
s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize;
}
}
if (s->txDMAChannel) {
return;
}
if (ISR & USART_FLAG_TXE) {
if (s->port.txBufferTail != s->port.txBufferHead) {
s->USARTx->TDR = s->port.txBuffer[s->port.txBufferTail];

2
src/flight_imu.c
View file

@ -31,9 +31,9 @@
#include "escservo.h"
#include "rc_controls.h"
#include "rx_common.h"
#include "telemetry_common.h"
#include "drivers/serial_common.h"
#include "serial_common.h"
#include "telemetry_common.h"
#include "failsafe.h"
#include "runtime_config.h"
#include "config.h"

14
src/flight_mixer.c
View file

@ -30,13 +30,13 @@ int16_t servo[MAX_SUPPORTED_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500
static int useServo;
servoParam_t *servoConf;
mixerConfig_t *mixerConfig;
flight3DConfig_t *flight3DConfig;
escAndServoConfig_t *escAndServoConfig;
airplaneConfig_t *airplaneConfig;
rxConfig_t *rxConfig;
gimbalConfig_t *gimbalConfig;
static servoParam_t *servoConf;
static mixerConfig_t *mixerConfig;
static flight3DConfig_t *flight3DConfig;
static escAndServoConfig_t *escAndServoConfig;
static airplaneConfig_t *airplaneConfig;
static rxConfig_t *rxConfig;
static gimbalConfig_t *gimbalConfig;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
static MultiType currentMixerConfiguration;

96
src/gps_common.c
View file

@ -22,10 +22,13 @@
#include "sensors_common.h"
#include "config.h"
#include "runtime_config.h"
#include "gps_common.h"
extern int16_t debug[4];
// **********************
// GPS
@ -55,10 +58,13 @@ static uint8_t gpsProvider;
// GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
#define GPS_TIMEOUT (2500)
// How many entries in gpsInitData array below
#define GPS_INIT_ENTRIES (GPS_BAUD_MAX + 1)
#define GPS_BAUD_DELAY (100)
#define GPS_INIT_ENTRIES (GPS_BAUDRATE_MAX + 1)
#define GPS_BAUDRATE_CHATE_DELAY (100)
gpsProfile_t *gpsProfile;
static gpsProfile_t *gpsProfile;
static serialConfig_t *serialConfig;
static serialPort_t *gpsPort;
typedef struct gpsInitData_t {
uint8_t index;
@ -69,12 +75,12 @@ typedef struct gpsInitData_t {
// NMEA will cycle through these until valid data is received
static const gpsInitData_t gpsInitData[] = {
{ GPS_BAUD_115200, 115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
{ GPS_BAUD_57600, 57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
{ GPS_BAUD_38400, 38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
{ GPS_BAUD_19200, 19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
{ GPS_BAUDRATE_115200, 115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
{ GPS_BAUDRATE_57600, 57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
{ GPS_BAUDRATE_38400, 38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
{ GPS_BAUDRATE_19200, 19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
// 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
{ GPS_BAUD_9600, 9600, "", "" }
{ GPS_BAUDRATE_9600, 9600, "", "" }
};
static const uint8_t ubloxInit[] = {
@ -133,26 +139,34 @@ void gpsUseProfile(gpsProfile_t *gpsProfileToUse)
}
// When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit()
void gpsInit(uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile)
void gpsInit(serialConfig_t *initialSerialConfig, uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile)
{
serialConfig = initialSerialConfig;
gpsProvider = initialGpsProvider;
gpsUseProfile(initialGpsProfile);
portMode_t mode = MODE_RXTX;
// init gpsData structure. if we're not actually enabled, don't bother doing anything else
gpsSetState(GPS_UNKNOWN);
gpsData.baudrateIndex = baudrateIndex;
gpsData.lastMessage = millis();
gpsData.errors = 0;
portMode_t mode = MODE_RXTX;
// only RX is needed for NMEA-style GPS
if (gpsProvider == GPS_NMEA)
mode = MODE_RX;
gpsUsePIDs(pidProfile);
// Open GPS UART, no callback - buffer will be read out in gpsThread()
serialPorts.gpsport = uartOpen(USART2, NULL, gpsInitData[baudrateIndex].baudrate, mode);
// no callback - buffer will be consumed in gpsThread()
gpsPort = openSerialPort(FUNCTION_GPS, NULL, gpsInitData[baudrateIndex].baudrate, mode, SERIAL_NOT_INVERTED);
if (!gpsPort) {
featureClear(FEATURE_GPS);
return;
}
// signal GPS "thread" to initialize when it gets to it
gpsSetState(GPS_INITIALIZING);
}
@ -162,7 +176,7 @@ void gpsInitHardware(void)
switch (gpsProvider) {
case GPS_NMEA:
// nothing to do, just set baud rate and try receiving some stuff and see if it parses
serialSetBaudRate(serialPorts.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
serialSetBaudRate(gpsPort, gpsInitData[gpsData.baudrateIndex].baudrate);
gpsSetState(GPS_RECEIVINGDATA);
break;
@ -170,19 +184,19 @@ void gpsInitHardware(void)
// UBX will run at mcfg.gps_baudrate, it shouldn't be "autodetected". So here we force it to that rate
// Wait until GPS transmit buffer is empty
if (!isSerialTransmitBufferEmpty(serialPorts.gpsport))
if (!isSerialTransmitBufferEmpty(gpsPort))
break;
if (gpsData.state == GPS_INITIALIZING) {
uint32_t m = millis();
if (m - gpsData.state_ts < GPS_BAUD_DELAY)
if (m - gpsData.state_ts < GPS_BAUDRATE_CHATE_DELAY)
return;
if (gpsData.state_position < GPS_INIT_ENTRIES) {
// try different speed to INIT
serialSetBaudRate(serialPorts.gpsport, gpsInitData[gpsData.state_position].baudrate);
serialSetBaudRate(gpsPort, gpsInitData[gpsData.state_position].baudrate);
// but print our FIXED init string for the baudrate we want to be at
serialPrint(serialPorts.gpsport, gpsInitData[gpsData.baudrateIndex].ubx);
serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx);
gpsData.state_position++;
gpsData.state_ts = m;
@ -193,10 +207,10 @@ void gpsInitHardware(void)
} else {
// GPS_INITDONE, set our real baud rate and push some ublox config strings
if (gpsData.state_position == 0)
serialSetBaudRate(serialPorts.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
serialSetBaudRate(gpsPort, gpsInitData[gpsData.baudrateIndex].baudrate);
if (gpsData.state_position < sizeof(ubloxInit)) {
serialWrite(serialPorts.gpsport, ubloxInit[gpsData.state_position]); // send ubx init binary
serialWrite(gpsPort, ubloxInit[gpsData.state_position]); // send ubx init binary
gpsData.state_position++;
} else {
@ -205,10 +219,6 @@ void gpsInitHardware(void)
}
}
break;
case GPS_MTK_NMEA:
case GPS_MTK_BINARY:
// TODO. need to find my old piece of shit MTK GPS.
break;
}
// clear error counter
@ -218,9 +228,9 @@ void gpsInitHardware(void)
void gpsThread(void)
{
// read out available GPS bytes
if (serialPorts.gpsport) {
while (serialTotalBytesWaiting(serialPorts.gpsport))
gpsNewData(serialRead(serialPorts.gpsport));
if (gpsPort) {
while (serialTotalBytesWaiting(gpsPort))
gpsNewData(serialRead(gpsPort));
}
switch (gpsData.state) {
@ -263,8 +273,6 @@ static bool gpsNewFrame(uint8_t c)
return gpsNewFrameNMEA(c);
case GPS_UBLOX: // UBX binary
return gpsNewFrameUBLOX(c);
case GPS_MTK_BINARY: // MTK in BINARY mode (TODO)
return false;
}
return false;
@ -463,10 +471,15 @@ static void gpsNewData(uint16_t c)
gpsData.lastLastMessage = gpsData.lastMessage;
gpsData.lastMessage = millis();
sensorsSet(SENSOR_GPS);
if (GPS_update == 1)
GPS_update = 0;
else
GPS_update = 1;
#if 1
debug[3] = GPS_update;
#endif
if (f.GPS_FIX && GPS_numSat >= 5) {
if (!f.ARMED)
f.GPS_FIX_HOME = 0;
@ -597,26 +610,39 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
}
int8_t gpsSetPassthrough(void)
gpsEnablePassthroughResult_e gpsEnablePassthrough(void)
{
if (gpsData.state != GPS_RECEIVINGDATA)
return -1;
return GPS_PASSTHROUGH_NO_GPS;
serialPort_t *gpsPassthroughPort = findOpenSerialPort(FUNCTION_GPS_PASSTHROUGH);
if (gpsPassthroughPort) {
waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
serialSetBaudRate(gpsPassthroughPort, serialConfig->gps_passthrough_baudrate);
} else {
gpsPassthroughPort = openSerialPort(FUNCTION_GPS_PASSTHROUGH, NULL, serialConfig->gps_passthrough_baudrate, MODE_RXTX, SERIAL_NOT_INVERTED);
if (!gpsPassthroughPort) {
return GPS_PASSTHROUGH_NO_SERIAL_PORT;
}
}
LED0_OFF;
LED1_OFF;
while(1) {
if (serialTotalBytesWaiting(serialPorts.gpsport)) {
if (serialTotalBytesWaiting(gpsPort)) {
LED0_ON;
serialWrite(serialPorts.mainport, serialRead(serialPorts.gpsport));
serialWrite(gpsPassthroughPort, serialRead(gpsPort));
LED0_OFF;
}
if (serialTotalBytesWaiting(serialPorts.mainport)) {
if (serialTotalBytesWaiting(gpsPassthroughPort)) {
LED1_ON;
serialWrite(serialPorts.gpsport, serialRead(serialPorts.mainport));
serialWrite(gpsPort, serialRead(gpsPassthroughPort));
LED1_OFF;
}
}
return GPS_PASSTHROUGH_ENABLED;
}
// OK here is the onboard GPS code

33
src/gps_common.h
View file

@ -7,28 +7,25 @@ typedef enum {
GPS_NMEA = 0,
GPS_UBLOX,
GPS_MTK_NMEA,
GPS_MTK_BINARY,
GPS_MAG_BINARY,
GPS_HARDWARE_MAX = GPS_MAG_BINARY,
} GPSHardware;
GPS_PROVIDER_MAX = GPS_MTK_NMEA,
} gpsProvider_e;
typedef enum {
GPS_BAUD_115200 = 0,
GPS_BAUD_57600,
GPS_BAUD_38400,
GPS_BAUD_19200,
GPS_BAUD_9600,
GPS_BAUD_MAX = GPS_BAUD_9600
} GPSBaudRates;
GPS_BAUDRATE_115200 = 0,
GPS_BAUDRATE_57600,
GPS_BAUDRATE_38400,
GPS_BAUDRATE_19200,
GPS_BAUDRATE_9600,
GPS_BAUDRATE_MAX = GPS_BAUDRATE_9600
} gpsBaudRate_e;
// Serial GPS only variables
// navigation mode
typedef enum NavigationMode
{
typedef enum {
NAV_MODE_NONE = 0,
NAV_MODE_POSHOLD,
NAV_MODE_WP
} NavigationMode;
} navigationMode_e;
typedef struct gpsProfile_s {
uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
@ -40,6 +37,12 @@ typedef struct gpsProfile_s {
uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS
} gpsProfile_t;
typedef enum {
GPS_PASSTHROUGH_ENABLED = 1,
GPS_PASSTHROUGH_NO_GPS,
GPS_PASSTHROUGH_NO_SERIAL_PORT
} gpsEnablePassthroughResult_e;
extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction
extern int32_t GPS_coord[2]; // LAT/LON
extern int32_t GPS_home[2];
@ -62,7 +65,7 @@ extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Str
extern int8_t nav_mode; // Navigation mode
void gpsThread(void);
int8_t gpsSetPassthrough(void);
gpsEnablePassthroughResult_e gpsEnablePassthrough(void);
void GPS_reset_home_position(void);
void GPS_reset_nav(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon);

71
src/main.c
View file

@ -44,22 +44,21 @@
#include "build_config.h"
//#define USE_SOFTSERIAL_FOR_MAIN_PORT
extern rcReadRawDataPtr rcReadRawFunc;
extern uint32_t previousTime;
failsafe_t *failsafe;
void initPrintfSupport(void);
void timerInit(void);
void initTelemetry(serialPorts_t *serialPorts);
void initTelemetry(void);
void serialInit(serialConfig_t *initialSerialConfig);
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
void pwmInit(drv_pwm_config_t *init);
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
void buzzerInit(failsafe_t *initialFailsafe);
void gpsInit(uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
void gpsInit(serialConfig_t *serialConfig, uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig);
void imuInit(void);
@ -98,13 +97,13 @@ int main(void)
serialPort_t* loopbackPort2 = NULL;
#endif
initPrintfSupport();
ensureEEPROMContainsValidData();
readEEPROM();
systemInit(masterConfig.emf_avoidance);
initPrintfSupport();
// configure power ADC
if (masterConfig.power_adc_channel > 0 && (masterConfig.power_adc_channel == 1 || masterConfig.power_adc_channel == 9))
adc_params.powerAdcChannel = masterConfig.power_adc_channel;
@ -151,6 +150,10 @@ int main(void)
imuInit(); // Mag is initialized inside imuInit
mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer);
if (!canSoftwareSerialBeUsed()) {
featureClear(FEATURE_SOFTSERIAL);
}
serialInit(&masterConfig.serialConfig);
// when using airplane/wing mixer, servo/motor outputs are remapped
@ -194,10 +197,11 @@ int main(void)
rxInit(&masterConfig.rxConfig, failsafe);
if (feature(FEATURE_GPS) && !feature(FEATURE_SERIALRX)) {
if (feature(FEATURE_GPS)) {
gpsInit(
masterConfig.gps_baudrate,
masterConfig.gps_type,
&masterConfig.serialConfig,
masterConfig.gps_initial_baudrate_index,
masterConfig.gps_provider,
&currentProfile.gpsProfile,
&currentProfile.pidProfile
);
@ -211,32 +215,8 @@ int main(void)
}
#endif
#ifndef FY90Q
if (canSoftwareSerialBeUsed()) {
#if defined(USE_SOFTSERIAL_FOR_MAIN_PORT) || (0)
masterConfig.serialConfig.softserial_baudrate = 19200;
#endif
setupSoftSerialPrimary(masterConfig.serialConfig.softserial_baudrate, masterConfig.serialConfig.softserial_1_inverted);
setupSoftSerialSecondary(masterConfig.serialConfig.softserial_2_inverted);
#ifdef SOFTSERIAL_LOOPBACK
loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]);
serialPrint(loopbackPort1, "SOFTSERIAL 1 - LOOPBACK ENABLED\r\n");
loopbackPort2 = (serialPort_t*)&(softSerialPorts[1]);
#ifndef OLIMEXINO // PB0/D27 and PB1/D28 internally connected so this would result in a continuous stream of data
serialPrint(loopbackPort2, "SOFTSERIAL 2 - LOOPBACK ENABLED\r\n");
#endif
#endif
#ifdef USE_SOFTSERIAL_FOR_MAIN_PORT
serialPorts.mainport = (serialPort_t*)&(softSerialPorts[0]); // Uncomment to switch the main port to use softserial.
#endif
}
#endif
if (feature(FEATURE_TELEMETRY))
initTelemetry(&serialPorts);
initTelemetry();
previousTime = micros();
@ -253,29 +233,6 @@ int main(void)
// loopy
while (1) {
loop();
#ifdef SOFTSERIAL_LOOPBACK
if (loopbackPort1) {
while (serialTotalBytesWaiting(loopbackPort1)) {
uint8_t b = serialRead(loopbackPort1);
serialWrite(loopbackPort1, b);
//serialWrite(core.mainport, 0x01);
//serialWrite(core.mainport, b);
};
}
if (loopbackPort2) {
while (serialTotalBytesWaiting(loopbackPort2)) {
#ifndef OLIMEXINO // PB0/D27 and PB1/D28 internally connected so this would result in a continuous stream of data
serialRead(loopbackPort2);
#else
uint8_t b = serialRead(loopbackPort2);
serialWrite(loopbackPort2, b);
//serialWrite(core.mainport, 0x02);
//serialWrite(core.mainport, b);
#endif // OLIMEXINO
};
}
#endif
}
}

4
src/rx_common.c
View file

@ -34,13 +34,13 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
#define PULSE_MAX 2250 // maximum PWM pulse width which is considered valid
rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
static rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
rxRuntimeConfig_t rxRuntimeConfig;
void serialRxInit(rxConfig_t *rxConfig);
failsafe_t *failsafe;
static failsafe_t *failsafe;
void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe)
{

5
src/rx_sbus.c
View file

@ -25,15 +25,16 @@ static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
static serialPort_t *sBusPort;
void sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
{
int b;
//rxConfig = initialRxConfig;
sBusPort = openSerialPort(FUNCTION_SERIAL_RX, sbusDataReceive, 100000, (portMode_t)(MODE_RX | MODE_SBUS), SERIAL_NOT_INVERTED);
for (b = 0; b < SBUS_MAX_CHANNEL; b++)
sbusChannelData[b] = 2 * (rxConfig->midrc - SBUS_OFFSET);
serialPorts.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, (portMode_t)(MODE_RX | MODE_SBUS));
if (callback)
*callback = sbusReadRawRC;
rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;

6
src/rx_spektrum.c
View file

@ -26,11 +26,13 @@ static bool rcFrameComplete = false;
static bool spekHiRes = false;
static bool spekDataIncoming = false;
volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
volatile static uint8_t spekFrame[SPEK_FRAME_SIZE];
static void spektrumDataReceive(uint16_t c);
static uint16_t spektrumReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
static serialPort_t *spektrumPort;
void spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
{
switch (rxConfig->serialrx_type) {
@ -50,7 +52,7 @@ void spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcRe
break;
}
serialPorts.rcvrport = uartOpen(USART2, spektrumDataReceive, 115200, MODE_RX);
spektrumPort = openSerialPort(FUNCTION_SERIAL_RX, spektrumDataReceive, 115200, MODE_RX, SERIAL_NOT_INVERTED);
if (callback)
*callback = spektrumReadRawRC;
}

4
src/rx_sumd.c
View file

@ -24,9 +24,11 @@ static uint16_t sumdReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
static uint32_t sumdChannelData[SUMD_MAX_CHANNEL];
static serialPort_t *sumdPort;
void sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
{
serialPorts.rcvrport = uartOpen(USART2, sumdDataReceive, 115200, MODE_RX);
sumdPort = openSerialPort(FUNCTION_SERIAL_RX, sumdDataReceive, 115200, MODE_RX, SERIAL_NOT_INVERTED);
if (callback)
*callback = sumdReadRawRC;

74
src/serial_cli.c
View file

@ -9,7 +9,6 @@
#include "platform.h"
#include "common/axis.h"
#include "common/printf.h"
#include "common/typeconversion.h"
#include "drivers/system_common.h"
@ -38,6 +37,8 @@
#include "config_profile.h"
#include "config_master.h"
#include "common/printf.h"
#include "serial_cli.h"
// we unset this on 'exit'
@ -59,7 +60,9 @@ static void cliSet(char *cmdline);
static void cliStatus(char *cmdline);
static void cliVersion(char *cmdline);
uint16_t cycleTime; // FIXME dependency on mw.c
extern uint16_t cycleTime; // FIXME dependency on mw.c
static serialPort_t *cliPort;
// signal that we're in cli mode
uint8_t cliMode = 0;
@ -166,19 +169,18 @@ const clivalue_t valueTable[] = {
{ "fixedwing_althold_dir", VAR_INT8, &masterConfig.fixedwing_althold_dir, -1, 1 },
{ "reboot_character", VAR_UINT8, &masterConfig.serialConfig.reboot_character, 48, 126 },
{ "serial_baudrate", VAR_UINT32, &masterConfig.serialConfig.port1_baudrate, 1200, 115200 },
{ "softserial_baudrate", VAR_UINT32, &masterConfig.serialConfig.softserial_baudrate, 1200, 19200 },
{ "softserial_1_inverted", VAR_UINT8, &masterConfig.serialConfig.softserial_1_inverted, 0, 1 },
{ "softserial_2_inverted", VAR_UINT8, &masterConfig.serialConfig.softserial_2_inverted, 0, 1 },
{ "msp_baudrate", VAR_UINT32, &masterConfig.serialConfig.msp_baudrate, 1200, 115200 },
{ "cli_baudrate", VAR_UINT32, &masterConfig.serialConfig.cli_baudrate, 1200, 115200 },
{ "gps_passthrough_baudrate", VAR_UINT32, &masterConfig.serialConfig.gps_passthrough_baudrate, 1200, 115200 },
{ "gps_type", VAR_UINT8, &masterConfig.gps_type, 0, GPS_HARDWARE_MAX },
{ "gps_baudrate", VAR_INT8, &masterConfig.gps_baudrate, 0, GPS_BAUD_MAX },
{ "gps_provider", VAR_UINT8, &masterConfig.gps_provider, 0, GPS_PROVIDER_MAX },
{ "gps_initial_baudrate_index", VAR_INT8, &masterConfig.gps_initial_baudrate_index, 0, GPS_BAUDRATE_MAX },
{ "serialrx_type", VAR_UINT8, &masterConfig.rxConfig.serialrx_type, 0, SERIALRX_PROVIDER_MAX },
{ "telemetry_provider", VAR_UINT8, &masterConfig.telemetryConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX },
{ "telemetry_port", VAR_UINT8, &masterConfig.telemetryConfig.telemetry_port, 0, TELEMETRY_PORT_MAX },
{ "telemetry_switch", VAR_UINT8, &masterConfig.telemetryConfig.telemetry_switch, 0, 1 },
{ "frsky_inversion", VAR_UINT8, &masterConfig.telemetryConfig.frsky_inversion, 0, 1 },
{ "vbatscale", VAR_UINT8, &masterConfig.batteryConfig.vbatscale, 10, 200 },
{ "vbatmaxcellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 },
@ -493,6 +495,15 @@ static void cliDump(char *cmdline)
}
}
static void cliEnter(void)
{
cliMode = 1;
beginSerialPortFunction(cliPort, FUNCTION_CLI);
setPrintfSerialPort(cliPort);
cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n");
cliPrompt();
}
static void cliExit(char *cmdline)
{
cliPrint("\r\nLeaving CLI mode...\r\n");
@ -501,8 +512,12 @@ static void cliExit(char *cmdline)
cliMode = 0;
// incase some idiot leaves a motor running during motortest, clear it here
mixerResetMotors();
// save and reboot... I think this makes the most sense
// save and reboot... I think this makes the most sense - otherwise config changes can be out of sync, maybe just need to applyConfig and return?
#if 1
cliSave(cmdline);
#else
releaseSerialPort(cliPort, FUNCTION_CLI);
#endif
}
static void cliFeature(char *cmdline)
@ -563,10 +578,20 @@ static void cliFeature(char *cmdline)
static void cliGpsPassthrough(char *cmdline)
{
if (gpsSetPassthrough() == -1)
gpsEnablePassthroughResult_e result = gpsEnablePassthrough();
switch (result) {
case GPS_PASSTHROUGH_NO_GPS:
cliPrint("Error: Enable and plug in GPS first\r\n");
else
cliPrint("Enabling GPS passthrough...\r\n");
break;
case GPS_PASSTHROUGH_NO_SERIAL_PORT:
cliPrint("Error: Enable and plug in GPS first\r\n");
break;
default:
break;
}
}
static void cliHelp(char *cmdline)
@ -707,7 +732,7 @@ static void cliProfile(char *cmdline)
static void cliReboot(void) {
cliPrint("\r\nRebooting...");
delay(10);
waitForSerialPortToFinishTransmitting(cliPort);
systemReset(false);
}
@ -729,12 +754,12 @@ static void cliDefaults(char *cmdline)
static void cliPrint(const char *str)
{
while (*str)
serialWrite(serialPorts.mainport, *(str++));
serialWrite(cliPort, *(str++));
}
static void cliWrite(uint8_t ch)
{
serialWrite(serialPorts.mainport, ch);
serialWrite(cliPort, ch);
}
static void cliPrintVar(const clivalue_t *var, uint32_t full)
@ -890,13 +915,11 @@ static void cliVersion(char *cmdline)
void cliProcess(void)
{
if (!cliMode) {
cliMode = 1;
cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n");
cliPrompt();
cliEnter();
}
while (serialTotalBytesWaiting(serialPorts.mainport)) {
uint8_t c = serialRead(serialPorts.mainport);
while (serialTotalBytesWaiting(cliPort)) {
uint8_t c = serialRead(cliPort);
if (c == '\t' || c == '?') {
// do tab completion
const clicmd_t *cmd, *pstart = NULL, *pend = NULL;
@ -977,3 +1000,12 @@ void cliProcess(void)
}
}
}
void cliInit(serialConfig_t *serialConfig)
{
cliPort = findOpenSerialPort(FUNCTION_CLI);
if (!cliPort) {
cliPort = openSerialPort(FUNCTION_CLI, NULL, serialConfig->cli_baudrate, MODE_RXTX, SERIAL_NOT_INVERTED);
}
}

292
src/serial_common.c
View file

@ -5,7 +5,10 @@
#include "platform.h"
#include "drivers/system_common.h"
#include "drivers/gpio_common.h"
#include "drivers/timer_common.h"
#include "drivers/serial_common.h"
#include "drivers/serial_softserial.h"
#include "drivers/serial_uart_common.h"
#include "serial_cli.h"
@ -13,26 +16,280 @@
#include "serial_common.h"
void mspInit(serialConfig_t *serialConfig);
void cliInit(serialConfig_t *serialConfig);
static serialConfig_t *serialConfig;
serialPorts_t serialPorts;
static serialPort_t *serialPorts[SERIAL_PORT_COUNT];
static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
{SERIAL_PORT_USART1, NULL, SCENARIO_MAIN_PORT, FUNCTION_NONE},
{SERIAL_PORT_USART2, NULL, SCENARIO_GPS_AND_TELEMETRY, FUNCTION_NONE},
{SERIAL_PORT_SOFTSERIAL1, NULL, SCENARIO_UNUSED, FUNCTION_NONE},
{SERIAL_PORT_SOFTSERIAL2, NULL, SCENARIO_UNUSED, FUNCTION_NONE}
};
#if 0 // example using softserial for telemetry, note that it is used because the scenario is more specific than telemetry on usart1
static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
{SERIAL_PORT_USART1, NULL, SCENARIO_MAIN_PORT, FUNCTION_NONE},
{SERIAL_PORT_USART2, NULL, SCENARIO_GPS_ONLY, FUNCTION_NONE},
{SERIAL_PORT_SOFTSERIAL1, NULL, SCENARIO_TELEMETRY_ONLY, FUNCTION_NONE},
{SERIAL_PORT_SOFTSERIAL2, NULL, SCENARIO_UNUSED, FUNCTION_NONE}
};
#endif
const static serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
{SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE },
{SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE},
{SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_IS_SOFTWARE_INVERTABLE},
{SERIAL_PORT_SOFTSERIAL2, 9600, 19200, SPF_IS_SOFTWARE_INVERTABLE}
};
typedef struct functionConstraint_s {
serialPortFunction_e function;
uint8_t requiredSerialPortFeatures;
} functionConstraint_t;
const functionConstraint_t functionConstraints[] = {
{ FUNCTION_CLI, SPF_NONE },
{ FUNCTION_GPS, SPF_NONE },
{ FUNCTION_GPS_PASSTHROUGH, SPF_NONE },
{ FUNCTION_MSP, SPF_NONE },
{ FUNCTION_SERIAL_RX, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK },
{ FUNCTION_TELEMETRY, SPF_NONE }
};
#define FUNCTION_CONSTRAINT_COUNT (sizeof(functionConstraints) / sizeof(functionConstraint_t))
typedef struct serialPortSearchResult_s {
bool found;
serialPortIndex_e portIndex;
serialPortFunction_t *portFunction;
const serialPortConstraint_t *portConstraint;
const functionConstraint_t *functionConstraint;
} serialPortSearchResult_t;
static serialPortIndex_e findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier)
{
serialPortIndex_e portIndex;
for (portIndex = 0; portIndex < SERIAL_PORT_COUNT; portIndex++) {
if (serialPortConstraints[portIndex].identifier == identifier) {
return portIndex;
}
}
return SERIAL_PORT_1; // FIXME use failureMode() ? - invalid identifier used.
}
static const functionConstraint_t *findFunctionConstraint(serialPortFunction_e function)
{
const functionConstraint_t *functionConstraint = NULL;
uint8_t functionConstraintIndex;
for (functionConstraintIndex = 0; functionConstraintIndex < FUNCTION_CONSTRAINT_COUNT; functionConstraintIndex++) {
functionConstraint = &functionConstraints[functionConstraintIndex];
if (functionConstraint->function == function) {
return functionConstraint;
}
}
return NULL;
}
static uint8_t countBits_uint32(uint32_t n) {
uint8_t c; // c accumulates the total bits set in n
for (c = 0; n; c++)
n &= n - 1; // clear the least significant bit set
return c;
}
static int serialPortFunctionMostSpecificFirstComparator(const void *aPtr, const void *bPtr)
{
serialPortFunction_t *a = (serialPortFunction_t *)aPtr;
serialPortFunction_t *b = (serialPortFunction_t *)bPtr;
return countBits_uint32(a->scenario) > countBits_uint32(b->scenario);
}
static void sortSerialPortFunctions(serialPortFunction_t *serialPortFunctions, uint8_t elements)
{
qsort(serialPortFunctions, elements, sizeof(serialPortFunction_t), serialPortFunctionMostSpecificFirstComparator);
}
/*
* since this method, and other methods that use it, use a single instance of
* searchPortSearchResult be sure to copy the data out of it before it gets overwritten by another caller.
* If this becomes a problem perhaps change the implementation to use a destination argument.
*/
static serialPortSearchResult_t *findSerialPort(serialPortFunction_e function)
{
static serialPortSearchResult_t serialPortSearchResult;
serialPortSearchResult.found = false;
const functionConstraint_t *functionConstraint = findFunctionConstraint(function);
if (!functionConstraint) {
return NULL;
}
sortSerialPortFunctions(serialPortFunctions, SERIAL_PORT_COUNT);
uint8_t serialPortFunctionIndex;
serialPortFunction_t *serialPortFunction;
for (serialPortFunctionIndex = 0; serialPortFunctionIndex < SERIAL_PORT_COUNT; serialPortFunctionIndex++) {
serialPortFunction = &serialPortFunctions[serialPortFunctionIndex];
if (!(serialPortFunction->scenario & function)) {
continue;
}
uint8_t serialPortIndex = findSerialPortIndexByIdentifier(serialPortFunction->identifier);
const serialPortConstraint_t *serialPortConstraint = &serialPortConstraints[serialPortIndex];
if (functionConstraint->requiredSerialPortFeatures != SPF_NONE) {
if (!(serialPortConstraint->feature & functionConstraint->requiredSerialPortFeatures)) {
continue;
}
}
// TODO check speed and mode
serialPortSearchResult.portIndex = serialPortIndex;
serialPortSearchResult.portConstraint = serialPortConstraint;
serialPortSearchResult.portFunction = serialPortFunction;
serialPortSearchResult.functionConstraint = functionConstraint;
serialPortSearchResult.found = true;
break;
}
return &serialPortSearchResult;
}
bool canOpenSerialPort(uint16_t functionMask)
{
serialPortSearchResult_t *result = findSerialPort(functionMask);
return result->found;
}
serialPortFunction_t *findSerialPortFunction(uint16_t functionMask)
{
serialPortIndex_e portIndex;
// find exact match first
for (portIndex = 0; portIndex < SERIAL_PORT_COUNT; portIndex++) {
serialPortFunction_t *serialPortFunction = &serialPortFunctions[portIndex];
if (serialPortFunction->scenario == functionMask) {
return serialPortFunction;
}
}
// find the first port that supports the function requested
for (portIndex = 0; portIndex < SERIAL_PORT_COUNT; portIndex++) {
serialPortFunction_t *serialPortFunction = &serialPortFunctions[portIndex];
if (serialPortFunction->scenario & functionMask) {
return serialPortFunction;
}
}
return NULL;
}
/*
* find a serial port that is:
* a) open
* b) matches the function mask exactly, or if an exact match is not found the first port that supports the function
*/
serialPort_t *findOpenSerialPort(uint16_t functionMask)
{
serialPortFunction_t *function = findSerialPortFunction(functionMask);
if (!function) {
return NULL;
}
return function->port;
}
serialPort_t *openSerialPort(serialPortFunction_e function, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion)
{
serialPort_t *serialPort = NULL;
serialPortSearchResult_t *searchResult = findSerialPort(function);
if (!searchResult->found) {
return NULL;
}
serialPortIndex_e portIndex = searchResult->portIndex;
const serialPortConstraint_t *serialPortConstraint = searchResult->portConstraint;
serialPortIdentifier_e identifier = serialPortConstraint->identifier;
switch(identifier) {
case SERIAL_PORT_USART1:
serialPort = uartOpen(USART1, callback, baudRate, mode, inversion);
break;
case SERIAL_PORT_USART2:
serialPort = uartOpen(USART2, callback, baudRate, mode, inversion);
break;
case SERIAL_PORT_SOFTSERIAL1:
serialPort = openSoftSerial1(baudRate, inversion);
serialSetMode(serialPort, mode);
break;
case SERIAL_PORT_SOFTSERIAL2:
serialPort = openSoftSerial2(baudRate, inversion);
serialSetMode(serialPort, mode);
break;
}
if (!serialPort) {
return NULL;
}
serialPort->identifier = identifier;
serialPorts[portIndex] = serialPort;
serialPortFunction_t *serialPortFunction = searchResult->portFunction;
serialPortFunction->port = serialPort;
//serialPortFunction->identifier = identifier;
beginSerialPortFunction(serialPort, function);
return serialPort;
}
static serialPortFunction_t * findSerialPortFunctionByPort(serialPort_t *port)
{
serialPortFunction_t *serialPortFunction;
uint8_t functionIndex;
for (functionIndex = 0; functionIndex < SERIAL_PORT_COUNT; functionIndex++) {
serialPortFunction = &serialPortFunctions[functionIndex];
if (serialPortFunction->port == port) {
return serialPortFunction;
}
}
return NULL;
}
void beginSerialPortFunction(serialPort_t *port, serialPortFunction_e function)
{
serialPortFunction_t *serialPortFunction = findSerialPortFunctionByPort(port);
serialPortFunction->currentFunction = function;
}
void endSerialPortFunction(serialPort_t *port, serialPortFunction_e function)
{
serialPortFunction_t *serialPortFunction = findSerialPortFunctionByPort(port);
serialPortFunction->currentFunction = FUNCTION_NONE;
}
void serialInit(serialConfig_t *initialSerialConfig)
{
serialConfig = initialSerialConfig;
resetMainSerialPort();
mspInit();
}
void resetMainSerialPort(void)
{
openMainSerialPort(serialConfig->port1_baudrate);
}
void openMainSerialPort(uint32_t baudrate)
{
serialPorts.mainport = uartOpen(USART1, NULL, baudrate, MODE_RXTX);
mspInit(serialConfig);
cliInit(serialConfig);
}
void handleSerial(void)
@ -46,6 +303,13 @@ void handleSerial(void)
mspProcess();
}
void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort)
{
while (!isSerialTransmitBufferEmpty(serialPort)) {
delay(10);
};
}
// evaluate all other incoming serial data
void evaluateOtherData(uint8_t sr)
{

87
src/serial_common.h
View file

@ -1,24 +1,85 @@
#pragma once
typedef struct serialPorts_s {
serialPort_t *mainport;
serialPort_t *gpsport;
serialPort_t *telemport;
serialPort_t *rcvrport;
} serialPorts_t;
typedef enum {
FUNCTION_NONE = 0,
FUNCTION_MSP = (1 << 0),
FUNCTION_CLI = (1 << 1),
FUNCTION_TELEMETRY = (1 << 2),
FUNCTION_SERIAL_RX = (1 << 3),
FUNCTION_GPS = (1 << 4),
FUNCTION_GPS_PASSTHROUGH = (1 << 5)
} serialPortFunction_e;
typedef enum {
SCENARIO_MAIN_PORT = FUNCTION_MSP | FUNCTION_CLI | FUNCTION_TELEMETRY | FUNCTION_GPS_PASSTHROUGH,
SCENARIO_CLI_ONLY = FUNCTION_CLI,
SCENARIO_GPS_AND_TELEMETRY = FUNCTION_GPS | FUNCTION_TELEMETRY,
SCENARIO_GPS_PASSTHROUGH_ONLY = FUNCTION_GPS_PASSTHROUGH,
SCENARIO_GPS_ONLY = FUNCTION_GPS,
SCENARIO_MSP_ONLY = FUNCTION_MSP,
SCENARIO_SERIAL_RX_ONLY = FUNCTION_SERIAL_RX,
SCENARIO_TELEMETRY_ONLY = FUNCTION_TELEMETRY,
SCENARIO_MSP_CLI_GPS_PASTHROUGH = FUNCTION_CLI | FUNCTION_MSP | FUNCTION_GPS_PASSTHROUGH,
SCENARIO_UNUSED = FUNCTION_NONE
} serialPortFunctionScenario_e;
#define SERIAL_PORT_COUNT 4
typedef enum {
SERIAL_PORT_1 = 0,
SERIAL_PORT_2,
SERIAL_PORT_3,
SERIAL_PORT_4
} serialPortIndex_e;
typedef enum {
SERIAL_PORT_USART1 = 0,
SERIAL_PORT_USART2,
SERIAL_PORT_SOFTSERIAL1,
SERIAL_PORT_SOFTSERIAL2
} serialPortIdentifier_e;
// bitmask
typedef enum {
SPF_NONE = 0,
SPF_SUPPORTS_CALLBACK = (1 << 0),
SPF_SUPPORTS_SBUS_MODE = (1 << 1),
SPF_IS_SOFTWARE_INVERTABLE = (1 << 2)
} serialPortFeature_t;
typedef struct serialPortConstraint_s {
const serialPortIdentifier_e identifier;
uint32_t minBaudRate;
uint32_t maxBaudRate;
serialPortFeature_t feature;
} serialPortConstraint_t;
typedef struct serialPortFunction_s {
serialPortIdentifier_e identifier;
serialPort_t *port; // a NULL values indicates the port has not been opened yet.
serialPortFunctionScenario_e scenario;
serialPortFunction_e currentFunction;
} serialPortFunction_t;
typedef struct serialConfig_s {
uint32_t port1_baudrate;
uint32_t msp_baudrate;
uint32_t cli_baudrate;
uint32_t gps_passthrough_baudrate;
uint32_t hott_baudrate;
uint32_t softserial_baudrate; // shared by both soft serial ports
uint8_t softserial_1_inverted; // use inverted softserial input and output signals on port 1
uint8_t softserial_2_inverted; // use inverted softserial input and output signals on port 2
uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else.
} serialConfig_t;
extern serialPorts_t serialPorts;
serialPort_t *findOpenSerialPort(uint16_t functionMask);
serialPort_t *openSerialPort(serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion);
bool canOpenSerialPort(uint16_t functionMask);
void beginSerialPortFunction(serialPort_t *port, serialPortFunction_e function);
void endSerialPortFunction(serialPort_t *port, serialPortFunction_e function);
serialPortFunction_t *findSerialPortFunction(uint16_t functionMask);
void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort);
void resetMainSerialPort(void);
void openMainSerialPort(uint32_t baudrate);
void evaluateOtherData(uint8_t sr);
void handleSerial(void);

27
src/serial_msp.c
View file

@ -39,6 +39,8 @@
#include "serial_msp.h"
static serialPort_t *mspPort;
extern uint16_t cycleTime; // FIXME dependency on mw.c
extern uint16_t rssi; // FIXME dependency on mw.c
extern int16_t debug[4]; // FIXME dependency on mw.c
@ -162,16 +164,16 @@ void serialize32(uint32_t a)
{
static uint8_t t;
t = a;
serialWrite(serialPorts.mainport, t);
serialWrite(mspPort, t);
checksum ^= t;
t = a >> 8;
serialWrite(serialPorts.mainport, t);
serialWrite(mspPort, t);
checksum ^= t;
t = a >> 16;
serialWrite(serialPorts.mainport, t);
serialWrite(mspPort, t);
checksum ^= t;
t = a >> 24;
serialWrite(serialPorts.mainport, t);
serialWrite(mspPort, t);
checksum ^= t;
}
@ -179,16 +181,16 @@ void serialize16(int16_t a)
{
static uint8_t t;
t = a;
serialWrite(serialPorts.mainport, t);
serialWrite(mspPort, t);
checksum ^= t;
t = a >> 8 & 0xff;
serialWrite(serialPorts.mainport, t);
serialWrite(mspPort, t);
checksum ^= t;
}
void serialize8(uint8_t a)
{
serialWrite(serialPorts.mainport, a);
serialWrite(mspPort, a);
checksum ^= a;
}
@ -275,7 +277,7 @@ reset:
}
}
void mspInit(void)
void mspInit(serialConfig_t *serialConfig)
{
int idx;
@ -313,6 +315,11 @@ void mspInit(void)
if (feature(FEATURE_TELEMETRY && masterConfig.telemetryConfig.telemetry_switch))
availableBoxes[idx++] = BOXTELEMETRY;
numberBoxItems = idx;
mspPort = findOpenSerialPort(FUNCTION_MSP);
if (!mspPort) {
mspPort = openSerialPort(FUNCTION_MSP, NULL, serialConfig->msp_baudrate, MODE_RXTX, SERIAL_NOT_INVERTED);
}
}
static void evaluateCommand(void)
@ -713,8 +720,8 @@ void mspProcess(void)
HEADER_CMD,
} c_state = IDLE;
while (serialTotalBytesWaiting(serialPorts.mainport)) {
c = serialRead(serialPorts.mainport);
while (serialTotalBytesWaiting(mspPort)) {
c = serialRead(mspPort);
if (c_state == IDLE) {
c_state = (c == '$') ? HEADER_START : IDLE;

1
src/serial_msp.h
View file

@ -1,5 +1,4 @@
#pragma once
void mspProcess(void);
void mspInit(void);

53
src/telemetry_common.c
View file

@ -1,5 +1,6 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
@ -12,14 +13,14 @@
#include "runtime_config.h"
#include "config.h"
#include "telemetry_common.h"
#include "telemetry_frsky.h"
#include "telemetry_hott.h"
#include "telemetry_common.h"
static bool isTelemetryConfigurationValid = false; // flag used to avoid repeated configuration checks
telemetryConfig_t *telemetryConfig;
static telemetryConfig_t *telemetryConfig;
void useTelemetryConfig(telemetryConfig_t *telemetryConfigToUse)
{
@ -42,46 +43,18 @@ bool canUseTelemetryWithCurrentConfiguration(void)
return false;
}
if (!feature(FEATURE_SOFTSERIAL)) {
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1 || telemetryConfig->telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) {
// softserial feature must be enabled to use telemetry on softserial ports
if (!canOpenSerialPort(FUNCTION_TELEMETRY)) {
return false;
}
}
if (isTelemetryProviderHoTT()) {
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_UART) {
// HoTT requires a serial port that supports RX/TX mode swapping
return false;
}
}
return true;
}
void initTelemetry(serialPorts_t *serialPorts)
void initTelemetry()
{
// Force telemetry to uart when softserial disabled
if (!canSoftwareSerialBeUsed())
telemetryConfig->telemetry_port = TELEMETRY_PORT_UART;
#ifdef FY90Q
// FY90Q does not support softserial
telemetryConfig->telemetry_port = TELEMETRY_PORT_UART;
serialPorts->telemport = serialPorts->mainport;
#endif
isTelemetryConfigurationValid = canUseTelemetryWithCurrentConfiguration();
#ifndef FY90Q
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1)
serialPorts->telemport = &(softSerialPorts[0].port);
else if (telemetryConfig->telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2)
serialPorts->telemport = &(softSerialPorts[1].port);
else
serialPorts->telemport = serialPorts->mainport;
#endif
checkTelemetryState();
}
@ -89,13 +62,17 @@ static bool telemetryEnabled = false;
bool determineNewTelemetryEnabledState(void)
{
bool telemetryPortIsShared;
bool enabled = true;
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_UART) {
if (!telemetryConfig->telemetry_switch)
enabled = f.ARMED;
else
serialPortFunction_t *function = findSerialPortFunction(FUNCTION_TELEMETRY);
telemetryPortIsShared = function && function->scenario != SCENARIO_TELEMETRY_ONLY;
if (telemetryPortIsShared) {
if (telemetryConfig->telemetry_switch)
enabled = rcOptions[BOXTELEMETRY];
else
enabled = f.ARMED;
}
return enabled;
@ -109,11 +86,11 @@ bool shouldChangeTelemetryStateNow(bool newState)
static void configureTelemetryPort(void)
{
if (isTelemetryProviderFrSky()) {
configureFrSkyTelemetryPort();
configureFrSkyTelemetryPort(telemetryConfig);
}
if (isTelemetryProviderHoTT()) {
configureHoTTTelemetryPort();
configureHoTTTelemetryPort(telemetryConfig);
}
}

13
src/telemetry_common.h
View file

@ -12,19 +12,12 @@ typedef enum {
TELEMETRY_PROVIDER_FRSKY = 0,
TELEMETRY_PROVIDER_HOTT,
TELEMETRY_PROVIDER_MAX = TELEMETRY_PROVIDER_HOTT
} TelemetryProvider;
typedef enum {
TELEMETRY_PORT_UART = 0,
TELEMETRY_PORT_SOFTSERIAL_1, // Requires FEATURE_SOFTSERIAL
TELEMETRY_PORT_SOFTSERIAL_2, // Requires FEATURE_SOFTSERIAL
TELEMETRY_PORT_MAX = TELEMETRY_PORT_SOFTSERIAL_2
} TelemetryPort;
} telemetryProvider_e;
typedef struct telemetryConfig_s {
uint8_t telemetry_provider; // See TelemetryProvider enum.
uint8_t telemetry_port; // See TelemetryPort enum.
telemetryProvider_e telemetry_provider;
uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
serialInversion_e frsky_inversion;
} telemetryConfig_t;
void checkTelemetryState(void);

59
src/telemetry_frsky.c
View file

@ -3,6 +3,7 @@
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
@ -29,8 +30,13 @@
#include "telemetry_common.h"
#include "telemetry_frsky.h"
extern telemetryConfig_t *telemetryConfig;
int16_t telemTemperature1; // FIXME dependency on mw.c
static serialPort_t *frskyPort;
#define FRSKY_BAUDRATE 9600
#define FRSKY_INITIAL_PORT_MODE MODE_TX
telemetryConfig_t *telemetryConfig;
extern int16_t telemTemperature1; // FIXME dependency on mw.c
#define CYCLETIME 125
@ -77,26 +83,26 @@ int16_t telemTemperature1; // FIXME dependency on mw.c
static void sendDataHead(uint8_t id)
{
serialWrite(serialPorts.telemport, PROTOCOL_HEADER);
serialWrite(serialPorts.telemport, id);
serialWrite(frskyPort, PROTOCOL_HEADER);
serialWrite(frskyPort, id);
}
static void sendTelemetryTail(void)
{
serialWrite(serialPorts.telemport, PROTOCOL_TAIL);
serialWrite(frskyPort, PROTOCOL_TAIL);
}
static void serializeFrsky(uint8_t data)
{
// take care of byte stuffing
if (data == 0x5e) {
serialWrite(serialPorts.telemport, 0x5d);
serialWrite(serialPorts.telemport, 0x3e);
serialWrite(frskyPort, 0x5d);
serialWrite(frskyPort, 0x3e);
} else if (data == 0x5d) {
serialWrite(serialPorts.telemport, 0x5d);
serialWrite(serialPorts.telemport, 0x3d);
serialWrite(frskyPort, 0x5d);
serialWrite(frskyPort, 0x3d);
} else
serialWrite(serialPorts.telemport, data);
serialWrite(frskyPort, data);
}
static void serialize16(int16_t a)
@ -237,17 +243,36 @@ static void sendHeading(void)
serialize16(0);
}
static portMode_t previousPortMode;
static uint32_t previousBaudRate;
void freeFrSkyTelemetryPort(void)
{
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_UART) {
resetMainSerialPort();
}
// FIXME only need to reset the port if the port is shared
serialSetMode(frskyPort, previousPortMode);
serialSetBaudRate(frskyPort, previousBaudRate);
endSerialPortFunction(frskyPort, FUNCTION_TELEMETRY);
}
void configureFrSkyTelemetryPort(void)
void configureFrSkyTelemetryPort(telemetryConfig_t *telemetryConfig)
{
if (telemetryConfig->telemetry_port == TELEMETRY_PORT_UART) {
openMainSerialPort(9600);
frskyPort = findOpenSerialPort(FUNCTION_TELEMETRY);
if (frskyPort) {
previousPortMode = frskyPort->mode;
previousBaudRate = frskyPort->baudRate;
//waitForSerialPortToFinishTransmitting(frskyPort); // FIXME locks up the system
serialSetBaudRate(frskyPort, FRSKY_BAUDRATE);
serialSetMode(frskyPort, FRSKY_INITIAL_PORT_MODE);
beginSerialPortFunction(frskyPort, FUNCTION_TELEMETRY);
} else {
frskyPort = openSerialPort(FUNCTION_TELEMETRY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->frsky_inversion);
// FIXME only need these values to reset the port if the port is shared
previousPortMode = frskyPort->mode;
previousBaudRate = frskyPort->baudRate;
}
}
@ -256,7 +281,7 @@ static uint8_t cycleNum = 0;
bool canSendFrSkyTelemetry(void)
{
return serialTotalBytesWaiting(serialPorts.telemport) == 0;
return serialTotalBytesWaiting(frskyPort) == 0;
}
bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis)

2
src/telemetry_frsky.h
View file

@ -11,7 +11,7 @@
void handleFrSkyTelemetry(void);
void checkFrSkyTelemetryState(void);
void configureFrSkyTelemetryPort(void);
void configureFrSkyTelemetryPort(telemetryConfig_t *telemetryConfig);
void freeFrSkyTelemetryPort(void);
#endif /* TELEMETRY_FRSKY_H_ */

49
src/telemetry_hott.c
View file

@ -64,6 +64,10 @@
#include "telemetry_common.h"
#include "telemetry_hott.h"
static serialPort_t *hottPort;
#define HOTT_BAUDRATE 19200
#define HOTT_INITIAL_PORT_MODE MODE_RX
extern telemetryConfig_t *telemetryConfig;
@ -237,7 +241,7 @@ void hottV4FormatAndSendEAMResponse(void)
static void hottV4Respond(uint8_t *data, uint8_t size)
{
serialSetMode(serialPorts.telemport, MODE_TX);
serialSetMode(hottPort, MODE_TX);
uint16_t crc = 0;
uint8_t i;
@ -254,31 +258,54 @@ static void hottV4Respond(uint8_t *data, uint8_t size)
delayMicroseconds(HOTTV4_TX_DELAY);
serialSetMode(serialPorts.telemport, MODE_RX);
serialSetMode(hottPort, MODE_RX);
}
static void hottV4SerialWrite(uint8_t c)
{
serialWrite(serialPorts.telemport, c);
serialWrite(hottPort, c);
}
void configureHoTTTelemetryPort(void)
{
// TODO set speed here to 19200?
serialSetMode(serialPorts.telemport, MODE_RX);
}
static portMode_t previousPortMode;
static uint32_t previousBaudRate;
void freeHoTTTelemetryPort(void)
{
serialSetMode(serialPorts.telemport, MODE_RXTX);
// FIXME only need to do this if the port is shared
serialSetMode(hottPort, previousPortMode);
serialSetBaudRate(hottPort, previousBaudRate);
endSerialPortFunction(hottPort, FUNCTION_TELEMETRY);
}
void configureHoTTTelemetryPort(telemetryConfig_t *telemetryConfig)
{
hottPort = findOpenSerialPort(FUNCTION_TELEMETRY);
if (hottPort) {
previousPortMode = hottPort->mode;
previousBaudRate = hottPort->baudRate;
//waitForSerialPortToFinishTransmitting(hottPort); // FIXME locks up the system
serialSetBaudRate(hottPort, HOTT_BAUDRATE);
serialSetMode(hottPort, HOTT_INITIAL_PORT_MODE);
beginSerialPortFunction(hottPort, FUNCTION_TELEMETRY);
} else {
hottPort = openSerialPort(FUNCTION_TELEMETRY, NULL, HOTT_BAUDRATE, HOTT_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
// FIXME only need to do this if the port is shared
previousPortMode = hottPort->mode;
previousBaudRate = hottPort->baudRate;
}
}
void handleHoTTTelemetry(void)
{
uint8_t c;
while (serialTotalBytesWaiting(serialPorts.telemport) > 0) {
c = serialRead(serialPorts.telemport);
while (serialTotalBytesWaiting(hottPort) > 0) {
c = serialRead(hottPort);
// Protocol specific waiting time to avoid collisions
delay(5);

2
src/telemetry_hott.h
View file

@ -203,7 +203,7 @@ typedef struct HoTTV4ElectricAirModule_t {
void handleHoTTTelemetry(void);
void checkTelemetryState(void);
void configureHoTTTelemetryPort(void);
void configureHoTTTelemetryPort(telemetryConfig_t *telemetryConfig);
void freeHoTTTelemetryPort(void);
#endif /* TELEMETRY_HOTT_H_ */