From 77a241bd5f3de7d8bc247291bb63b36b03d86935 Mon Sep 17 00:00:00 2001 From: "timecop@gmail.com" Date: Wed, 28 Aug 2013 01:26:10 +0000 Subject: [PATCH] Software serial implementation. Compile with SOFTSERIAL_19200_LOOPBACK to test. Without the define the implementation will have no effect. Next step is to add a 'feature' to enable softserial and settings for the baud rate. Note, only READ is currently supported, write will come later. The highlevel api calls are used in main.c. Uart implementation needs cleanup to make serial port code generic, see uart files for details. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@390 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61 --- Makefile | 1 + baseflight.uvproj | 15 ++++ src/board.h | 6 +- src/drv_pwm.c | 124 +++++++++++++------------- src/drv_pwm.h | 14 +-- src/drv_softserial.c | 203 +++++++++++++++++++++++++++++++++++++++++++ src/drv_softserial.h | 25 ++++++ src/drv_timer.c | 121 ++++++++++++++++---------- src/drv_timer.h | 2 + src/drv_uart.c | 20 +++-- src/drv_uart.h | 61 +++++++------ src/main.c | 20 ++++- src/sensors.c | 16 ++-- 13 files changed, 466 insertions(+), 162 deletions(-) create mode 100644 src/drv_softserial.c create mode 100644 src/drv_softserial.h diff --git a/Makefile b/Makefile index cb26d8e7c1..a90b00f4d4 100644 --- a/Makefile +++ b/Makefile @@ -94,6 +94,7 @@ OLIMEXINO_SRC = drv_spi.c \ drv_l3g4200d.c \ drv_pwm.c \ drv_timer.c \ + drv_softserial.c \ $(COMMON_SRC) # Search path for baseflight sources diff --git a/baseflight.uvproj b/baseflight.uvproj index 8663723525..a1b03bd84c 100755 --- a/baseflight.uvproj +++ b/baseflight.uvproj @@ -652,6 +652,11 @@ 1 .\src\drv_timer.c + + drv_softserial.c + 1 + .\src\drv_softserial.c + @@ -1478,6 +1483,11 @@ 1 .\src\drv_timer.c + + drv_softserial.c + 1 + .\src\drv_softserial.c + @@ -2488,6 +2498,11 @@ 1 .\src\drv_timer.c + + drv_softserial.c + 1 + .\src\drv_softserial.c + diff --git a/src/board.h b/src/board.h index b46ae3c2e5..dc4ca80db2 100755 --- a/src/board.h +++ b/src/board.h @@ -199,9 +199,10 @@ typedef struct baro_t #include "drv_pwm.h" #include "drv_timer.h" #include "drv_uart.h" +#include "drv_softserial.h" #else - // AfroFlight32 + // AfroFlight32 #include "drv_adc.h" #include "drv_adxl345.h" #include "drv_bmp085.h" @@ -217,7 +218,8 @@ typedef struct baro_t #include "drv_pwm.h" #include "drv_timer.h" #include "drv_uart.h" +#include "drv_softserial.h" #include "drv_hcsr04.h" -#endif +#endif #endif diff --git a/src/drv_pwm.c b/src/drv_pwm.c index eeea201c1f..9c6438a5a2 100755 --- a/src/drv_pwm.c +++ b/src/drv_pwm.c @@ -1,4 +1,4 @@ -#include "board.h" +#include "board.h" #define PULSE_1MS (1000) // 1ms pulse width @@ -6,9 +6,9 @@ Configuration maps: 1) multirotor PPM input - PWM1 used for PPM - PWM5..8 used for motors - PWM9..10 used for servo or else motors + PWM1 used for PPM + PWM5..8 used for motors + PWM9..10 used for servo or else motors PWM11..14 used for motors 2) multirotor PPM input with more servos @@ -28,9 +28,9 @@ PWM11.14 used for servos 4) airplane / flying wing with PPM - PWM1 used for PPM - PWM5..8 used for servos - PWM9 used for motor throttle +PWM10 for 2nd motor + PWM1 used for PPM + PWM5..8 used for servos + PWM9 used for motor throttle +PWM10 for 2nd motor PWM11.14 used for servos */ @@ -38,9 +38,9 @@ typedef struct { volatile uint16_t *ccr; uint16_t period; - // for input only - uint8_t channel; - uint8_t state; + // for input only + uint8_t channel; + uint8_t state; uint16_t rise; uint16_t fall; uint16_t capture; @@ -134,27 +134,17 @@ static const uint8_t * const hardwareMaps[] = { multiPWM, multiPPM, airPWM, - airPPM, -}; - -static void pwmTimeBase(TIM_TypeDef *tim, uint32_t period) -{ - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = period - 1; - TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / 1000000) - 1; // all timers run at 1MHz - TIM_TimeBaseStructure.TIM_ClockDivision = 0; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure); -} + airPPM, +}; + +#define PWM_TIMER_MHZ 1 static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value) { TIM_OCInitTypeDef TIM_OCInitStructure; - - TIM_OCStructInit(&TIM_OCInitStructure); - TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; + + TIM_OCStructInit(&TIM_OCInitStructure); + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; TIM_OCInitStructure.TIM_Pulse = value; @@ -178,13 +168,13 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value) TIM_OC4Init(tim, &TIM_OCInitStructure); TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); break; - } -} - -static void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) -{ - TIM_ICInitTypeDef TIM_ICInitStructure; - + } +} + +void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) +{ + TIM_ICInitTypeDef TIM_ICInitStructure; + TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_Channel = channel; TIM_ICInitStructure.TIM_ICPolarity = polarity; @@ -205,13 +195,13 @@ static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode) gpioInit(gpio, &cfg); } -static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value) -{ - pwmPortData_t *p = &pwmPorts[port]; - pwmTimeBase(timerHardware[port].tim, period); - pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP); - pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value); - // Needed only on TIM1 +static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value) +{ + pwmPortData_t *p = &pwmPorts[port]; + configTimeBase(timerHardware[port].tim, period, PWM_TIMER_MHZ); + pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP); + pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value); + // Needed only on TIM1 if (timerHardware[port].outputEnable) TIM_CtrlPWMOutputs(timerHardware[port].tim, ENABLE); TIM_Cmd(timerHardware[port].tim, ENABLE); @@ -227,32 +217,32 @@ static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value p->ccr = &timerHardware[port].tim->CCR3; break; case TIM_Channel_4: - p->ccr = &timerHardware[port].tim->CCR4; - break; - } + p->ccr = &timerHardware[port].tim->CCR4; + break; + } return p; } static pwmPortData_t *pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uint8_t channel) { pwmPortData_t *p = &pwmPorts[port]; - pwmTimeBase(timerHardware[port].tim, 0xFFFF); - pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_IPD); - pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Rising); - TIM_Cmd(timerHardware[port].tim, ENABLE); - timerNVICConfig(timerHardware[port].irq); + const timerHardware_t *timerHardwarePtr = &(timerHardware[port]); p->channel = channel; - configureTimerCaptureCompareInterrupt(&(timerHardware[port]), port, callback); + pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPD); + pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); + + timerInConfig(timerHardwarePtr, 0xFFFF, PWM_TIMER_MHZ); + configureTimerCaptureCompareInterrupt(timerHardwarePtr, port, callback); return p; } static void ppmCallback(uint8_t port, uint16_t capture) -{ - uint16_t diff; - static uint16_t now; +{ + uint16_t diff; + static uint16_t now; static uint16_t last = 0; static uint8_t chan = 0; static uint8_t GoodPulses; @@ -317,9 +307,9 @@ bool pwmInit(drv_pwm_config_t *init) setup = hardwareMaps[i]; for (i = 0; i < MAX_PORTS; i++) { - uint8_t port = setup[i] & 0x0F; - uint8_t mask = setup[i] & 0xF0; - + uint8_t port = setup[i] & 0x0F; + uint8_t mask = setup[i] & 0xF0; + if (setup[i] == 0xFF) // terminator break; @@ -328,14 +318,20 @@ bool pwmInit(drv_pwm_config_t *init) if (port == PWM2) continue; #endif - - // skip UART ports for GPS - if (init->useUART && (port == PWM3 || port == PWM4)) - continue; - - // skip ADC for powerMeter if configured - if (init->adcChannel && (init->adcChannel == port)) - continue; + + // skip UART ports for GPS + if (init->useUART && (port == PWM3 || port == PWM4)) + continue; + +#ifdef SOFTSERIAL_19200_LOOPBACK + // skip softSerial ports + if ((port == PWM5 || port == PWM6)) + continue; +#endif + + // skip ADC for powerMeter if configured + if (init->adcChannel && (init->adcChannel == port)) + continue; // hacks to allow current functionality if (mask & (TYPE_IP | TYPE_IW) && !init->enableInput) diff --git a/src/drv_pwm.h b/src/drv_pwm.h index c5d5e289ed..416a2c93ba 100755 --- a/src/drv_pwm.h +++ b/src/drv_pwm.h @@ -30,15 +30,17 @@ enum { PWM9, PWM10, PWM11, - PWM12, - PWM13, - PWM14, + PWM12, + PWM13, + PWM14, MAX_PORTS }; +void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); + bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not void pwmWriteMotor(uint8_t index, uint16_t value); void pwmWriteServo(uint8_t index, uint16_t value); -uint16_t pwmRead(uint8_t channel); - -// void pwmWrite(uint8_t channel, uint16_t value); +uint16_t pwmRead(uint8_t channel); + +// void pwmWrite(uint8_t channel, uint16_t value); diff --git a/src/drv_softserial.c b/src/drv_softserial.c new file mode 100644 index 0000000000..9f45664597 --- /dev/null +++ b/src/drv_softserial.c @@ -0,0 +1,203 @@ +#include "board.h" + +enum serialBitStatus { + WAITING_FOR_START_BIT = -1, BIT_0, BIT_1, BIT_2, BIT_3, BIT_4, BIT_5, BIT_6, BIT_7, WAITING_FOR_STOP_BIT +}; + +#define MAX_SOFTSERIAL_PORTS 2 +softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS]; + +softSerial_t* lookupSoftSerial(uint8_t reference) +{ + assert_param(reference >= 0 && reference <= MAX_SOFTSERIAL_PORTS); + + return &(softSerialPorts[reference]); +} + + +void stopSerialTimer(softSerial_t *softSerial) +{ + TIM_Cmd(softSerial->timerHardware->tim, DISABLE); + TIM_SetCounter(softSerial->timerHardware->tim, 0); +} + +void startSerialTimer(softSerial_t *softSerial) +{ + TIM_SetCounter(softSerial->timerHardware->tim, 0); + TIM_Cmd(softSerial->timerHardware->tim, ENABLE); +} + +static void waitForFirstBit(softSerial_t *softSerial) +{ + softSerial->state = BIT_0; + startSerialTimer(softSerial); +} + +static void onSerialPinChange(uint8_t reference, uint16_t capture) +{ + softSerial_t *softSerial = lookupSoftSerial(reference); + if (softSerial->state == WAITING_FOR_START_BIT) { + waitForFirstBit(softSerial); + } +} + +uint8_t readSerialSignal(softSerial_t *softSerial) +{ + return GPIO_ReadInputDataBit(softSerial->timerHardware->gpio, softSerial->timerHardware->pin); +} + +void mergeSignalWithCurrentByte(softSerial_t *softSerial, uint8_t serialSignal) +{ + softSerial->rxBuffer[softSerial->port.rxBufferTail] |= (serialSignal << softSerial->state); +} + +inline void initialiseCurrentByteWithFirstSignal(softSerial_t *softSerial, uint8_t serialSignal) +{ + softSerial->rxBuffer[softSerial->port.rxBufferTail] = serialSignal; +} + +inline void prepareForNextSignal(softSerial_t *softSerial) { + softSerial->state++; +} + +void updateBufferIndex(softSerial_t *softSerial) +{ + if (softSerial->port.rxBufferTail >= softSerial->port.rxBufferSize - 1) + { + softSerial->port.rxBufferTail = 0; //cycling the buffer + } else { + softSerial->port.rxBufferTail++; + } +} + +void prepareForNextByte(softSerial_t *softSerial) +{ + stopSerialTimer(softSerial); + softSerial->state = WAITING_FOR_START_BIT; + updateBufferIndex(softSerial); +} + +void onSerialTimer(uint8_t portIndex, uint16_t capture) +{ + softSerial_t *softSerial = &(softSerialPorts[portIndex]); + + uint8_t serialSignal = readSerialSignal(softSerial); + + switch (softSerial->state) { + case BIT_0: + initialiseCurrentByteWithFirstSignal(softSerial, serialSignal); + prepareForNextSignal(softSerial); + break; + + case BIT_1: + case BIT_2: + case BIT_3: + case BIT_4: + case BIT_5: + case BIT_6: + case BIT_7: + mergeSignalWithCurrentByte(softSerial, serialSignal); + + prepareForNextSignal(softSerial); + break; + + case WAITING_FOR_STOP_BIT: + prepareForNextByte(softSerial); + break; + } +} + +#define SOFT_SERIAL_TIMER_MHZ 1 +#define SOFT_SERIAL_1_TIMER_HARDWARE 4 + +static void softSerialGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode) +{ + gpio_config_t cfg; + + cfg.pin = pin; + cfg.mode = mode; + cfg.speed = Speed_2MHz; + gpioInit(gpio, &cfg); +} + +void serialInputPortConfig(const timerHardware_t *timerHardwarePtr, uint16_t baud, uint8_t reference, timerCCCallbackPtr callback) +{ + softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU); + + pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); + + int oneBitPeriod = (SOFT_SERIAL_TIMER_MHZ * 1000000) / baud; + + timerInConfig(timerHardwarePtr, oneBitPeriod, SOFT_SERIAL_TIMER_MHZ); + configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, callback); +} + +void setupSoftSerial1(uint32_t baud) +{ + int portIndex = 0; + softSerial_t *softSerial = &(softSerialPorts[portIndex]); + + softSerial->timerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_HARDWARE]); + softSerial->timerIndex = SOFT_SERIAL_1_TIMER_HARDWARE; + softSerial->state = WAITING_FOR_START_BIT; + + softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE; + softSerial->port.rxBuffer = softSerial->rxBuffer; + softSerial->port.rxBufferTail = 0; + softSerial->port.rxBufferHead = 0; + + softSerial->port.txBuffer = 0; + softSerial->port.txBufferSize = 0; + softSerial->port.txBufferTail = 0; + softSerial->port.txBufferHead = 0; + + softSerial->port.baudRate = baud; + softSerial->port.mode = MODE_RX; + + // FIXME this uart specific initialisation doesn't belong really here + softSerial->port.txDMAChannel = 0; + softSerial->port.txDMAChannel = 0; + + configureTimerChannelCallback(softSerial->timerHardware->tim, TIM_Channel_2, portIndex, onSerialTimer); + + TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE); + stopSerialTimer(softSerial); + + serialInputPortConfig(softSerial->timerHardware, baud, portIndex, onSerialPinChange); +} + +bool serialAvailable(softSerial_t *softSerial) +{ + if (softSerial->port.rxBufferTail == softSerial->port.rxBufferHead) { + return 0; + } + + int availableBytes; + if (softSerial->port.rxBufferTail > softSerial->port.rxBufferHead) { + availableBytes = softSerial->port.rxBufferTail - softSerial->port.rxBufferHead; + } else { + availableBytes = softSerial->port.rxBufferTail + softSerial->port.rxBufferSize - softSerial->port.rxBufferHead; + } + return availableBytes; +} + +static void moveHeadToNextByte(softSerial_t *softSerial) +{ + if (softSerial->port.rxBufferHead < softSerial->port.rxBufferSize - 1) { + softSerial->port.rxBufferHead++; + } else { + softSerial->port.rxBufferHead = 0; + } +} + +uint8_t serialReadByte(softSerial_t *softSerial) +{ + if (serialAvailable(softSerial) == 0) { + return 0; + } + + char b = softSerial->port.rxBuffer[softSerial->port.rxBufferHead]; + + moveHeadToNextByte(softSerial); + return b; +} diff --git a/src/drv_softserial.h b/src/drv_softserial.h new file mode 100644 index 0000000000..a99d1026e6 --- /dev/null +++ b/src/drv_softserial.h @@ -0,0 +1,25 @@ +/* + * drv_softserial.h + * + * Created on: 23 Aug 2013 + * Author: Hydra + */ + +#pragma once + +#define SOFT_SERIAL_BUFFER_SIZE 256 + +typedef struct softSerial_s { + const timerHardware_t *timerHardware; + uint8_t timerIndex; + serialPort_t port; + volatile int state; + volatile uint8_t rxBuffer[SOFT_SERIAL_BUFFER_SIZE]; +} softSerial_t; +void setupSoftSerial1(uint32_t baud); + +uint8_t serialReadByte(softSerial_t *softSerial); +bool serialAvailable(softSerial_t *softSerial); + +extern timerHardware_t* serialTimerHardware; +extern softSerial_t softSerialPorts[]; diff --git a/src/drv_timer.c b/src/drv_timer.c index e778e2c658..fe25f9ce59 100644 --- a/src/drv_timer.c +++ b/src/drv_timer.c @@ -71,9 +71,9 @@ static const uint8_t channels[CC_CHANNELS_PER_TIMER] = { }; typedef struct timerConfig_s { - timerCCCallbackPtr *callback; - uint8_t channel; TIM_TypeDef *tim; + uint8_t channel; + timerCCCallbackPtr *callback; uint8_t reference; } timerConfig_t; @@ -97,7 +97,77 @@ static uint8_t lookupChannelIndex(const uint8_t channel) return channelIndex; } -static timerConfig_t *findTimerConfig(TIM_TypeDef *tim, uint8_t channel) +void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback) +{ + assert_param(IS_TIM_CHANNEL(channel)); + + uint8_t timerConfigIndex = (lookupTimerIndex(tim) * MAX_TIMERS) + lookupChannelIndex(channel); + + if (timerConfigIndex >= MAX_TIMERS * CC_CHANNELS_PER_TIMER) { + return; + } + + timerConfig[timerConfigIndex].callback = callback; + timerConfig[timerConfigIndex].channel = channel; + timerConfig[timerConfigIndex].reference = reference; +} + +void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel) +{ + switch (channel) { + case TIM_Channel_1: + TIM_ITConfig(tim, TIM_IT_CC1, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(tim, TIM_IT_CC2, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(tim, TIM_IT_CC3, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(tim, TIM_IT_CC4, ENABLE); + break; + } +} + +void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback) +{ + configureTimerChannelCallback(timerHardwarePtr->tim, timerHardwarePtr->channel, reference, callback); + configureTimerInputCaptureCompareChannel(timerHardwarePtr->tim, timerHardwarePtr->channel); +} + +void timerNVICConfig(uint8_t irq) +{ + NVIC_InitTypeDef NVIC_InitStructure; + + NVIC_InitStructure.NVIC_IRQChannel = irq; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); +} + +void configTimeBase(TIM_TypeDef *tim, uint32_t period, uint8_t mhz) +{ + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Period = period - 1; + TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure); +} + +void timerInConfig(const timerHardware_t *timerHardwarePtr, uint32_t period, uint8_t mhz) +{ + configTimeBase(timerHardwarePtr->tim, period, mhz); + TIM_Cmd(timerHardwarePtr->tim, ENABLE); + timerNVICConfig(timerHardwarePtr->irq); +} + + +timerConfig_t *findTimerConfig(TIM_TypeDef *tim, uint8_t channel) { uint8_t timerConfigIndex = (lookupTimerIndex(tim) * MAX_TIMERS) + lookupChannelIndex(channel); return &(timerConfig[timerConfigIndex]); @@ -128,6 +198,8 @@ static void timCCxHandler(TIM_TypeDef *tim) timerConfig = findTimerConfig(tim, TIM_Channel_4); capture = TIM_GetCapture4(tim); + } else { + return; // avoid uninitialised variable dereference } if (!timerConfig->callback) { @@ -155,46 +227,3 @@ void TIM4_IRQHandler(void) { timCCxHandler(TIM4); } - -void timerNVICConfig(uint8_t irq) -{ - NVIC_InitTypeDef NVIC_InitStructure; - - NVIC_InitStructure.NVIC_IRQChannel = irq; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); -} - -void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel) -{ - switch (channel) { - case TIM_Channel_1: - TIM_ITConfig(tim, TIM_IT_CC1, ENABLE); - break; - case TIM_Channel_2: - TIM_ITConfig(tim, TIM_IT_CC2, ENABLE); - break; - case TIM_Channel_3: - TIM_ITConfig(tim, TIM_IT_CC3, ENABLE); - break; - case TIM_Channel_4: - TIM_ITConfig(tim, TIM_IT_CC4, ENABLE); - break; - } -} - -void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback) -{ - uint8_t timerConfigIndex = (lookupTimerIndex(tim) * MAX_TIMERS) + lookupChannelIndex(channel); - timerConfig[timerConfigIndex].callback = callback; - timerConfig[timerConfigIndex].channel = channel; - timerConfig[timerConfigIndex].reference = reference; -} - -void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback) -{ - configureTimerChannelCallback(timerHardwarePtr->tim, timerHardwarePtr->channel, reference, callback); - configureTimerInputCaptureCompareChannel(timerHardwarePtr->tim, timerHardwarePtr->channel); -} diff --git a/src/drv_timer.h b/src/drv_timer.h index 4712206461..79edc5beff 100644 --- a/src/drv_timer.h +++ b/src/drv_timer.h @@ -13,6 +13,8 @@ typedef struct { extern const timerHardware_t timerHardware[]; +void configTimeBase(TIM_TypeDef *tim, uint32_t period, uint8_t mhz); +void timerInConfig(const timerHardware_t *timerHardwarePtr, uint32_t period, uint8_t mhz); void timerNVICConfig(uint8_t irq); void configureTimerInputCaptureCompareChannel(TIM_TypeDef *tim, const uint8_t channel); diff --git a/src/drv_uart.c b/src/drv_uart.c index 702e05588a..8515cdf071 100755 --- a/src/drv_uart.c +++ b/src/drv_uart.c @@ -238,12 +238,20 @@ void uartWrite(serialPort_t *s, uint8_t ch) uartStartTxDMA(s); } else { USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE); - } -} - -// Handlers - -// USART1 Tx DMA Handler + } +} + +void uartPrint(serialPort_t *s, const char *str) +{ + uint8_t ch; + while ((ch = *(str++))) { + uartWrite(s, ch); + } +} + +// Handlers + +// USART1 Tx DMA Handler void DMA1_Channel4_IRQHandler(void) { serialPort_t *s = &serialPort1; diff --git a/src/drv_uart.h b/src/drv_uart.h index df6369a116..c6798aa1d8 100755 --- a/src/drv_uart.h +++ b/src/drv_uart.h @@ -12,33 +12,40 @@ typedef enum portmode_t { MODE_RX = 1, MODE_TX = 2, - MODE_RXTX = 3 -} portmode_t; - -typedef struct { - uint32_t baudRate; - uint32_t rxBufferSize; - uint32_t txBufferSize; - volatile uint8_t *rxBuffer; - volatile uint8_t *txBuffer; - uint32_t rxDMAPos; - uint32_t rxBufferHead; - uint32_t rxBufferTail; - uint32_t txBufferHead; - uint32_t txBufferTail; - - DMA_Channel_TypeDef *rxDMAChannel; - DMA_Channel_TypeDef *txDMAChannel; - uint32_t rxDMAIrq; - uint32_t txDMAIrq; - bool txDMAEmpty; - USART_TypeDef *USARTx; - - uartReceiveCallbackPtr callback; - portmode_t mode; -} serialPort_t; - -serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode); + MODE_RXTX = 3 +} portmode_t; + +// FIXME this is a uart_t really. Move the generic properties into a separate structure (serialPort_t) and update the code to use it +typedef struct { + portmode_t mode; + uint32_t baudRate; + + uint32_t rxBufferSize; + uint32_t txBufferSize; + volatile uint8_t *rxBuffer; + volatile uint8_t *txBuffer; + uint32_t rxBufferHead; + uint32_t rxBufferTail; + uint32_t txBufferHead; + uint32_t txBufferTail; + + // FIXME rename callback type so something more generic + uartReceiveCallbackPtr callback; + + // FIXME these are uart specific and do not belong in here + DMA_Channel_TypeDef *rxDMAChannel; + DMA_Channel_TypeDef *txDMAChannel; + + uint32_t rxDMAIrq; + uint32_t txDMAIrq; + + uint32_t rxDMAPos; + bool txDMAEmpty; + + USART_TypeDef *USARTx; +} serialPort_t; + +serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode); void uartChangeBaud(serialPort_t *s, uint32_t baudRate); bool isUartAvailable(serialPort_t *s); bool isUartTransmitEmpty(serialPort_t *s); diff --git a/src/main.c b/src/main.c index a2eb82fab2..af28f7ad00 100755 --- a/src/main.c +++ b/src/main.c @@ -120,10 +120,17 @@ int main(void) // Check battery type/voltage if (feature(FEATURE_VBAT)) - batteryInit(); + batteryInit(); + +#ifdef SOFTSERIAL_19200_LOOPBACK + + serialInit(19200); + setupSoftSerial1(19200); + uartPrint(core.mainport, "LOOPBACK 19200 ENABLED"); +#else + serialInit(mcfg.serial_baudrate); +#endif - serialInit(mcfg.serial_baudrate); - previousTime = micros(); if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) calibratingA = 400; @@ -134,6 +141,13 @@ int main(void) // loopy while (1) { loop(); +#ifdef SOFTSERIAL_19200_LOOPBACK + while (serialAvailable(&softSerialPorts[0])) { + + uint8_t b = serialReadByte(&softSerialPorts[0]); + uartWrite(core.mainport, b); + }; +#endif } } diff --git a/src/sensors.c b/src/sensors.c index be3439e689..8cead6bc45 100755 --- a/src/sensors.c +++ b/src/sensors.c @@ -64,14 +64,14 @@ retry: mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct accHardware = ACC_MPU6050; if (mcfg.acc_hardware == ACC_MPU6050) - break; - } - ; // fallthrough - case 3: // MMA8452 -#ifndef OLIMEXINO - if (mma8452Detect(&acc)) { - accHardware = ACC_MMA8452; - if (mcfg.acc_hardware == ACC_MMA8452) + break; + } + ; // fallthrough +#ifndef OLIMEXINO + case 3: // MMA8452 + if (mma8452Detect(&acc)) { + accHardware = ACC_MMA8452; + if (mcfg.acc_hardware == ACC_MMA8452) break; } #endif