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

Merge remote-tracking branch 'multiwii/upstream'

Browse source 
Conflicts:
	src/board.h
This commit is contained in:
Dominic Clifton 2013-08-28 19:11:18 +01:00
commit 14d0f90278
21 changed files with 862 additions and 382 deletions
Download patch file Download diff file Expand all files Collapse all files

3
Makefile
View file

@ -77,6 +77,7 @@ NAZE_SRC = drv_spi.c \
drv_mpu6050.c \ drv_mpu6050.c \
drv_l3g4200d.c \ drv_l3g4200d.c \
drv_pwm.c \ drv_pwm.c \
drv_timer.c \
$(COMMON_SRC) $(COMMON_SRC)
# Source files for the FY90Q target # Source files for the FY90Q target
@ -92,6 +93,8 @@ OLIMEXINO_SRC = drv_spi.c \
drv_mpu6050.c \ drv_mpu6050.c \
drv_l3g4200d.c \ drv_l3g4200d.c \
drv_pwm.c \ drv_pwm.c \
drv_timer.c \
drv_softserial.c \
$(COMMON_SRC) $(COMMON_SRC)
# Search path for baseflight sources # Search path for baseflight sources

30
baseflight.uvproj
View file

@ -647,6 +647,16 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>.\src\drv_spi.c</FilePath> <FilePath>.\src\drv_spi.c</FilePath>
</File> </File>
<File>
<FileName>drv_timer.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_timer.c</FilePath>
</File>
<File>
<FileName>drv_softserial.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_softserial.c</FilePath>
</File>
</Files> </Files>
</Group> </Group>
<Group> <Group>
@ -1468,6 +1478,16 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>.\src\drv_spi.c</FilePath> <FilePath>.\src\drv_spi.c</FilePath>
</File> </File>
<File>
<FileName>drv_timer.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_timer.c</FilePath>
</File>
<File>
<FileName>drv_softserial.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_softserial.c</FilePath>
</File>
</Files> </Files>
</Group> </Group>
<Group> <Group>
@ -2473,6 +2493,16 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>.\src\drv_spi.c</FilePath> <FilePath>.\src\drv_spi.c</FilePath>
</File> </File>
<File>
<FileName>drv_timer.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_timer.c</FilePath>
</File>
<File>
<FileName>drv_softserial.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_softserial.c</FilePath>
</File>
</Files> </Files>
</Group> </Group>
<Group> <Group>

16
src/board.h
View file

@ -110,9 +110,9 @@ typedef struct baro_t
#ifdef FY90Q #ifdef FY90Q
// FY90Q // FY90Q
#define LED0_GPIO GPIOC #define LED0_GPIO GPIOC
#define LED0_PIN GPIO_Pin_12 #define LED0_PIN Pin_12
#define LED1_GPIO GPIOA #define LED1_GPIO GPIOA
#define LED1_PIN GPIO_Pin_15 #define LED1_PIN Pin_15
#define GYRO #define GYRO
#define ACC #define ACC
@ -148,13 +148,13 @@ typedef struct baro_t
// Afroflight32 // Afroflight32
#define LED0_GPIO GPIOB #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_GPIO GPIOB
#define LED1_PIN GPIO_Pin_4 // PB4 (LED) #define LED1_PIN Pin_4 // PB4 (LED)
#define BEEP_GPIO GPIOA #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_GPIO GPIOC
#define BARO_PIN GPIO_Pin_13 #define BARO_PIN Pin_13
#define GYRO #define GYRO
#define ACC #define ACC
@ -221,7 +221,9 @@ typedef struct baro_t
#include "drv_mpu6050.h" #include "drv_mpu6050.h"
#include "drv_l3g4200d.h" #include "drv_l3g4200d.h"
#include "drv_pwm.h" #include "drv_pwm.h"
#include "drv_timer.h"
#include "drv_uart.h" #include "drv_uart.h"
#include "drv_softserial.h"
#else #else
// AfroFlight32 // AfroFlight32
@ -238,7 +240,9 @@ typedef struct baro_t
#include "drv_mpu6050.h" #include "drv_mpu6050.h"
#include "drv_l3g4200d.h" #include "drv_l3g4200d.h"
#include "drv_pwm.h" #include "drv_pwm.h"
#include "drv_timer.h"
#include "drv_uart.h" #include "drv_uart.h"
#include "drv_softserial.h"
#include "drv_hcsr04.h" #include "drv_hcsr04.h"
#endif #endif

