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