diff --git a/Makefile b/Makefile
index 98339305c2..a90b00f4d4 100644
--- a/Makefile
+++ b/Makefile
@@ -77,6 +77,7 @@ NAZE_SRC = drv_spi.c \
drv_mpu6050.c \
drv_l3g4200d.c \
drv_pwm.c \
+ drv_timer.c \
$(COMMON_SRC)
# Source files for the FY90Q target
@@ -92,6 +93,8 @@ OLIMEXINO_SRC = drv_spi.c \
drv_mpu6050.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 2b1badd2a1..a1b03bd84c 100755
--- a/baseflight.uvproj
+++ b/baseflight.uvproj
@@ -647,6 +647,16 @@
1
.\src\drv_spi.c
+
+ drv_timer.c
+ 1
+ .\src\drv_timer.c
+
+
+ drv_softserial.c
+ 1
+ .\src\drv_softserial.c
+
@@ -1468,6 +1478,16 @@
1
.\src\drv_spi.c
+
+ drv_timer.c
+ 1
+ .\src\drv_timer.c
+
+
+ drv_softserial.c
+ 1
+ .\src\drv_softserial.c
+
@@ -2473,6 +2493,16 @@
1
.\src\drv_spi.c
+
+ drv_timer.c
+ 1
+ .\src\drv_timer.c
+
+
+ drv_softserial.c
+ 1
+ .\src\drv_softserial.c
+
diff --git a/src/board.h b/src/board.h
index 199f6b425c..620f0bae4a 100755
--- a/src/board.h
+++ b/src/board.h
@@ -110,9 +110,9 @@ typedef struct baro_t
#ifdef FY90Q
// FY90Q
#define LED0_GPIO GPIOC
-#define LED0_PIN GPIO_Pin_12
+#define LED0_PIN Pin_12
#define LED1_GPIO GPIOA
-#define LED1_PIN GPIO_Pin_15
+#define LED1_PIN Pin_15
#define GYRO
#define ACC
@@ -148,13 +148,13 @@ typedef struct baro_t
// Afroflight32
#define LED0_GPIO GPIOB
-#define LED0_PIN GPIO_Pin_3 // PB3 (LED)
+#define LED0_PIN Pin_3 // PB3 (LED)
#define LED1_GPIO GPIOB
-#define LED1_PIN GPIO_Pin_4 // PB4 (LED)
+#define LED1_PIN Pin_4 // PB4 (LED)
#define BEEP_GPIO GPIOA
-#define BEEP_PIN GPIO_Pin_12 // PA12 (Buzzer)
+#define BEEP_PIN Pin_12 // PA12 (Buzzer)
#define BARO_GPIO GPIOC
-#define BARO_PIN GPIO_Pin_13
+#define BARO_PIN Pin_13
#define GYRO
#define ACC
@@ -221,7 +221,9 @@ typedef struct baro_t
#include "drv_mpu6050.h"
#include "drv_l3g4200d.h"
#include "drv_pwm.h"
+#include "drv_timer.h"
#include "drv_uart.h"
+#include "drv_softserial.h"
#else
// AfroFlight32
@@ -238,7 +240,9 @@ typedef struct baro_t
#include "drv_mpu6050.h"
#include "drv_l3g4200d.h"
#include "drv_pwm.h"
+#include "drv_timer.h"
#include "drv_uart.h"
+#include "drv_softserial.h"
#include "drv_hcsr04.h"
#endif
diff --git a/src/cli.c b/src/cli.c
index c12371af94..4b689bf447 100644
--- a/src/cli.c
+++ b/src/cli.c
@@ -175,11 +175,15 @@ const clivalue_t valueTable[] = {
{ "gimbal_roll_max", VAR_UINT16, &cfg.gimbal_roll_max, 100, 3000 },
{ "gimbal_roll_mid", VAR_UINT16, &cfg.gimbal_roll_mid, 100, 3000 },
{ "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 },
+ { "accxy_deadband", VAR_UINT8, &cfg.accxy_deadband, 0, 100 },
+ { "accz_deadband", VAR_UINT8, &cfg.accz_deadband, 0, 100 },
+ { "acc_unarmedcal", VAR_UINT8, &cfg.acc_unarmedcal, 0, 1 },
{ "acc_trim_pitch", VAR_INT16, &cfg.angleTrim[PITCH], -300, 300 },
{ "acc_trim_roll", VAR_INT16, &cfg.angleTrim[ROLL], -300, 300 },
{ "baro_tab_size", VAR_UINT8, &cfg.baro_tab_size, 0, BARO_TAB_SIZE_MAX },
{ "baro_noise_lpf", VAR_FLOAT, &cfg.baro_noise_lpf, 0, 1 },
- { "baro_cf", VAR_FLOAT, &cfg.baro_cf, 0, 1 },
+ { "baro_cf_vel", VAR_FLOAT, &cfg.baro_cf_vel, 0, 1 },
+ { "baro_cf_alt", VAR_FLOAT, &cfg.baro_cf_alt, 0, 1 },
{ "mag_declination", VAR_INT16, &cfg.mag_declination, -18000, 18000 },
{ "gps_pos_p", VAR_UINT8, &cfg.P8[PIDPOS], 0, 200 },
{ "gps_pos_i", VAR_UINT8, &cfg.I8[PIDPOS], 0, 200 },
diff --git a/src/config.c b/src/config.c
index 9e88f6f301..ab29a21a15 100755
--- a/src/config.c
+++ b/src/config.c
@@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles
config_t cfg; // profile config struct
const char rcChannelLetters[] = "AERT1234";
-static const uint8_t EEPROM_CONF_VERSION = 48;
+static const uint8_t EEPROM_CONF_VERSION = 49;
static uint32_t enabledSensors = 0;
static void resetConf(void);
@@ -85,6 +85,7 @@ void readEEPROM(void)
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
setPIDController(cfg.pidController);
+ GPS_set_pids();
}
void writeEEPROM(uint8_t b, uint8_t updateProfile)
@@ -217,9 +218,9 @@ static void resetConf(void)
cfg.P8[YAW] = 85;
cfg.I8[YAW] = 45;
cfg.D8[YAW] = 0;
- cfg.P8[PIDALT] = 64;
+ cfg.P8[PIDALT] = 50;
cfg.I8[PIDALT] = 25;
- cfg.D8[PIDALT] = 24;
+ cfg.D8[PIDALT] = 80;
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
cfg.D8[PIDPOS] = 0;
@@ -249,10 +250,13 @@ static void resetConf(void)
cfg.angleTrim[1] = 0;
cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
cfg.acc_lpf_factor = 4;
- cfg.accz_deadband = 50;
+ cfg.accz_deadband = 40;
+ cfg.accxy_deadband = 40;
cfg.baro_tab_size = 21;
cfg.baro_noise_lpf = 0.6f;
- cfg.baro_cf = 0.985f;
+ cfg.baro_cf_vel = 0.995f;
+ cfg.baro_cf_alt = 0.900f;
+ cfg.acc_unarmedcal = 1;
// Radio
parseRcChannels("AETR1234");
diff --git a/src/drv_i2c_soft.c b/src/drv_i2c_soft.c
index ac8b79c8bf..264ff06645 100644
--- a/src/drv_i2c_soft.c
+++ b/src/drv_i2c_soft.c
@@ -7,14 +7,14 @@
#ifdef SOFT_I2C
-#define SCL_H GPIOB->BSRR = GPIO_Pin_10 /* GPIO_SetBits(GPIOB , GPIO_Pin_10) */
-#define SCL_L GPIOB->BRR = GPIO_Pin_10 /* GPIO_ResetBits(GPIOB , GPIO_Pin_10) */
+#define SCL_H GPIOB->BSRR = Pin_10 /* GPIO_SetBits(GPIOB , GPIO_Pin_10) */
+#define SCL_L GPIOB->BRR = Pin_10 /* GPIO_ResetBits(GPIOB , GPIO_Pin_10) */
-#define SDA_H GPIOB->BSRR = GPIO_Pin_11 /* GPIO_SetBits(GPIOB , GPIO_Pin_11) */
-#define SDA_L GPIOB->BRR = GPIO_Pin_11 /* GPIO_ResetBits(GPIOB , GPIO_Pin_11) */
+#define SDA_H GPIOB->BSRR = Pin_11 /* GPIO_SetBits(GPIOB , GPIO_Pin_11) */
+#define SDA_L GPIOB->BRR = Pin_11 /* GPIO_ResetBits(GPIOB , GPIO_Pin_11) */
-#define SCL_read (GPIOB->IDR & GPIO_Pin_10) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_10) */
-#define SDA_read (GPIOB->IDR & GPIO_Pin_11) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_11) */
+#define SCL_read (GPIOB->IDR & Pin_10) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_10) */
+#define SDA_read (GPIOB->IDR & Pin_11) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_11) */
static void I2C_delay(void)
{
diff --git a/src/drv_mpu6050.c b/src/drv_mpu6050.c
index 5323ab72f3..7dc6e9eff3 100644
--- a/src/drv_mpu6050.c
+++ b/src/drv_mpu6050.c
@@ -233,9 +233,9 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
static void mpu6050AccInit(void)
{
if (mpuAccelHalf)
- acc_1G = 255;
+ acc_1G = 255 * 8;
else
- acc_1G = 512;
+ acc_1G = 512 * 8;
}
static void mpu6050AccRead(int16_t *accData)
@@ -244,9 +244,9 @@ static void mpu6050AccRead(int16_t *accData)
#ifndef MPU6050_DMP
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
- accData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 8;
- accData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 8;
- accData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 8;
+ accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
+ accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
+ accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
#else
accData[0] = accData[1] = accData[2] = 0;
#endif
diff --git a/src/drv_pwm.c b/src/drv_pwm.c
index 4bf6e9a0d3..9c6438a5a2 100755
--- a/src/drv_pwm.c
+++ b/src/drv_pwm.c
@@ -1,52 +1,14 @@
-#include "board.h"
-
-#define PULSE_1MS (1000) // 1ms pulse width
-
-/* FreeFlight/Naze32 timer layout
- TIM2_CH1 RC1 PWM1
- TIM2_CH2 RC2 PWM2
- TIM2_CH3 RC3/UA2_TX PWM3
- TIM2_CH4 RC4/UA2_RX PWM4
- TIM3_CH1 RC5 PWM5
- TIM3_CH2 RC6 PWM6
- TIM3_CH3 RC7 PWM7
- TIM3_CH4 RC8 PWM8
- TIM1_CH1 PWM1 PWM9
- TIM1_CH4 PWM2 PWM10
- TIM4_CH1 PWM3 PWM11
- TIM4_CH2 PWM4 PWM12
- TIM4_CH3 PWM5 PWM13
- TIM4_CH4 PWM6 PWM14
-
- // RX1 TIM2_CH1 PA0 [also PPM] [also used for throttle calibration]
- // RX2 TIM2_CH2 PA1
- // RX3 TIM2_CH3 PA2 [also UART2_TX]
- // RX4 TIM2_CH4 PA3 [also UART2_RX]
- // RX5 TIM3_CH1 PA6 [also ADC_IN6]
- // RX6 TIM3_CH2 PA7 [also ADC_IN7]
- // RX7 TIM3_CH3 PB0 [also ADC_IN8]
- // RX8 TIM3_CH4 PB1 [also ADC_IN9]
-
- // Outputs
- // PWM1 TIM1_CH1 PA8
- // PWM2 TIM1_CH4 PA11
- // PWM3 TIM4_CH1 PB6? [also I2C1_SCL]
- // PWM4 TIM4_CH2 PB7 [also I2C1_SDA]
- // PWM5 TIM4_CH3 PB8
- // PWM6 TIM4_CH4 PB9
-
- Groups that allow running different period (ex 50Hz servos + 400Hz throttle + etc):
- TIM2 4 channels
- TIM3 4 channels
- TIM1 2 channels
- TIM4 4 channels
-
- Configuration maps:
-
- 1) multirotor PPM input
- PWM1 used for PPM
- PWM5..8 used for motors
- PWM9..10 used for servo or else motors
+#include "board.h"
+
+#define PULSE_1MS (1000) // 1ms pulse width
+
+/*
+ Configuration maps:
+
+ 1) multirotor PPM input
+ 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
@@ -66,39 +28,19 @@
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
- PWM11.14 used for servos
-*/
-
-typedef void pwmCallbackPtr(uint8_t port, uint16_t capture);
-
-static const pwmHardware_t timerHardware[] = {
- { TIM2, GPIOA, GPIO_Pin_0, TIM_Channel_1, TIM2_IRQn, 0, }, // PWM1
- { TIM2, GPIOA, GPIO_Pin_1, TIM_Channel_2, TIM2_IRQn, 0, }, // PWM2
- { TIM2, GPIOA, GPIO_Pin_2, TIM_Channel_3, TIM2_IRQn, 0, }, // PWM3
- { TIM2, GPIOA, GPIO_Pin_3, TIM_Channel_4, TIM2_IRQn, 0, }, // PWM4
- { TIM3, GPIOA, GPIO_Pin_6, TIM_Channel_1, TIM3_IRQn, 0, }, // PWM5
- { TIM3, GPIOA, GPIO_Pin_7, TIM_Channel_2, TIM3_IRQn, 0, }, // PWM6
- { TIM3, GPIOB, GPIO_Pin_0, TIM_Channel_3, TIM3_IRQn, 0, }, // PWM7
- { TIM3, GPIOB, GPIO_Pin_1, TIM_Channel_4, TIM3_IRQn, 0, }, // PWM8
- { TIM1, GPIOA, GPIO_Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, }, // PWM9
- { TIM1, GPIOA, GPIO_Pin_11, TIM_Channel_4, TIM1_CC_IRQn, 1, }, // PWM10
- { TIM4, GPIOB, GPIO_Pin_6, TIM_Channel_1, TIM4_IRQn, 0, }, // PWM11
- { TIM4, GPIOB, GPIO_Pin_7, TIM_Channel_2, TIM4_IRQn, 0, }, // PWM12
- { TIM4, GPIOB, GPIO_Pin_8, TIM_Channel_3, TIM4_IRQn, 0, }, // PWM13
- { TIM4, GPIOB, GPIO_Pin_9, TIM_Channel_4, TIM4_IRQn, 0, }, // PWM14
-};
-
-typedef struct {
- pwmCallbackPtr *callback;
- volatile uint16_t *ccr;
- uint16_t period;
-
- // for input only
- uint8_t channel;
- uint8_t state;
+ PWM1 used for PPM
+ PWM5..8 used for servos
+ PWM9 used for motor throttle +PWM10 for 2nd motor
+ PWM11.14 used for servos
+*/
+
+typedef struct {
+ volatile uint16_t *ccr;
+ uint16_t period;
+
+ // for input only
+ uint8_t channel;
+ uint8_t state;
uint16_t rise;
uint16_t fall;
uint16_t capture;
@@ -192,38 +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);
-}
-
-static void pwmNVICConfig(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);
-}
-
-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;
+ 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_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
TIM_OCInitStructure.TIM_Pulse = value;
@@ -247,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;
@@ -274,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);
@@ -296,99 +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;
- }
- return p;
-}
-
-static pwmPortData_t *pwmInConfig(uint8_t port, pwmCallbackPtr 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);
- pwmNVICConfig(timerHardware[port].irq);
- // set callback before configuring interrupts
- p->callback = callback;
- p->channel = channel;
-
- switch (timerHardware[port].channel) {
- case TIM_Channel_1:
- TIM_ITConfig(timerHardware[port].tim, TIM_IT_CC1, ENABLE);
- break;
- case TIM_Channel_2:
- TIM_ITConfig(timerHardware[port].tim, TIM_IT_CC2, ENABLE);
- break;
- case TIM_Channel_3:
- TIM_ITConfig(timerHardware[port].tim, TIM_IT_CC3, ENABLE);
- break;
- case TIM_Channel_4:
- TIM_ITConfig(timerHardware[port].tim, TIM_IT_CC4, ENABLE);
- break;
- }
- return p;
-}
-
-void TIM1_CC_IRQHandler(void)
-{
- uint8_t port;
-
- if (TIM_GetITStatus(TIM1, TIM_IT_CC1) == SET) {
- port = PWM9;
- TIM_ClearITPendingBit(TIM1, TIM_IT_CC1);
- pwmPorts[port].callback(port, TIM_GetCapture1(TIM1));
- } else if (TIM_GetITStatus(TIM1, TIM_IT_CC4) == SET) {
- port = PWM10;
- TIM_ClearITPendingBit(TIM1, TIM_IT_CC4);
- pwmPorts[port].callback(port, TIM_GetCapture4(TIM1));
- }
-}
-
-static void pwmTIMxHandler(TIM_TypeDef *tim, uint8_t portBase)
-{
- int8_t port;
-
- // Generic CC handler for TIM2,3,4
- if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) {
- port = portBase + 0;
- TIM_ClearITPendingBit(tim, TIM_IT_CC1);
- pwmPorts[port].callback(port, TIM_GetCapture1(tim));
- } else if (TIM_GetITStatus(tim, TIM_IT_CC2) == SET) {
- port = portBase + 1;
- TIM_ClearITPendingBit(tim, TIM_IT_CC2);
- pwmPorts[port].callback(port, TIM_GetCapture2(tim));
- } else if (TIM_GetITStatus(tim, TIM_IT_CC3) == SET) {
- port = portBase + 2;
- TIM_ClearITPendingBit(tim, TIM_IT_CC3);
- pwmPorts[port].callback(port, TIM_GetCapture3(tim));
- } else if (TIM_GetITStatus(tim, TIM_IT_CC4) == SET) {
- port = portBase + 3;
- TIM_ClearITPendingBit(tim, TIM_IT_CC4);
- pwmPorts[port].callback(port, TIM_GetCapture4(tim));
- }
-}
-
-void TIM2_IRQHandler(void)
-{
- pwmTIMxHandler(TIM2, PWM1); // PWM1..4
-}
-
-void TIM3_IRQHandler(void)
-{
- pwmTIMxHandler(TIM3, PWM5); // PWM5..8
-}
-
-void TIM4_IRQHandler(void)
-{
- pwmTIMxHandler(TIM4, PWM11); // PWM11..14
-}
-
-static void ppmCallback(uint8_t port, uint16_t capture)
-{
- uint16_t diff;
- static uint16_t now;
+ 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];
+ const timerHardware_t *timerHardwarePtr = &(timerHardware[port]);
+
+ p->channel = channel;
+
+ 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;
static uint16_t last = 0;
static uint8_t chan = 0;
static uint8_t GoodPulses;
@@ -453,25 +307,31 @@ 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;
-
- if (setup[i] == 0xFF) // terminator
- break;
-
-#ifdef OLIMEXINO
- // PWM2 is connected to LED2 on the board and cannot be connected.
- 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;
+ uint8_t port = setup[i] & 0x0F;
+ uint8_t mask = setup[i] & 0xF0;
+
+ if (setup[i] == 0xFF) // terminator
+ break;
+
+#ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER
+ // PWM2 is connected to LED2 on the board and cannot be connected unless you cut LED2_E
+ if (port == PWM2)
+ continue;
+#endif
+
+ // 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 496a79ccc7..416a2c93ba 100755
--- a/src/drv_pwm.h
+++ b/src/drv_pwm.h
@@ -30,24 +30,17 @@ enum {
PWM9,
PWM10,
PWM11,
- PWM12,
- PWM13,
- PWM14,
- MAX_PORTS
-};
-
-typedef struct {
- TIM_TypeDef *tim;
- GPIO_TypeDef *gpio;
- uint32_t pin;
- uint8_t channel;
- uint8_t irq;
- uint8_t outputEnable;
-} pwmHardware_t;
-
-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);
+ 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);
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
new file mode 100644
index 0000000000..fe25f9ce59
--- /dev/null
+++ b/src/drv_timer.c
@@ -0,0 +1,229 @@
+#include "board.h"
+
+#define PULSE_1MS (1000) // 1ms pulse width
+
+/* FreeFlight/Naze32 timer layout
+ TIM2_CH1 RC1 PWM1
+ TIM2_CH2 RC2 PWM2
+ TIM2_CH3 RC3/UA2_TX PWM3
+ TIM2_CH4 RC4/UA2_RX PWM4
+ TIM3_CH1 RC5 PWM5
+ TIM3_CH2 RC6 PWM6
+ TIM3_CH3 RC7 PWM7
+ TIM3_CH4 RC8 PWM8
+ TIM1_CH1 PWM1 PWM9
+ TIM1_CH4 PWM2 PWM10
+ TIM4_CH1 PWM3 PWM11
+ TIM4_CH2 PWM4 PWM12
+ TIM4_CH3 PWM5 PWM13
+ TIM4_CH4 PWM6 PWM14
+
+ RX1 TIM2_CH1 PA0 [also PPM] [also used for throttle calibration]
+ RX2 TIM2_CH2 PA1
+ RX3 TIM2_CH3 PA2 [also UART2_TX]
+ RX4 TIM2_CH4 PA3 [also UART2_RX]
+ RX5 TIM3_CH1 PA6 [also ADC_IN6]
+ RX6 TIM3_CH2 PA7 [also ADC_IN7]
+ RX7 TIM3_CH3 PB0 [also ADC_IN8]
+ RX8 TIM3_CH4 PB1 [also ADC_IN9]
+
+ Outputs
+ PWM1 TIM1_CH1 PA8
+ PWM2 TIM1_CH4 PA11
+ PWM3 TIM4_CH1 PB6? [also I2C1_SCL]
+ PWM4 TIM4_CH2 PB7 [also I2C1_SDA]
+ PWM5 TIM4_CH3 PB8
+ PWM6 TIM4_CH4 PB9
+
+ Groups that allow running different period (ex 50Hz servos + 400Hz throttle + etc):
+ TIM2 4 channels
+ TIM3 4 channels
+ TIM1 2 channels
+ TIM4 4 channels
+*/
+
+const timerHardware_t timerHardware[] = {
+ { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, }, // PWM1
+ { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, }, // PWM2
+ { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, }, // PWM3
+ { TIM2, GPIOA, Pin_3, TIM_Channel_4, TIM2_IRQn, 0, }, // PWM4
+ { TIM3, GPIOA, Pin_6, TIM_Channel_1, TIM3_IRQn, 0, }, // PWM5
+ { TIM3, GPIOA, Pin_7, TIM_Channel_2, TIM3_IRQn, 0, }, // PWM6
+ { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, }, // PWM7
+ { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, }, // PWM8
+ { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, }, // PWM9
+ { TIM1, GPIOA, Pin_11, TIM_Channel_4, TIM1_CC_IRQn, 1, }, // PWM10
+ { TIM4, GPIOB, Pin_6, TIM_Channel_1, TIM4_IRQn, 0, }, // PWM11
+ { TIM4, GPIOB, Pin_7, TIM_Channel_2, TIM4_IRQn, 0, }, // PWM12
+ { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 0, }, // PWM13
+ { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 0, }, // PWM14
+};
+
+#define MAX_TIMERS 4 // TIM1..TIM4
+#define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4
+
+static const TIM_TypeDef *timers[MAX_TIMERS] = {
+ TIM1, TIM2, TIM3, TIM4
+};
+
+static const uint8_t channels[CC_CHANNELS_PER_TIMER] = {
+ TIM_Channel_1, TIM_Channel_2, TIM_Channel_3, TIM_Channel_4
+};
+
+typedef struct timerConfig_s {
+ TIM_TypeDef *tim;
+ uint8_t channel;
+ timerCCCallbackPtr *callback;
+ uint8_t reference;
+} timerConfig_t;
+
+timerConfig_t timerConfig[MAX_TIMERS * CC_CHANNELS_PER_TIMER];
+
+static uint8_t lookupTimerIndex(const TIM_TypeDef *tim)
+{
+ uint8_t timerIndex = 0;
+ while (timers[timerIndex] != tim) {
+ timerIndex++;
+ }
+ return timerIndex;
+}
+
+static uint8_t lookupChannelIndex(const uint8_t channel)
+{
+ uint8_t channelIndex = 0;
+ while (channels[channelIndex] != channel) {
+ channelIndex++;
+ }
+ return channelIndex;
+}
+
+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]);
+}
+
+static void timCCxHandler(TIM_TypeDef *tim)
+{
+ uint16_t capture;
+ timerConfig_t *timerConfig;
+
+ if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) {
+ TIM_ClearITPendingBit(tim, TIM_IT_CC1);
+
+ timerConfig = findTimerConfig(tim, TIM_Channel_1);
+ capture = TIM_GetCapture1(tim);
+ } else if (TIM_GetITStatus(tim, TIM_IT_CC2) == SET) {
+ TIM_ClearITPendingBit(tim, TIM_IT_CC2);
+
+ timerConfig = findTimerConfig(tim, TIM_Channel_2);
+ capture = TIM_GetCapture2(tim);
+ } else if (TIM_GetITStatus(tim, TIM_IT_CC3) == SET) {
+ TIM_ClearITPendingBit(tim, TIM_IT_CC3);
+
+ timerConfig = findTimerConfig(tim, TIM_Channel_3);
+ capture = TIM_GetCapture3(tim);
+ } else if (TIM_GetITStatus(tim, TIM_IT_CC4) == SET) {
+ TIM_ClearITPendingBit(tim, TIM_IT_CC4);
+
+ timerConfig = findTimerConfig(tim, TIM_Channel_4);
+ capture = TIM_GetCapture4(tim);
+ } else {
+ return; // avoid uninitialised variable dereference
+ }
+
+ if (!timerConfig->callback) {
+ return;
+ }
+ timerConfig->callback(timerConfig->reference, capture);
+}
+
+void TIM1_CC_IRQHandler(void)
+{
+ timCCxHandler(TIM1);
+}
+
+void TIM2_IRQHandler(void)
+{
+ timCCxHandler(TIM2);
+}
+
+void TIM3_IRQHandler(void)
+{
+ timCCxHandler(TIM3);
+}
+
+void TIM4_IRQHandler(void)
+{
+ timCCxHandler(TIM4);
+}
diff --git a/src/drv_timer.h b/src/drv_timer.h
new file mode 100644
index 0000000000..79edc5beff
--- /dev/null
+++ b/src/drv_timer.h
@@ -0,0 +1,22 @@
+#pragma once
+
+typedef void timerCCCallbackPtr(uint8_t port, uint16_t capture);
+
+typedef struct {
+ TIM_TypeDef *tim;
+ GPIO_TypeDef *gpio;
+ uint32_t pin;
+ uint8_t channel;
+ uint8_t irq;
+ uint8_t outputEnable;
+} timerHardware_t;
+
+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);
+void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback);
+void configureTimerChannelCallback(TIM_TypeDef *tim, uint8_t channel, uint8_t reference, timerCCCallbackPtr *callback);
diff --git a/src/drv_uart.c b/src/drv_uart.c
index e2e30e7fd8..8515cdf071 100755
--- a/src/drv_uart.c
+++ b/src/drv_uart.c
@@ -68,7 +68,7 @@ serialPort_t *serialUSART2(uint32_t baudRate, portmode_t mode)
// USART2_TX PA2
// USART2_RX PA3
gpio.speed = Speed_2MHz;
- gpio.pin = GPIO_Pin_2;
+ gpio.pin = Pin_2;
gpio.mode = Mode_AF_PP;
if (mode & MODE_TX)
gpioInit(GPIOA, &gpio);
@@ -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/gps.c b/src/gps.c
index 28dae1d9c2..e74b738c0f 100644
--- a/src/gps.c
+++ b/src/gps.c
@@ -8,7 +8,6 @@
const uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 };
static void GPS_NewData(uint16_t c);
-static void GPS_set_pids(void);
static void gpsPrint(const char *str);
static const char * const gpsInitStrings[] = {
@@ -391,7 +390,7 @@ void GPS_reset_home_position(void)
}
}
-//reset navigation (stop the navigation processor, and clear nav)
+// reset navigation (stop the navigation processor, and clear nav)
void GPS_reset_nav(void)
{
int i;
@@ -406,8 +405,8 @@ void GPS_reset_nav(void)
}
}
-//Get the relevant P I D values and set the PID controllers
-static void GPS_set_pids(void)
+// Get the relevant P I D values and set the PID controllers
+void GPS_set_pids(void)
{
posholdPID_PARAM.kP = (float)cfg.P8[PIDPOS] / 100.0f;
posholdPID_PARAM.kI = (float)cfg.I8[PIDPOS] / 100.0f;
diff --git a/src/imu.c b/src/imu.c
index 8470b4f26d..5adf86fb31 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -2,6 +2,9 @@
#include "mw.h"
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
+int32_t accSum[3];
+uint32_t accTimeSum = 0; // keep track for integration of acc
+int accSumCount = 0;
int16_t acc_25deg = 0;
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
@@ -178,6 +181,68 @@ static int16_t _atan2f(float y, float x)
return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));
}
+int32_t applyDeadband(int32_t value, int32_t deadband)
+{
+ if (abs(value) < deadband) {
+ value = 0;
+ } else if (value > 0) {
+ value -= deadband;
+ } else if (value < 0) {
+ value += deadband;
+ }
+ return value;
+}
+
+// rotate acc into Earth frame and calculate acceleration in it
+void acc_calc(uint32_t deltaT)
+{
+ static int32_t accZoffset = 0;
+ float rpy[3];
+ t_fp_vector accel_ned;
+
+ // the accel values have to be rotated into the earth frame
+ rpy[0] = -(float) angle[ROLL] * RADX10;
+ rpy[1] = -(float) angle[PITCH] * RADX10;
+ rpy[2] = -(float) heading * RADX10 * 10;
+
+ accel_ned.V.X = accSmooth[0];
+ accel_ned.V.Y = accSmooth[1];
+ accel_ned.V.Z = accSmooth[2];
+
+ rotateV(&accel_ned.V, rpy);
+
+ if (cfg.acc_unarmedcal == 1) {
+ if (!f.ARMED) {
+ accZoffset -= accZoffset / 64;
+ accZoffset += accel_ned.V.Z;
+ }
+ accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
+ } else
+ accel_ned.V.Z -= acc_1G;
+
+ // apply Deadband to reduce integration drift and vibration influence
+ accel_ned.V.Z = applyDeadband(accel_ned.V.Z, cfg.accz_deadband);
+ accel_ned.V.X = applyDeadband(accel_ned.V.X, cfg.accxy_deadband);
+ accel_ned.V.Y = applyDeadband(accel_ned.V.Y, cfg.accxy_deadband);
+
+ // sum up Values for later integration to get velocity and distance
+ accTimeSum += deltaT;
+ accSumCount++;
+
+ accSum[0] += accel_ned.V.X;
+ accSum[1] += accel_ned.V.Y;
+ accSum[2] += accel_ned.V.Z;
+}
+
+void accSum_reset(void)
+{
+ accSum[0] = 0;
+ accSum[1] = 0;
+ accSum[2] = 0;
+ accSumCount = 0;
+ accTimeSum = 0;
+}
+
// Use original baseflight angle calculation
// #define BASEFLIGHT_CALC
static float invG;
@@ -190,6 +255,7 @@ static void getEstimatedAttitude(void)
static float accLPF[3];
static uint32_t previousT;
uint32_t currentT = micros();
+ uint32_t deltaT;
float scale, deltaGyroAngle[3];
#ifndef BASEFLIGHT_CALC
float sqGZ;
@@ -198,8 +264,8 @@ static void getEstimatedAttitude(void)
float sqGX_sqGZ;
float invmagXZ;
#endif
-
- scale = (currentT - previousT) * gyro.scale;
+ deltaT = currentT - previousT;
+ scale = deltaT * gyro.scale;
previousT = currentT;
// Initialization
@@ -284,6 +350,8 @@ static void getEstimatedAttitude(void)
}
#endif
+ acc_calc(deltaT); // rotate acc vector into earth frame
+
if (cfg.throttle_angle_correction) {
int cosZ = EstG.V.Z / acc_1G * 100.0f;
throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
@@ -293,19 +361,6 @@ static void getEstimatedAttitude(void)
#ifdef BARO
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
-#define INIT_DELAY 4000000 // 4 sec initialization delay
-
-int16_t applyDeadband(int32_t value, int32_t deadband)
-{
- if (abs(value) < deadband) {
- value = 0;
- } else if (value > 0) {
- value -= deadband;
- } else if (value < 0) {
- value += deadband;
- }
- return value;
-}
int getEstimatedAltitude(void)
{
@@ -315,10 +370,11 @@ int getEstimatedAltitude(void)
uint32_t dTime;
int32_t error;
int32_t baroVel;
- int32_t accZ;
int32_t vel_tmp;
- static int32_t accZoffset = 0;
+ int32_t BaroAlt_tmp;
+ float vel_calc;
static float vel = 0.0f;
+ static float accAlt = 0.0f;
static int32_t lastBaroAlt;
dTime = currentT - previousT;
@@ -329,53 +385,59 @@ int getEstimatedAltitude(void)
if (calibratingB > 0) {
baroGroundPressure = baroPressureSum / (cfg.baro_tab_size - 1);
calibratingB--;
+ vel = 0;
+ accAlt = 0;
}
// pressure relative to ground pressure with temperature compensation (fast!)
// baroGroundPressure is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
- BaroAlt = logf(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter
- EstAlt = (EstAlt * 6 + BaroAlt * 2) / 8; // additional LPF to reduce baro noise
+ BaroAlt_tmp = logf(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter
+ BaroAlt = BaroAlt * cfg.baro_noise_lpf + BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
+
+ // Integrator - velocity, cm/sec
+ vel_calc = (float) accSum[2] * accVelScale * (float) accTimeSum / (float) accSumCount;
+ vel += vel_calc;
+
+ // Integrator - Altitude in cm
+ accAlt += vel * ((float) accTimeSum * 0.0000005f); // integrate velocity to get distance (x= a/2 * t^2)
+ accAlt = accAlt * cfg.baro_cf_alt + (float) BaroAlt *(1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
+ EstAlt = accAlt;
+
+#if 0
+ debug[0] = accSum[2] / accSumCount; // acceleration
+ debug[1] = vel; // velocity
+ debug[2] = accAlt; // height
+#endif
+
+ accSum_reset();
//P
error = constrain(AltHold - EstAlt, -300, 300);
- error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
- BaroPID = constrain(((cfg.P8[PIDALT] * error) / 128), -150, +150);
+ error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
+ BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -150, +150);
//I
- errorAltitudeI += (cfg.I8[PIDALT] * error) / 64;
+ errorAltitudeI += cfg.I8[PIDALT] * error / 64;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
BaroPID += errorAltitudeI / 512; // I in range +/-60
- // projection of ACC vector to global Z, with 1G subtructed
- // Math: accZ = A * G / |G| - 1G (invG is calculated in getEstimatedAttitude)
- accZ = (accSmooth[ROLL] * EstG.V.X + accSmooth[PITCH] * EstG.V.Y + accSmooth[YAW] * EstG.V.Z) * invG;
-
- if (!f.ARMED) {
- accZoffset -= accZoffset / 8;
- accZoffset += accZ;
- }
- accZ -= accZoffset / 8;
- accZ = applyDeadband(accZ, cfg.accz_deadband);
-
- // Integrator - velocity, cm/sec
- vel += accZ * accVelScale * dTime;
-
- baroVel = (EstAlt - lastBaroAlt) * 1000000.0f / dTime;
- lastBaroAlt = EstAlt;
+
+ baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
+ lastBaroAlt = BaroAlt;
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
- baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
+ baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
- vel = vel * 0.985f + baroVel * 0.015f;
+ vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);
// D
vel_tmp = vel;
vel_tmp = applyDeadband(vel_tmp, 5);
vario = vel_tmp;
- BaroPID -= constrain((cfg.D8[PIDALT] * vel_tmp) / 16, -150, 150);
+ BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp / 16, -150, 150);
return 1;
}
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/mw.h b/src/mw.h
index 6d912b36cb..e97560c4a8 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -164,10 +164,13 @@ typedef struct config_t {
// sensor-related stuff
uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
- uint8_t accz_deadband; // ??
+ uint8_t accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations
+ uint8_t accxy_deadband; // set the acc deadband for xy-Axis
uint8_t baro_tab_size; // size of baro filter array
float baro_noise_lpf; // additional LPF to reduce baro noise
- float baro_cf; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
+ float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
+ float baro_cf_alt; // apply CF to use ACC for height estimation
+ uint8_t acc_unarmedcal; // turn automatic acc compensation on/off
uint16_t activate[CHECKBOXITEMS]; // activate switches
@@ -325,7 +328,10 @@ extern int16_t failsafeCnt;
extern int16_t debug[4];
extern int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
+extern int32_t accSum[3];
extern uint16_t acc_1G;
+extern uint32_t accTimeSum;
+extern int accSumCount;
extern uint32_t currentTime;
extern uint32_t previousTime;
extern uint16_t cycleTime;
@@ -451,6 +457,7 @@ void cliProcess(void);
void gpsInit(uint32_t baudrate);
void GPS_reset_home_position(void);
void GPS_reset_nav(void);
+void GPS_set_pids(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon);
int32_t wrap_18000(int32_t error);
diff --git a/src/sensors.c b/src/sensors.c
index 30dd529ce0..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
@@ -289,7 +289,7 @@ int Baro_update(void)
return 0;
baroDeadline = currentTime;
-
+
if (state) {
baro.get_up();
baro.start_ut();
@@ -470,21 +470,21 @@ int Mag_getADC(void)
writeEEPROM(1, true);
}
}
-
+
return 1;
}
#endif
#ifdef SONAR
-void Sonar_init(void)
+void Sonar_init(void)
{
hcsr04_init(sonar_rc78);
sensorsSet(SENSOR_SONAR);
sonarAlt = 0;
}
-void Sonar_update(void)
+void Sonar_update(void)
{
hcsr04_get_distance(&sonarAlt);
}
diff --git a/src/serial.c b/src/serial.c
index bf56a3c4b6..b35fe7a09f 100755
--- a/src/serial.c
+++ b/src/serial.c
@@ -439,8 +439,14 @@ static void evaluateCommand(void)
break;
case MSP_RAW_IMU:
headSerialReply(18);
- for (i = 0; i < 3; i++)
- serialize16(accSmooth[i]);
+ // Retarded hack until multiwiidorks start using real units for sensor data
+ if (acc_1G > 1024) {
+ for (i = 0; i < 3; i++)
+ serialize16(accSmooth[i] / 8);
+ } else {
+ for (i = 0; i < 3; i++)
+ serialize16(accSmooth[i]);
+ }
for (i = 0; i < 3; i++)
serialize16(gyroData[i]);
for (i = 0; i < 3; i++)