6
src/cli.c
View file

@ -175,11 +175,15 @@ const clivalue_t valueTable[] = {
{ "gimbal_roll_max", VAR_UINT16, &cfg.gimbal_roll_max, 100, 3000 }, { "gimbal_roll_max", VAR_UINT16, &cfg.gimbal_roll_max, 100, 3000 },
{ "gimbal_roll_mid", VAR_UINT16, &cfg.gimbal_roll_mid, 100, 3000 }, { "gimbal_roll_mid", VAR_UINT16, &cfg.gimbal_roll_mid, 100, 3000 },
{ "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 }, { "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_pitch", VAR_INT16, &cfg.angleTrim[PITCH], -300, 300 },
{ "acc_trim_roll", VAR_INT16, &cfg.angleTrim[ROLL], -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_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_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 }, { "mag_declination", VAR_INT16, &cfg.mag_declination, -18000, 18000 },
{ "gps_pos_p", VAR_UINT8, &cfg.P8[PIDPOS], 0, 200 }, { "gps_pos_p", VAR_UINT8, &cfg.P8[PIDPOS], 0, 200 },
{ "gps_pos_i", VAR_UINT8, &cfg.I8[PIDPOS], 0, 200 }, { "gps_pos_i", VAR_UINT8, &cfg.I8[PIDPOS], 0, 200 },

14
src/config.c
View file

@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles
config_t cfg; // profile config struct config_t cfg; // profile config struct
const char rcChannelLetters[] = "AERT1234"; 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 uint32_t enabledSensors = 0;
static void resetConf(void); 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 cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
setPIDController(cfg.pidController); setPIDController(cfg.pidController);
GPS_set_pids();
} }
void writeEEPROM(uint8_t b, uint8_t updateProfile) void writeEEPROM(uint8_t b, uint8_t updateProfile)
@ -217,9 +218,9 @@ static void resetConf(void)
cfg.P8[YAW] = 85; cfg.P8[YAW] = 85;
cfg.I8[YAW] = 45; cfg.I8[YAW] = 45;
cfg.D8[YAW] = 0; cfg.D8[YAW] = 0;
cfg.P8[PIDALT] = 64; cfg.P8[PIDALT] = 50;
cfg.I8[PIDALT] = 25; cfg.I8[PIDALT] = 25;
cfg.D8[PIDALT] = 24; cfg.D8[PIDALT] = 80;
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100; cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100; cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
cfg.D8[PIDPOS] = 0; cfg.D8[PIDPOS] = 0;
@ -249,10 +250,13 @@ static void resetConf(void)
cfg.angleTrim[1] = 0; cfg.angleTrim[1] = 0;
cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
cfg.acc_lpf_factor = 4; cfg.acc_lpf_factor = 4;
cfg.accz_deadband = 50; cfg.accz_deadband = 40;
cfg.accxy_deadband = 40;
cfg.baro_tab_size = 21; cfg.baro_tab_size = 21;
cfg.baro_noise_lpf = 0.6f; 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 // Radio
parseRcChannels("AETR1234"); parseRcChannels("AETR1234");

12
src/drv_i2c_soft.c
View file

