From bf90246c95baf3b68aa91d06b7afd527e990c2cf Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 10 May 2014 16:12:26 +0100 Subject: [PATCH] Add callback support to software serial. Also fixed problem with shared serial port detection. --- docs/Configuration.md | 6 ++- src/drivers/serial_softserial.c | 86 +++++++++++---------------------- src/drivers/serial_softserial.h | 8 ++- src/serial_common.c | 20 ++++++-- src/serial_common.h | 2 +- src/telemetry_common.c | 4 +- 6 files changed, 56 insertions(+), 70 deletions(-) diff --git a/docs/Configuration.md b/docs/Configuration.md index b2f1a2158..048e067a6 100644 --- a/docs/Configuration.md +++ b/docs/Configuration.md @@ -44,6 +44,7 @@ b) SERIAL_RX and TELEMETRY (when armed) - SERIAL_RX on UART2 ``` +feature -PARALLEL_PWM feature TELEMETRY feature SERIAL_RX set serial_port_2_scenario = 3 @@ -56,6 +57,8 @@ c) GPS and TELEMETRY via softserial - GPS on UART2 ``` +feature -PARALLEL_PWM +feature PPM feature TELEMETRY feature GPS feature SOFTSERIAL @@ -67,10 +70,9 @@ d) SERIAL_RX, GPS and TELEMETRY (when armed) MSP/CLI via softserial - GPS on UART1 - SERIAL RX on UART2 - TELEMETRY,MSP,CLI,GPS PASSTHROUGH on SOFTSERIAL1 -Note: PPM needed to disable parallel PWM which in turn allows SOFTSERIAL to be used. ``` -feature PPM +feature -PARALLEL_PWM feature TELEMETRY feature GPS feature SERIALRX diff --git a/src/drivers/serial_softserial.c b/src/drivers/serial_softserial.c index 765ff2baa..dca435982 100644 --- a/src/drivers/serial_softserial.c +++ b/src/drivers/serial_softserial.c @@ -48,30 +48,6 @@ void setTxSignal(softSerial_t *softSerial, uint8_t state) } } -softSerial_t* lookupSoftSerial(uint8_t reference) -{ - assert_param(reference >= 0 && reference <= MAX_SOFTSERIAL_PORTS); - - return &(softSerialPorts[reference]); -} - -void resetSerialTimer(softSerial_t *softSerial) -{ - //uint16_t counter = TIM_GetCounter(softSerial->rxTimerHardware->tim); - TIM_SetCounter(softSerial->rxTimerHardware->tim, 0); - //counter = TIM_GetCounter(softSerial->rxTimerHardware->tim); -} - -void stopSerialTimer(softSerial_t *softSerial) -{ - TIM_Cmd(softSerial->rxTimerHardware->tim, DISABLE); -} - -void startSerialTimer(softSerial_t *softSerial) -{ - TIM_Cmd(softSerial->rxTimerHardware->tim, ENABLE); -} - static void softSerialGPIOConfig(GPIO_TypeDef *gpio, uint16_t pin, GPIO_Mode mode) { gpio_config_t cfg; @@ -87,12 +63,12 @@ void serialInputPortConfig(const timerHardware_t *timerHardwarePtr) softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU); } -bool isTimerPeriodTooLarge(uint32_t timerPeriod) +static bool isTimerPeriodTooLarge(uint32_t timerPeriod) { return timerPeriod > 0xFFFF; } -void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference, uint32_t baud) +static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference, uint32_t baud) { uint32_t clock = SystemCoreClock; uint32_t timerPeriod; @@ -113,7 +89,7 @@ void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t refere configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, onSerialTimer); } -void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) +static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) { TIM_ICInitTypeDef TIM_ICInitStructure; @@ -127,19 +103,19 @@ 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, serialInversion_e inversion) +static void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference, serialInversion_e inversion) { // start bit is usually a FALLING signal serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, inversion == SERIAL_INVERTED ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, onSerialRxPinChange); } -void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr) +static void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr) { softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_Out_PP); } -void resetBuffers(softSerial_t *softSerial) +static void resetBuffers(softSerial_t *softSerial) { softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE; softSerial->port.rxBuffer = softSerial->rxBuffer; @@ -152,12 +128,25 @@ void resetBuffers(softSerial_t *softSerial) softSerial->port.txBufferHead = 0; } -void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t baud, serialInversion_e inversion) +serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint32_t baud, serialInversion_e inversion) { + softSerial_t *softSerial = &(softSerialPorts[portIndex]); + + if (portIndex == SOFTSERIAL1) { + softSerial->rxTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_RX_HARDWARE]); + softSerial->txTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_TX_HARDWARE]); + } + + if (portIndex == SOFTSERIAL2) { + softSerial->rxTimerHardware = &(timerHardware[SOFT_SERIAL_2_TIMER_RX_HARDWARE]); + softSerial->txTimerHardware = &(timerHardware[SOFT_SERIAL_2_TIMER_TX_HARDWARE]); + } + softSerial->port.vTable = softSerialVTable; softSerial->port.baudRate = baud; softSerial->port.mode = MODE_RXTX; softSerial->port.inversion = inversion; + softSerial->port.callback = callback; resetBuffers(softSerial); @@ -179,30 +168,6 @@ void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t serialTimerTxConfig(softSerial->txTimerHardware, portIndex, baud); serialTimerRxConfig(softSerial->rxTimerHardware, portIndex, inversion); -} - -serialPort_t *openSoftSerial1(uint32_t baud, serialInversion_e inversion) -{ - uint8_t portIndex = 0; - softSerial_t *softSerial = &(softSerialPorts[portIndex]); - - softSerial->rxTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_RX_HARDWARE]); - softSerial->txTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_TX_HARDWARE]); - - initialiseSoftSerial(softSerial, portIndex, baud, inversion); - - return &softSerial->port; -} - -serialPort_t * openSoftSerial2(uint32_t baud, serialInversion_e inversion) -{ - int portIndex = 1; - softSerial_t *softSerial = &(softSerialPorts[portIndex]); - - softSerial->rxTimerHardware = &(timerHardware[SOFT_SERIAL_2_TIMER_RX_HARDWARE]); - softSerial->txTimerHardware = &(timerHardware[SOFT_SERIAL_2_TIMER_TX_HARDWARE]); - - initialiseSoftSerial(softSerial, portIndex, baud, inversion); return &softSerial->port; } @@ -305,8 +270,13 @@ void extractAndStoreRxByte(softSerial_t *softSerial) } uint8_t rxByte = (softSerial->internalRxBuffer >> 1) & 0xFF; - softSerial->port.rxBuffer[softSerial->port.rxBufferTail] = rxByte; - updateBufferIndex(softSerial); + + if (softSerial->port.callback) { + softSerial->port.callback(rxByte); + } else { + softSerial->port.rxBuffer[softSerial->port.rxBufferTail] = rxByte; + updateBufferIndex(softSerial); + } } void processRxState(softSerial_t *softSerial) @@ -443,7 +413,7 @@ void softSerialWriteByte(serialPort_t *s, uint8_t ch) void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate) { softSerial_t *softSerial = (softSerial_t *)s; - initialiseSoftSerial(softSerial, softSerial->softSerialPortIndex, baudRate, softSerial->port.inversion); + openSoftSerial(softSerial->softSerialPortIndex, s->callback, baudRate, softSerial->port.inversion); } void softSerialSetMode(serialPort_t *instance, portMode_t mode) diff --git a/src/drivers/serial_softserial.h b/src/drivers/serial_softserial.h index c211a01a8..82c13ac5c 100644 --- a/src/drivers/serial_softserial.h +++ b/src/drivers/serial_softserial.h @@ -9,6 +9,11 @@ #define SOFT_SERIAL_BUFFER_SIZE 256 +typedef enum { + SOFTSERIAL1 = 0, + SOFTSERIAL2 +} softSerialPortIndex_e; + typedef struct softSerial_s { serialPort_t port; @@ -40,8 +45,7 @@ extern softSerial_t softSerialPorts[]; extern const struct serialPortVTable softSerialVTable[]; -serialPort_t *openSoftSerial1(uint32_t baud, uint8_t inverted); -serialPort_t *openSoftSerial2(uint32_t baud, uint8_t inverted); +serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint32_t baud, serialInversion_e inversion); // serialPort API void softSerialWriteByte(serialPort_t *instance, uint8_t ch); diff --git a/src/serial_common.c b/src/serial_common.c index c3062ba75..f18811d7d 100644 --- a/src/serial_common.c +++ b/src/serial_common.c @@ -49,8 +49,8 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { 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} + {SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}, + {SERIAL_PORT_SOFTSERIAL2, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE} }; typedef struct functionConstraint_s { @@ -200,7 +200,7 @@ bool canOpenSerialPort(uint16_t functionMask) return result != NULL; } -serialPortFunction_t *findSerialPortFunction(uint16_t functionMask) +static serialPortFunction_t *findSerialPortFunction(uint16_t functionMask) { serialPortIndex_e portIndex; @@ -223,6 +223,16 @@ serialPortFunction_t *findSerialPortFunction(uint16_t functionMask) return NULL; } +bool isSerialPortFunctionShared(serialPortFunction_e functionToUse, uint16_t functionMask) +{ + serialPortSearchResult_t *result = findSerialPort(functionToUse); + if (!result) { + return false; + } + + return result->portFunction->scenario & functionMask; +} + /* * find a serial port that is: * a) open @@ -260,11 +270,11 @@ serialPort_t *openSerialPort(serialPortFunction_e function, serialReceiveCallbac serialPort = uartOpen(USART2, callback, baudRate, mode, inversion); break; case SERIAL_PORT_SOFTSERIAL1: - serialPort = openSoftSerial1(baudRate, inversion); + serialPort = openSoftSerial(SOFTSERIAL1, callback, baudRate, inversion); serialSetMode(serialPort, mode); break; case SERIAL_PORT_SOFTSERIAL2: - serialPort = openSoftSerial2(baudRate, inversion); + serialPort = openSoftSerial(SOFTSERIAL2, callback, baudRate, inversion); serialSetMode(serialPort, mode); break; } diff --git a/src/serial_common.h b/src/serial_common.h index 7f42f99e0..3341130ae 100644 --- a/src/serial_common.h +++ b/src/serial_common.h @@ -88,13 +88,13 @@ serialPort_t *openSerialPort(serialPortFunction_e functionMask, serialReceiveCal 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 applySerialConfigToPortFunctions(serialConfig_t *serialConfig); bool isSerialConfigValid(serialConfig_t *serialConfig); bool doesConfigurationUsePort(serialConfig_t *serialConfig, serialPortIdentifier_e portIdentifier); +bool isSerialPortFunctionShared(serialPortFunction_e functionToUse, uint16_t functionMask); void evaluateOtherData(uint8_t sr); void handleSerial(void); diff --git a/src/telemetry_common.c b/src/telemetry_common.c index 2bed7b15e..d0a6f4389 100644 --- a/src/telemetry_common.c +++ b/src/telemetry_common.c @@ -65,8 +65,8 @@ bool determineNewTelemetryEnabledState(void) bool telemetryPortIsShared; bool enabled = true; - serialPortFunction_t *function = findSerialPortFunction(FUNCTION_TELEMETRY); - telemetryPortIsShared = function && function->scenario != SCENARIO_TELEMETRY_ONLY; + + telemetryPortIsShared = isSerialPortFunctionShared(FUNCTION_TELEMETRY, FUNCTION_MSP); if (telemetryPortIsShared) { if (telemetryConfig->telemetry_switch)