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_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

30
baseflight.uvproj
View file

@ -647,6 +647,16 @@
<FileType>1</FileType>
<FilePath>.\src\drv_spi.c</FilePath>
</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>
</Group>
<Group>
@ -1468,6 +1478,16 @@
<FileType>1</FileType>
<FilePath>.\src\drv_spi.c</FilePath>
</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>
</Group>
<Group>
@ -2473,6 +2493,16 @@
<FileType>1</FileType>
<FilePath>.\src\drv_spi.c</FilePath>
</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>
</Group>
<Group>

16
src/board.h
View file

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

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_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 },

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

12
src/drv_i2c_soft.c
View file

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

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)
{
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

182
src/drv_pwm.c
View file

@ -2,45 +2,7 @@
#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
@ -72,27 +34,7 @@
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;
@ -195,28 +137,7 @@ static const uint8_t * const hardwareMaps[] = {
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);
}
#define PWM_TIMER_MHZ 1
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
{
@ -250,7 +171,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
}
}
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;
@ -277,7 +198,7 @@ static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value)
{
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);
pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value);
// Needed only on TIM1
@ -302,89 +223,22 @@ static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value
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];
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;
const timerHardware_t *timerHardwarePtr = &(timerHardware[port]);
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;
}
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;
}
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;
@ -459,8 +313,8 @@ bool pwmInit(drv_pwm_config_t *init)
if (setup[i] == 0xFF) // terminator
break;
#ifdef OLIMEXINO
// PWM2 is connected to LED2 on the board and cannot be connected.
#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
@ -469,6 +323,12 @@ bool pwmInit(drv_pwm_config_t *init)
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;

9
src/drv_pwm.h
View file

@ -36,14 +36,7 @@ enum {
MAX_PORTS
};
typedef struct {
TIM_TypeDef *tim;
GPIO_TypeDef *gpio;
uint32_t pin;
uint8_t channel;
uint8_t irq;
uint8_t outputEnable;
} pwmHardware_t;
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);

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

10
src/drv_uart.c
View file

@ -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);
@ -241,6 +241,14 @@ void uartWrite(serialPort_t *s, uint8_t ch)
}
}
void uartPrint(serialPort_t *s, const char *str)
{
uint8_t ch;
while ((ch = *(str++))) {
uartWrite(s, ch);
}
}
// Handlers
// USART1 Tx DMA Handler

17
src/drv_uart.h
View file

@ -15,27 +15,34 @@ typedef enum portmode_t {
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 rxDMAPos;
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;
bool txDMAEmpty;
USART_TypeDef *USARTx;
uartReceiveCallbackPtr callback;
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);

7
src/gps.c
View file

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

138
src/imu.c
View file

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

14
src/main.c
View file

@ -122,7 +122,14 @@ int main(void)
if (feature(FEATURE_VBAT))
batteryInit();
#ifdef SOFTSERIAL_19200_LOOPBACK
serialInit(19200);
setupSoftSerial1(19200);
uartPrint(core.mainport, "LOOPBACK 19200 ENABLED");
#else
serialInit(mcfg.serial_baudrate);
#endif
previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
@ -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
}
}

11
src/mw.h
View file

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

2
src/sensors.c
View file

@ -67,8 +67,8 @@ retry:
break;
}
; // fallthrough
case 3: // MMA8452
#ifndef OLIMEXINO
case 3: // MMA8452
if (mma8452Detect(&acc)) {
accHardware = ACC_MMA8452;
if (mcfg.acc_hardware == ACC_MMA8452)

6
src/serial.c
View file

@ -439,8 +439,14 @@ static void evaluateCommand(void)
break;
case MSP_RAW_IMU:
headSerialReply(18);
// 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++)