@ -7,14 +7,14 @@
#ifdef SOFT_I2C #ifdef SOFT_I2C
#define SCL_H GPIOB->BSRR = GPIO_Pin_10 /* GPIO_SetBits(GPIOB , GPIO_Pin_10) */ #define SCL_H GPIOB->BSRR = Pin_10 /* GPIO_SetBits(GPIOB , GPIO_Pin_10) */
#define SCL_L GPIOB->BRR = GPIO_Pin_10 /* GPIO_ResetBits(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_H GPIOB->BSRR = Pin_11 /* GPIO_SetBits(GPIOB , GPIO_Pin_11) */
#define SDA_L GPIOB->BRR = GPIO_Pin_11 /* GPIO_ResetBits(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 SCL_read (GPIOB->IDR & Pin_10) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_10) */
#define SDA_read (GPIOB->IDR & GPIO_Pin_11) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_11) */ #define SDA_read (GPIOB->IDR & Pin_11) /* GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_11) */
static void I2C_delay(void) static void I2C_delay(void)
{ {

10
src/drv_mpu6050.c
View file

@ -233,9 +233,9 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
static void mpu6050AccInit(void) static void mpu6050AccInit(void)
{ {
if (mpuAccelHalf) if (mpuAccelHalf)
acc_1G = 255; acc_1G = 255 * 8;
else else
acc_1G = 512; acc_1G = 512 * 8;
} }
static void mpu6050AccRead(int16_t *accData) static void mpu6050AccRead(int16_t *accData)
@ -244,9 +244,9 @@ static void mpu6050AccRead(int16_t *accData)
#ifndef MPU6050_DMP #ifndef MPU6050_DMP
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf); i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
accData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 8; accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
accData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 8; accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
accData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 8; accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
#else #else
accData[0] = accData[1] = accData[2] = 0; accData[0] = accData[1] = accData[2] = 0;
#endif #endif

340
src/drv_pwm.c
View file

@ -1,52 +1,14 @@
#include "board.h" #include "board.h"
#define PULSE_1MS (1000) // 1ms pulse width #define PULSE_1MS (1000) // 1ms pulse width
/* FreeFlight/Naze32 timer layout /*
TIM2_CH1 RC1 PWM1 Configuration maps:
TIM2_CH2 RC2 PWM2
TIM2_CH3 RC3/UA2_TX PWM3 1) multirotor PPM input
TIM2_CH4 RC4/UA2_RX PWM4 PWM1 used for PPM
TIM3_CH1 RC5 PWM5 PWM5..8 used for motors
TIM3_CH2 RC6 PWM6 PWM9..10 used for servo or else motors
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
PWM11..14 used for motors PWM11..14 used for motors
2) multirotor PPM input with more servos 2) multirotor PPM input with more servos
@ -66,39 +28,19 @@
PWM11.14 used for servos PWM11.14 used for servos
4) airplane / flying wing with PPM 4) airplane / flying wing with PPM
PWM1 used for PPM PWM1 used for PPM
PWM5..8 used for servos PWM5..8 used for servos
PWM9 used for motor throttle +PWM10 for 2nd motor PWM9 used for motor throttle +PWM10 for 2nd motor
PWM11.14 used for servos PWM11.14 used for servos
*/ */
typedef void pwmCallbackPtr(uint8_t port, uint16_t capture); typedef struct {
volatile uint16_t *ccr;
static const pwmHardware_t timerHardware[] = { uint16_t period;
{ TIM2, GPIOA, GPIO_Pin_0, TIM_Channel_1, TIM2_IRQn, 0, }, // PWM1
{ TIM2, GPIOA, GPIO_Pin_1, TIM_Channel_2, TIM2_IRQn, 0, }, // PWM2 // for input only
{ TIM2, GPIOA, GPIO_Pin_2, TIM_Channel_3, TIM2_IRQn, 0, }, // PWM3 uint8_t channel;
{ TIM2, GPIOA, GPIO_Pin_3, TIM_Channel_4, TIM2_IRQn, 0, }, // PWM4 uint8_t state;
{ 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;
uint16_t rise; uint16_t rise;
uint16_t fall; uint16_t fall;
uint16_t capture; uint16_t capture;
@ -192,38 +134,17 @@ static const uint8_t * const hardwareMaps[] = {
multiPWM, multiPWM,
multiPPM, multiPPM,
airPWM, airPWM,
airPPM, airPPM,
}; };
static void pwmTimeBase(TIM_TypeDef *tim, uint32_t period) #define PWM_TIMER_MHZ 1
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
{
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_TimeBaseStructure.TIM_Period = period - 1;
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / 1000000) - 1; // all timers run at 1MHz TIM_OCStructInit(&TIM_OCInitStructure);
TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
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;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
TIM_OCInitStructure.TIM_Pulse = value; 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_OC4Init(tim, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable);
break; break;
} }
} }
static void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
{ {
TIM_ICInitTypeDef TIM_ICInitStructure; TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = channel; TIM_ICInitStructure.TIM_Channel = channel;
TIM_ICInitStructure.TIM_ICPolarity = polarity; TIM_ICInitStructure.TIM_ICPolarity = polarity;
@ -274,13 +195,13 @@ static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
gpioInit(gpio, &cfg); gpioInit(gpio, &cfg);
} }
static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value) static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value)
{ {
pwmPortData_t *p = &pwmPorts[port]; pwmPortData_t *p = &pwmPorts[port];
pwmTimeBase(timerHardware[port].tim, period); configTimeBase(timerHardware[port].tim, period, PWM_TIMER_MHZ);
pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP); pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP);
pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value); pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value);
// Needed only on TIM1 // Needed only on TIM1
if (timerHardware[port].outputEnable) if (timerHardware[port].outputEnable)
TIM_CtrlPWMOutputs(timerHardware[port].tim, ENABLE); TIM_CtrlPWMOutputs(timerHardware[port].tim, ENABLE);
TIM_Cmd(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; p->ccr = &timerHardware[port].tim->CCR3;
break; break;
case TIM_Channel_4: case TIM_Channel_4:
p->ccr = &timerHardware[port].tim->CCR4; p->ccr = &timerHardware[port].tim->CCR4;
break; break;
} }
return p; return p;
} }
static pwmPortData_t *pwmInConfig(uint8_t port, pwmCallbackPtr callback, uint8_t channel) static pwmPortData_t *pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uint8_t channel)
{ {
pwmPortData_t *p = &pwmPorts[port]; pwmPortData_t *p = &pwmPorts[port];
pwmTimeBase(timerHardware[port].tim, 0xFFFF); const timerHardware_t *timerHardwarePtr = &(timerHardware[port]);
pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_IPD);
pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Rising); p->channel = channel;
TIM_Cmd(timerHardware[port].tim, ENABLE);
pwmNVICConfig(timerHardware[port].irq); pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPD);
// set callback before configuring interrupts pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
p->callback = callback;
p->channel = channel; timerInConfig(timerHardwarePtr, 0xFFFF, PWM_TIMER_MHZ);
configureTimerCaptureCompareInterrupt(timerHardwarePtr, port, callback);
switch (timerHardware[port].channel) {
case TIM_Channel_1: return p;
TIM_ITConfig(timerHardware[port].tim, TIM_IT_CC1, ENABLE); }
break;
case TIM_Channel_2: static void ppmCallback(uint8_t port, uint16_t capture)
TIM_ITConfig(timerHardware[port].tim, TIM_IT_CC2, ENABLE); {
break; uint16_t diff;
case TIM_Channel_3: static uint16_t now;
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;
static uint16_t last = 0; static uint16_t last = 0;
static uint8_t chan = 0; static uint8_t chan = 0;
static uint8_t GoodPulses; static uint8_t GoodPulses;
@ -453,25 +307,31 @@ bool pwmInit(drv_pwm_config_t *init)
setup = hardwareMaps[i]; setup = hardwareMaps[i];
for (i = 0; i < MAX_PORTS; i++) { for (i = 0; i < MAX_PORTS; i++) {
uint8_t port = setup[i] & 0x0F; uint8_t port = setup[i] & 0x0F;
uint8_t mask = setup[i] & 0xF0; uint8_t mask = setup[i] & 0xF0;
if (setup[i] == 0xFF) // terminator if (setup[i] == 0xFF) // terminator
break; break;
#ifdef OLIMEXINO #ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER
// PWM2 is connected to LED2 on the board and cannot be connected. // PWM2 is connected to LED2 on the board and cannot be connected unless you cut LED2_E
if (port == PWM2) if (port == PWM2)
continue; continue;
#endif #endif
// skip UART ports for GPS // skip UART ports for GPS
if (init->useUART && (port == PWM3 || port == PWM4)) if (init->useUART && (port == PWM3 || port == PWM4))
continue; continue;
// skip ADC for powerMeter if configured #ifdef SOFTSERIAL_19200_LOOPBACK
if (init->adcChannel && (init->adcChannel == port)) // skip softSerial ports
continue; 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 // hacks to allow current functionality
if (mask & (TYPE_IP | TYPE_IW) && !init->enableInput) if (mask & (TYPE_IP | TYPE_IW) && !init->enableInput)

35
src/drv_pwm.h
View file

@ -30,24 +30,17 @@ enum {
PWM9, PWM9,
PWM10, PWM10,
PWM11, PWM11,
PWM12, PWM12,
PWM13, PWM13,
PWM14, PWM14,
MAX_PORTS MAX_PORTS
}; };
typedef struct { void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity);
TIM_TypeDef *tim;
GPIO_TypeDef *gpio; bool pwmInit(drv_pwm_config_t *init); // returns whether driver is asking to calibrate throttle or not
uint32_t pin; void pwmWriteMotor(uint8_t index, uint16_t value);
uint8_t channel; void pwmWriteServo(uint8_t index, uint16_t value);
uint8_t irq; uint16_t pwmRead(uint8_t channel);
uint8_t outputEnable;
} pwmHardware_t; // void pwmWrite(uint8_t channel, uint16_t value);
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);

203
src/drv_softserial.c Normal file
View file

@ -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;
}

25
src/drv_softserial.h Normal file
View file

@ -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[];

229
src/drv_timer.c Normal file
View file

@ -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);
}

22
src/drv_timer.h Normal file
View file

@ -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);

22
src/drv_uart.c
View file

@ -68,7 +68,7 @@ serialPort_t *serialUSART2(uint32_t baudRate, portmode_t mode)
// USART2_TX PA2 // USART2_TX PA2
// USART2_RX PA3 // USART2_RX PA3
gpio.speed = Speed_2MHz; gpio.speed = Speed_2MHz;
gpio.pin = GPIO_Pin_2; gpio.pin = Pin_2;
gpio.mode = Mode_AF_PP; gpio.mode = Mode_AF_PP;
if (mode & MODE_TX) if (mode & MODE_TX)
gpioInit(GPIOA, &gpio); gpioInit(GPIOA, &gpio);
@ -238,12 +238,20 @@ void uartWrite(serialPort_t *s, uint8_t ch)
uartStartTxDMA(s); uartStartTxDMA(s);
} else { } else {
USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE); USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE);
} }
} }
// Handlers void uartPrint(serialPort_t *s, const char *str)
{
// USART1 Tx DMA Handler uint8_t ch;
while ((ch = *(str++))) {
uartWrite(s, ch);
}
}
// Handlers
// USART1 Tx DMA Handler
void DMA1_Channel4_IRQHandler(void) void DMA1_Channel4_IRQHandler(void)
{ {
serialPort_t *s = &serialPort1; serialPort_t *s = &serialPort1;

61
src/drv_uart.h
View file

@ -12,33 +12,40 @@
typedef enum portmode_t { typedef enum portmode_t {
MODE_RX = 1, MODE_RX = 1,
MODE_TX = 2, MODE_TX = 2,
MODE_RXTX = 3 MODE_RXTX = 3
} portmode_t; } portmode_t;
typedef struct { // FIXME this is a uart_t really. Move the generic properties into a separate structure (serialPort_t) and update the code to use it
uint32_t baudRate; typedef struct {
uint32_t rxBufferSize; portmode_t mode;
uint32_t txBufferSize; uint32_t baudRate;
volatile uint8_t *rxBuffer;
volatile uint8_t *txBuffer; uint32_t rxBufferSize;
uint32_t rxDMAPos; uint32_t txBufferSize;
uint32_t rxBufferHead; volatile uint8_t *rxBuffer;
uint32_t rxBufferTail; volatile uint8_t *txBuffer;
uint32_t txBufferHead; uint32_t rxBufferHead;
uint32_t txBufferTail; uint32_t rxBufferTail;
uint32_t txBufferHead;
DMA_Channel_TypeDef *rxDMAChannel; uint32_t txBufferTail;
DMA_Channel_TypeDef *txDMAChannel;
uint32_t rxDMAIrq; // FIXME rename callback type so something more generic
uint32_t txDMAIrq; uartReceiveCallbackPtr callback;
bool txDMAEmpty;
USART_TypeDef *USARTx; // FIXME these are uart specific and do not belong in here
DMA_Channel_TypeDef *rxDMAChannel;
uartReceiveCallbackPtr callback; DMA_Channel_TypeDef *txDMAChannel;
portmode_t mode;
} serialPort_t; uint32_t rxDMAIrq;
uint32_t txDMAIrq;
serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode);
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); void uartChangeBaud(serialPort_t *s, uint32_t baudRate);
bool isUartAvailable(serialPort_t *s); bool isUartAvailable(serialPort_t *s);
bool isUartTransmitEmpty(serialPort_t *s); bool isUartTransmitEmpty(serialPort_t *s);

7
src/gps.c
View file

@ -8,7 +8,6 @@
const uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 }; const uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 };
static void GPS_NewData(uint16_t c); static void GPS_NewData(uint16_t c);
static void GPS_set_pids(void);
static void gpsPrint(const char *str); static void gpsPrint(const char *str);
static const char * const gpsInitStrings[] = { 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) void GPS_reset_nav(void)
{ {
int i; int i;
@ -406,8 +405,8 @@ void GPS_reset_nav(void)
} }
} }
//Get the relevant P I D values and set the PID controllers // Get the relevant P I D values and set the PID controllers
static void GPS_set_pids(void) void GPS_set_pids(void)
{ {
posholdPID_PARAM.kP = (float)cfg.P8[PIDPOS] / 100.0f; posholdPID_PARAM.kP = (float)cfg.P8[PIDPOS] / 100.0f;
posholdPID_PARAM.kI = (float)cfg.I8[PIDPOS] / 100.0f; posholdPID_PARAM.kI = (float)cfg.I8[PIDPOS] / 100.0f;

144
src/imu.c
View file

@ -2,6 +2,9 @@
#include "mw.h" #include "mw.h"
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; 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; int16_t acc_25deg = 0;
int32_t baroPressure = 0; int32_t baroPressure = 0;
int32_t baroTemperature = 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)); 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 // Use original baseflight angle calculation
// #define BASEFLIGHT_CALC // #define BASEFLIGHT_CALC
static float invG; static float invG;
@ -190,6 +255,7 @@ static void getEstimatedAttitude(void)
static float accLPF[3]; static float accLPF[3];
static uint32_t previousT; static uint32_t previousT;
uint32_t currentT = micros(); uint32_t currentT = micros();
uint32_t deltaT;
float scale, deltaGyroAngle[3]; float scale, deltaGyroAngle[3];
#ifndef BASEFLIGHT_CALC #ifndef BASEFLIGHT_CALC
float sqGZ; float sqGZ;
@ -198,8 +264,8 @@ static void getEstimatedAttitude(void)
float sqGX_sqGZ; float sqGX_sqGZ;
float invmagXZ; float invmagXZ;
#endif #endif
deltaT = currentT - previousT;
scale = (currentT - previousT) * gyro.scale; scale = deltaT * gyro.scale;
previousT = currentT; previousT = currentT;
// Initialization // Initialization
@ -284,6 +350,8 @@ static void getEstimatedAttitude(void)
} }
#endif #endif
acc_calc(deltaT); // rotate acc vector into earth frame
if (cfg.throttle_angle_correction) { if (cfg.throttle_angle_correction) {
int cosZ = EstG.V.Z / acc_1G * 100.0f; int cosZ = EstG.V.Z / acc_1G * 100.0f;
throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8; throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
@ -293,19 +361,6 @@ static void getEstimatedAttitude(void)
#ifdef BARO #ifdef BARO
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc) #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) int getEstimatedAltitude(void)
{ {
@ -315,10 +370,11 @@ int getEstimatedAltitude(void)
uint32_t dTime; uint32_t dTime;
int32_t error; int32_t error;
int32_t baroVel; int32_t baroVel;
int32_t accZ;
int32_t vel_tmp; int32_t vel_tmp;
static int32_t accZoffset = 0; int32_t BaroAlt_tmp;
float vel_calc;
static float vel = 0.0f; static float vel = 0.0f;
static float accAlt = 0.0f;
static int32_t lastBaroAlt; static int32_t lastBaroAlt;
dTime = currentT - previousT; dTime = currentT - previousT;
@ -329,53 +385,59 @@ int getEstimatedAltitude(void)
if (calibratingB > 0) { if (calibratingB > 0) {
baroGroundPressure = baroPressureSum / (cfg.baro_tab_size - 1); baroGroundPressure = baroPressureSum / (cfg.baro_tab_size - 1);
calibratingB--; calibratingB--;
vel = 0;
accAlt = 0;
} }
// pressure relative to ground pressure with temperature compensation (fast!) // pressure relative to ground pressure with temperature compensation (fast!)
// baroGroundPressure is not supposed to be 0 here // baroGroundPressure is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp // 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 BaroAlt_tmp = 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 = 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 //P
error = constrain(AltHold - EstAlt, -300, 300); error = constrain(AltHold - EstAlt, -300, 300);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain(((cfg.P8[PIDALT] * error) / 128), -150, +150); BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -150, +150);
//I //I
errorAltitudeI += (cfg.I8[PIDALT] * error) / 64; errorAltitudeI += cfg.I8[PIDALT] * error / 64;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000); errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
BaroPID += errorAltitudeI / 512; // I in range +/-60 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) baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
accZ = (accSmooth[ROLL] * EstG.V.X + accSmooth[PITCH] * EstG.V.Y + accSmooth[YAW] * EstG.V.Z) * invG; lastBaroAlt = BaroAlt;
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 = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s 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). // 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 // 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 // D
vel_tmp = vel; vel_tmp = vel;
vel_tmp = applyDeadband(vel_tmp, 5); vel_tmp = applyDeadband(vel_tmp, 5);
vario = vel_tmp; 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; return 1;
} }

20
src/main.c
View file

@ -120,10 +120,17 @@ int main(void)
// Check battery type/voltage // Check battery type/voltage
if (feature(FEATURE_VBAT)) 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(); previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = 400; calibratingA = 400;
@ -134,6 +141,13 @@ int main(void)
// loopy // loopy
while (1) { while (1) {
loop(); loop();
#ifdef SOFTSERIAL_19200_LOOPBACK
while (serialAvailable(&softSerialPorts[0])) {
uint8_t b = serialReadByte(&softSerialPorts[0]);
uartWrite(core.mainport, b);
};
#endif
} }
} }

11
src/mw.h
View file

@ -164,10 +164,13 @@ typedef struct config_t {
// sensor-related stuff // 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 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 uint8_t baro_tab_size; // size of baro filter array
float baro_noise_lpf; // additional LPF to reduce baro noise 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 uint16_t activate[CHECKBOXITEMS]; // activate switches
@ -325,7 +328,10 @@ extern int16_t failsafeCnt;
extern int16_t debug[4]; extern int16_t debug[4];
extern int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; extern int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
extern int32_t accSum[3];
extern uint16_t acc_1G; extern uint16_t acc_1G;
extern uint32_t accTimeSum;
extern int accSumCount;
extern uint32_t currentTime; extern uint32_t currentTime;
extern uint32_t previousTime; extern uint32_t previousTime;
extern uint16_t cycleTime; extern uint16_t cycleTime;
@ -451,6 +457,7 @@ void cliProcess(void);
void gpsInit(uint32_t baudrate); void gpsInit(uint32_t baudrate);
void GPS_reset_home_position(void); void GPS_reset_home_position(void);
void GPS_reset_nav(void); void GPS_reset_nav(void);
void GPS_set_pids(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon); void GPS_set_next_wp(int32_t* lat, int32_t* lon);
int32_t wrap_18000(int32_t error); int32_t wrap_18000(int32_t error);

24
src/sensors.c
View file

@ -64,14 +64,14 @@ retry:
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050; accHardware = ACC_MPU6050;
if (mcfg.acc_hardware == ACC_MPU6050) if (mcfg.acc_hardware == ACC_MPU6050)
break; break;
} }
; // fallthrough ; // fallthrough
case 3: // MMA8452 #ifndef OLIMEXINO
#ifndef OLIMEXINO case 3: // MMA8452
if (mma8452Detect(&acc)) { if (mma8452Detect(&acc)) {
accHardware = ACC_MMA8452; accHardware = ACC_MMA8452;
if (mcfg.acc_hardware == ACC_MMA8452) if (mcfg.acc_hardware == ACC_MMA8452)
break; break;
} }
#endif #endif
@ -289,7 +289,7 @@ int Baro_update(void)
return 0; return 0;
baroDeadline = currentTime; baroDeadline = currentTime;
if (state) { if (state) {
baro.get_up(); baro.get_up();
baro.start_ut(); baro.start_ut();
@ -470,21 +470,21 @@ int Mag_getADC(void)
writeEEPROM(1, true); writeEEPROM(1, true);
} }
} }
return 1; return 1;
} }
#endif #endif
#ifdef SONAR #ifdef SONAR
void Sonar_init(void) void Sonar_init(void)
{ {
hcsr04_init(sonar_rc78); hcsr04_init(sonar_rc78);
sensorsSet(SENSOR_SONAR); sensorsSet(SENSOR_SONAR);
sonarAlt = 0; sonarAlt = 0;
} }
void Sonar_update(void) void Sonar_update(void)
{ {
hcsr04_get_distance(&sonarAlt); hcsr04_get_distance(&sonarAlt);
} }

10
src/serial.c
View file

@ -439,8 +439,14 @@ static void evaluateCommand(void)
break; break;
case MSP_RAW_IMU: case MSP_RAW_IMU:
headSerialReply(18); headSerialReply(18);
for (i = 0; i < 3; i++) // Retarded hack until multiwiidorks start using real units for sensor data
serialize16(accSmooth[i]); 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++) for (i = 0; i < 3; i++)
serialize16(gyroData[i]); serialize16(gyroData[i]);
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)