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

cleaned up bmp085 driver

Browse source 
added ms5611 driver
refactored pressure sensor subsystem to allow multiple sensors
couple changes in PWM driver to make motor/servo arrangement for airplane mode more intuitive
moved MAX_MOTORS/MAX_SERVOS etc into drv_pwm.h
staring to merge back in airplane/flyingwing mixes
fix for tri servo display - mwc moved it to servo[5] again, gui was broken, function not.
will probably implement custom mixer soon (motors only, no servos)


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@198 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-08-20 10:25:10 +00:00
parent ee76242525
commit e6cb4a0b1c
15 changed files with 3042 additions and 2698 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -59,6 +59,7 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \
NAZE_SRC = drv_adc.c \ NAZE_SRC = drv_adc.c \
drv_adxl345.c \ drv_adxl345.c \
drv_bmp085.c \ drv_bmp085.c \
drv_ms5611.c \
drv_hcsr04.c \ drv_hcsr04.c \
drv_hmc5883l.c \ drv_hmc5883l.c \
drv_ledring.c \ drv_ledring.c \

15
baseflight.uvproj
View file

@ -630,6 +630,11 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>.\src\drv_l3g4200d.c</FilePath> <FilePath>.\src\drv_l3g4200d.c</FilePath>
</File> </File>
<File>
<FileName>drv_ms5611.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_ms5611.c</FilePath>
</File>
</Files> </Files>
</Group> </Group>
<Group> <Group>
@ -1424,6 +1429,11 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>.\src\drv_l3g4200d.c</FilePath> <FilePath>.\src\drv_l3g4200d.c</FilePath>
</File> </File>
<File>
<FileName>drv_ms5611.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_ms5611.c</FilePath>
</File>
</Files> </Files>
</Group> </Group>
<Group> <Group>
@ -2402,6 +2412,11 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>.\src\drv_l3g4200d.c</FilePath> <FilePath>.\src\drv_l3g4200d.c</FilePath>
</File> </File>
<File>
<FileName>drv_ms5611.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_ms5611.c</FilePath>
</File>
</Files> </Files>
</Group> </Group>
<Group> <Group>

5063
obj/baseflight.hex
View file

File diff suppressed because it is too large Load diff

14
src/board.h
View file

@ -51,6 +51,7 @@ typedef enum {
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype typedef void (* sensorInitFuncPtr)(void); // sensor init prototype
typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef int32_t (* baroCalculateFuncPtr)(void); // baro calculation (returns altitude in cm based on static data collected)
typedef void (* uartReceiveCallbackPtr)(uint16_t data); // used by uart2 driver to return frames to app typedef void (* uartReceiveCallbackPtr)(uint16_t data); // used by uart2 driver to return frames to app
typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data
@ -62,6 +63,18 @@ typedef struct sensor_t
sensorReadFuncPtr temperature; sensorReadFuncPtr temperature;
} sensor_t; } sensor_t;
typedef struct baro_t
{
uint16_t ut_delay;
uint16_t up_delay;
uint16_t repeat_delay;
sensorInitFuncPtr start_ut;
sensorInitFuncPtr get_ut;
sensorInitFuncPtr start_up;
sensorInitFuncPtr get_up;
baroCalculateFuncPtr calculate;
} baro_t;
#define digitalHi(p, i) { p->BSRR = i; } #define digitalHi(p, i) { p->BSRR = i; }
#define digitalLo(p, i) { p->BRR = i; } #define digitalLo(p, i) { p->BRR = i; }
#define digitalToggle(p, i) { p->ODR ^= i; } #define digitalToggle(p, i) { p->ODR ^= i; }
@ -133,6 +146,7 @@ typedef struct sensor_t
#include "drv_adc.h" #include "drv_adc.h"
#include "drv_adxl345.h" #include "drv_adxl345.h"
#include "drv_bmp085.h" #include "drv_bmp085.h"
#include "drv_ms5611.h"
#include "drv_hmc5883l.h" #include "drv_hmc5883l.h"
#include "drv_i2c.h" #include "drv_i2c.h"
#include "drv_ledring.h" #include "drv_ledring.h"

2
src/cli.c
View file

@ -30,7 +30,7 @@ const char *mixerNames[] = {
"TRI", "QUADP", "QUADX", "BI", "TRI", "QUADP", "QUADX", "BI",
"GIMBAL", "Y6", "HEX6", "GIMBAL", "Y6", "HEX6",
"FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX", "FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX",
"AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4", NULL "AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4", "CUSTOM", NULL
}; };
// sync this with AvailableFeatures enum from board.h // sync this with AvailableFeatures enum from board.h

154
src/drv_bmp085.c
View file

@ -35,10 +35,8 @@ typedef struct {
uint8_t mode; uint8_t mode;
uint8_t chip_id, ml_version, al_version; uint8_t chip_id, ml_version, al_version;
uint8_t dev_addr; uint8_t dev_addr;
uint8_t sensortype;
int32_t param_b5; int32_t param_b5;
int16_t oversampling_setting; int16_t oversampling_setting;
int16_t smd500_t_resolution, smd500_masterclock;
} bmp085_t; } bmp085_t;
#define BMP085_I2C_ADDR 0x77 #define BMP085_I2C_ADDR 0x77
@ -64,8 +62,6 @@ typedef struct {
#define BMP085_ML_VERSION__MSK 0x0F #define BMP085_ML_VERSION__MSK 0x0F
#define BMP085_ML_VERSION__REG BMP085_VERSION_REG #define BMP085_ML_VERSION__REG BMP085_VERSION_REG
#define BMP085_AL_VERSION__POS 4 #define BMP085_AL_VERSION__POS 4
#define BMP085_AL_VERSION__LEN 4 #define BMP085_AL_VERSION__LEN 4
#define BMP085_AL_VERSION__MSK 0xF0 #define BMP085_AL_VERSION__MSK 0xF0
@ -79,16 +75,24 @@ typedef struct {
#define SMD500_PARAM_MI 3791 //calibration parameter #define SMD500_PARAM_MI 3791 //calibration parameter
static bmp085_t bmp085 = { { 0, } }; static bmp085_t bmp085 = { { 0, } };
static bmp085_t *p_bmp085 = &bmp085; /**< pointer to SMD500 / BMP085 device area */
static bool bmp085InitDone = false; static bool bmp085InitDone = false;
static uint16_t bmp085_ut; // static result of temperature measurement
static uint32_t bmp085_up; // static result of pressure measurement
static void bmp085_get_cal_param(void); static void bmp085_get_cal_param(void);
static void bmp085_start_ut(void);
static void bmp085_get_ut(void);
static void bmp085_start_up(void);
static void bmp085_get_up(void);
static int16_t bmp085_get_temperature(uint32_t ut);
static int32_t bmp085_get_pressure(uint32_t up);
static int32_t bmp085_calculate(void);
bool bmp085Init(void) bool bmp085Detect(baro_t *baro)
{ {
GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitTypeDef GPIO_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure; EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitTypeDef NVIC_InitStructure;
uint8_t data; uint8_t data;
if (bmp085InitDone) if (bmp085InitDone)
@ -119,49 +123,34 @@ bool bmp085Init(void)
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure); NVIC_Init(&NVIC_InitStructure);
delay(12); // datasheet says 10ms, we'll be careful and do 12. delay(20); // datasheet says 10ms, we'll be careful and do 20. this is after ms5611 driver kills us, so longer the better.
p_bmp085->sensortype = E_SENSOR_NOT_DETECTED; i2cRead(BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
p_bmp085->dev_addr = BMP085_I2C_ADDR; /* preset BMP085 I2C_addr */ bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
i2cRead(p_bmp085->dev_addr, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */ bmp085.oversampling_setting = 3;
p_bmp085->chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
p_bmp085->oversampling_setting = 3;
if (p_bmp085->chip_id == BMP085_CHIP_ID) { /* get bitslice */ if (bmp085.chip_id == BMP085_CHIP_ID) { /* get bitslice */
p_bmp085->sensortype = BOSCH_PRESSURE_BMP085; i2cRead(BMP085_I2C_ADDR, BMP085_VERSION_REG, 1, &data); /* read Version reg */
bmp085.ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */
i2cRead(p_bmp085->dev_addr, BMP085_VERSION_REG, 1, &data); /* read Version reg */ bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
p_bmp085->ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */
p_bmp085->al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
bmp085_get_cal_param(); /* readout bmp085 calibparam structure */ bmp085_get_cal_param(); /* readout bmp085 calibparam structure */
bmp085InitDone = true; bmp085InitDone = true;
baro->ut_delay = 4600;
baro->up_delay = 26000;
baro->repeat_delay = 5000;
baro->start_ut = bmp085_start_ut;
baro->get_ut = bmp085_get_ut;
baro->start_up = bmp085_start_up;
baro->get_up = bmp085_get_up;
baro->calculate = bmp085_calculate;
return true; return true;
} }
BARO_OFF;
return false; return false;
} }
int16_t bmp085_read_temperature(void)
{
convDone = false;
bmp085_start_ut();
if (!convDone)
convOverrun++;
return bmp085_get_temperature(bmp085_get_ut());
}
int32_t bmp085_read_pressure(void)
{
convDone = false;
bmp085_start_up();
if (!convDone)
convOverrun++;
return bmp085_get_pressure(bmp085_get_up());
}
// #define BMP_TEMP_OSS 4 // #define BMP_TEMP_OSS 4
static int16_t bmp085_get_temperature(uint32_t ut)
int16_t bmp085_get_temperature(uint32_t ut)
{ {
int16_t temperature; int16_t temperature;
int32_t x1, x2; int32_t x1, x2;
@ -169,12 +158,10 @@ int16_t bmp085_get_temperature(uint32_t ut)
static uint32_t temp; static uint32_t temp;
#endif #endif
if (p_bmp085->sensortype == BOSCH_PRESSURE_BMP085) { x1 = (((int32_t) ut - (int32_t) bmp085.cal_param.ac6) * (int32_t) bmp085.cal_param.ac5) >> 15;
x1 = (((int32_t) ut - (int32_t) p_bmp085->cal_param.ac6) * (int32_t) p_bmp085->cal_param.ac5) >> 15; x2 = ((int32_t) bmp085.cal_param.mc << 11) / (x1 + bmp085.cal_param.md);
x2 = ((int32_t) p_bmp085->cal_param.mc << 11) / (x1 + p_bmp085->cal_param.md); bmp085.param_b5 = x1 + x2;
p_bmp085->param_b5 = x1 + x2; temperature = ((bmp085.param_b5 + 8) >> 4); // temperature in 0.1°C
}
temperature = ((p_bmp085->param_b5 + 8) >> 4); // temperature in 0.1°C
#ifdef BMP_TEMP_OSS #ifdef BMP_TEMP_OSS
temp *= (1 << BMP_TEMP_OSS) - 1; // multiply the temperature variable by 3 - we have tau == 1/4 temp *= (1 << BMP_TEMP_OSS) - 1; // multiply the temperature variable by 3 - we have tau == 1/4
@ -186,31 +173,31 @@ int16_t bmp085_get_temperature(uint32_t ut)
#endif #endif
} }
int32_t bmp085_get_pressure(uint32_t up) static int32_t bmp085_get_pressure(uint32_t up)
{ {
int32_t pressure, x1, x2, x3, b3, b6; int32_t pressure, x1, x2, x3, b3, b6;
uint32_t b4, b7; uint32_t b4, b7;
b6 = p_bmp085->param_b5 - 4000; b6 = bmp085.param_b5 - 4000;
// *****calculate B3************ // *****calculate B3************
x1 = (b6 * b6) >> 12; x1 = (b6 * b6) >> 12;
x1 *= p_bmp085->cal_param.b2; x1 *= bmp085.cal_param.b2;
x1 >>= 11; x1 >>= 11;
x2 = (p_bmp085->cal_param.ac2 * b6); x2 = (bmp085.cal_param.ac2 * b6);
x2 >>= 11; x2 >>= 11;
x3 = x1 + x2; x3 = x1 + x2;
b3 = (((((int32_t)p_bmp085->cal_param.ac1) * 4 + x3) << p_bmp085->oversampling_setting) + 2) >> 2; b3 = (((((int32_t)bmp085.cal_param.ac1) * 4 + x3) << bmp085.oversampling_setting) + 2) >> 2;
// *****calculate B4************ // *****calculate B4************
x1 = (p_bmp085->cal_param.ac3 * b6) >> 13; x1 = (bmp085.cal_param.ac3 * b6) >> 13;
x2 = (p_bmp085->cal_param.b1 * ((b6 * b6) >> 12) ) >> 16; x2 = (bmp085.cal_param.b1 * ((b6 * b6) >> 12) ) >> 16;
x3 = ((x1 + x2) + 2) >> 2; x3 = ((x1 + x2) + 2) >> 2;
b4 = (p_bmp085->cal_param.ac4 * (uint32_t) (x3 + 32768)) >> 15; b4 = (bmp085.cal_param.ac4 * (uint32_t) (x3 + 32768)) >> 15;
b7 = ((uint32_t)(up - b3) * (50000 >> p_bmp085->oversampling_setting)); b7 = ((uint32_t)(up - b3) * (50000 >> bmp085.oversampling_setting));
if (b7 < 0x80000000) { if (b7 < 0x80000000) {
pressure = (b7 << 1) / b4; pressure = (b7 << 1) / b4;
} else { } else {
@ -226,15 +213,14 @@ int32_t bmp085_get_pressure(uint32_t up)
return pressure; return pressure;
} }
void bmp085_start_ut(void) static void bmp085_start_ut(void)
{ {
convDone = false; convDone = false;
i2cWrite(p_bmp085->dev_addr, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE); i2cWrite(BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
} }
uint16_t bmp085_get_ut(void) static void bmp085_get_ut(void)
{ {
uint16_t ut;
uint8_t data[2]; uint8_t data[2];
uint16_t timeout = 10000; uint16_t timeout = 10000;
@ -246,27 +232,25 @@ uint16_t bmp085_get_ut(void)
__NOP(); __NOP();
} }
#endif #endif
i2cRead(p_bmp085->dev_addr, BMP085_ADC_OUT_MSB_REG, 2, data); i2cRead(BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 2, data);
ut = (data[0] << 8) | data[1]; bmp085_ut = (data[0] << 8) | data[1];
return ut;
} }
void bmp085_start_up(void) static void bmp085_start_up(void)
{ {
uint8_t ctrl_reg_data; uint8_t ctrl_reg_data;
ctrl_reg_data = BMP085_P_MEASURE + (p_bmp085->oversampling_setting << 6); ctrl_reg_data = BMP085_P_MEASURE + (bmp085.oversampling_setting << 6);
convDone = false; convDone = false;
i2cWrite(p_bmp085->dev_addr, BMP085_CTRL_MEAS_REG, ctrl_reg_data); i2cWrite(BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, ctrl_reg_data);
} }
/** read out up for pressure conversion /** read out up for pressure conversion
depending on the oversampling ratio setting up can be 16 to 19 bit depending on the oversampling ratio setting up can be 16 to 19 bit
\return up parameter that represents the uncompensated pressure value \return up parameter that represents the uncompensated pressure value
*/ */
uint32_t bmp085_get_up(void) static void bmp085_get_up(void)
{ {
uint32_t up = 0;
uint8_t data[3]; uint8_t data[3];
uint16_t timeout = 10000; uint16_t timeout = 10000;
@ -278,31 +262,35 @@ uint32_t bmp085_get_up(void)
__NOP(); __NOP();
} }
#endif #endif
i2cRead(p_bmp085->dev_addr, BMP085_ADC_OUT_MSB_REG, 3, data); i2cRead(BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data);
up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - p_bmp085->oversampling_setting); bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - bmp085.oversampling_setting);
}
return up; static int32_t bmp085_calculate(void)
{
bmp085_get_temperature(bmp085_ut);
return bmp085_get_pressure(bmp085_up);
} }
static void bmp085_get_cal_param(void) static void bmp085_get_cal_param(void)
{ {
uint8_t data[22]; uint8_t data[22];
i2cRead(p_bmp085->dev_addr, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data); i2cRead(BMP085_I2C_ADDR, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data);
/*parameters AC1-AC6*/ /*parameters AC1-AC6*/
p_bmp085->cal_param.ac1 = (data[0] <<8) | data[1]; bmp085.cal_param.ac1 = (data[0] << 8) | data[1];
p_bmp085->cal_param.ac2 = (data[2] <<8) | data[3]; bmp085.cal_param.ac2 = (data[2] << 8) | data[3];
p_bmp085->cal_param.ac3 = (data[4] <<8) | data[5]; bmp085.cal_param.ac3 = (data[4] << 8) | data[5];
p_bmp085->cal_param.ac4 = (data[6] <<8) | data[7]; bmp085.cal_param.ac4 = (data[6] << 8) | data[7];
p_bmp085->cal_param.ac5 = (data[8] <<8) | data[9]; bmp085.cal_param.ac5 = (data[8] << 8) | data[9];
p_bmp085->cal_param.ac6 = (data[10] <<8) | data[11]; bmp085.cal_param.ac6 = (data[10] << 8) | data[11];
/*parameters B1,B2*/ /*parameters B1,B2*/
p_bmp085->cal_param.b1 = (data[12] <<8) | data[13]; bmp085.cal_param.b1 = (data[12] << 8) | data[13];
p_bmp085->cal_param.b2 = (data[14] <<8) | data[15]; bmp085.cal_param.b2 = (data[14] << 8) | data[15];
/*parameters MB,MC,MD*/ /*parameters MB,MC,MD*/
p_bmp085->cal_param.mb = (data[16] <<8) | data[17]; bmp085.cal_param.mb = (data[16] << 8) | data[17];
p_bmp085->cal_param.mc = (data[18] <<8) | data[19]; bmp085.cal_param.mc = (data[18] << 8) | data[19];
p_bmp085->cal_param.md = (data[20] <<8) | data[21]; bmp085.cal_param.md = (data[20] << 8) | data[21];
} }

14
src/drv_bmp085.h
View file

@ -1,15 +1,3 @@
#pragma once #pragma once
bool bmp085Init(void); bool bmp085Detect(baro_t *baro);
// polled versions
int16_t bmp085_read_temperature(void);
int32_t bmp085_read_pressure(void);
// interrupt versions
void bmp085_start_ut(void);
uint16_t bmp085_get_ut(void);
void bmp085_start_up(void);
uint32_t bmp085_get_up(void);
int16_t bmp085_get_temperature(uint32_t ut);
int32_t bmp085_get_pressure(uint32_t up);

180
src/drv_ms5611.c Normal file
View file

@ -0,0 +1,180 @@
#include "board.h"
// MS5611, Standard address 0x77
#define MS5611_ADDR 0x77
// Autodetect: turn off BMP085 while initializing ms5611 and check PROM crc to confirm device
#define BMP085_OFF digitalLo(BARO_GPIO, BARO_PIN);
#define BMP085_ON digitalHi(BARO_GPIO, BARO_PIN);
#define CMD_RESET 0x1E // ADC reset command
#define CMD_ADC_READ 0x00 // ADC read command
#define CMD_ADC_CONV 0x40 // ADC conversion command
#define CMD_ADC_D1 0x00 // ADC D1 conversion
#define CMD_ADC_D2 0x10 // ADC D2 conversion
#define CMD_ADC_256 0x00 // ADC OSR=256
#define CMD_ADC_512 0x02 // ADC OSR=512
#define CMD_ADC_1024 0x04 // ADC OSR=1024
#define CMD_ADC_2048 0x06 // ADC OSR=2048
#define CMD_ADC_4096 0x08 // ADC OSR=4096
#define CMD_PROM_RD 0xA0 // Prom read command
#define PROM_NB 8
static void ms5611_reset(void);
static uint16_t ms5611_prom(int8_t coef_num);
static int8_t ms5611_crc(uint16_t *prom);
static uint32_t ms5611_read_adc(void);
static void ms5611_start_ut(void);
static void ms5611_get_ut(void);
static void ms5611_start_up(void);
static void ms5611_get_up(void);
static int32_t ms5611_calculate(void);
static uint32_t ms5611_ut; // static result of temperature measurement
static uint32_t ms5611_up; // static result of pressure measurement
static uint16_t ms5611_c[PROM_NB]; // on-chip ROM
static uint8_t ms5611_osr = CMD_ADC_4096;
bool ms5611Detect(baro_t *baro)
{
GPIO_InitTypeDef GPIO_InitStructure;
bool ack = false;
uint8_t sig;
int i;
// PC13 (BMP085's XCLR reset input, which we use to disable it)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOC, &GPIO_InitStructure);
BMP085_OFF;
delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms
// BMP085 is disabled. If we have a MS5611, it will reply. if no reply, means either
// we have BMP085 or no baro at all.
ack = i2cRead(MS5611_ADDR, CMD_PROM_RD, 1, &sig);
if (!ack)
return false;
ms5611_reset();
// read all coefficients
for (i = 0; i < PROM_NB; i++)
ms5611_c[i] = ms5611_prom(i);
// check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line!
if (ms5611_crc(ms5611_c) != 0)
return false;
// TODO prom + CRC
baro->ut_delay = 10000;
baro->up_delay = 10000;
baro->repeat_delay = 4000;
baro->start_ut = ms5611_start_ut;
baro->get_ut = ms5611_get_ut;
baro->start_up = ms5611_start_up;
baro->get_up = ms5611_get_up;
baro->calculate = ms5611_calculate;
return true;
}
static void ms5611_reset(void)
{
i2cWrite(MS5611_ADDR, CMD_RESET, 1);
delayMicroseconds(2800);
}
static uint16_t ms5611_prom(int8_t coef_num)
{
uint8_t rxbuf[2] = { 0, 0 };
i2cRead(MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command
return rxbuf[0] << 8 | rxbuf[1];
}
static int8_t ms5611_crc(uint16_t *prom)
{
int32_t i, j;
uint32_t res = 0;
uint8_t zero = 1;
uint8_t crc = prom[7] & 0xF;
prom[7] &= 0xFF00;
// if eeprom is all zeros, we're probably fucked - BUT this will return valid CRC lol
for (i = 0; i < 8; i++) {
if (prom[i] != 0)
zero = 0;
}
if (zero)
return -1;
for (i = 0; i < 16; i++) {
if (i & 1)
res ^= ((prom[i >> 1]) & 0x00FF);
else
res ^= (prom[i >> 1] >> 8);
for (j = 8; j > 0; j--) {
if (res & 0x8000)
res ^= 0x1800;
res <<= 1;
}
}
prom[7] |= crc;
if (crc == ((res >> 12) & 0xF))
return 0;
return -1;
}
static uint32_t ms5611_read_adc(void)
{
uint8_t rxbuf[3];
i2cRead(MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC
return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
}
static void ms5611_start_ut(void)
{
i2cWrite(MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
}
static void ms5611_get_ut(void)
{
ms5611_ut = ms5611_read_adc();
}
static void ms5611_start_up(void)
{
i2cWrite(MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start!
}
static void ms5611_get_up(void)
{
ms5611_up = ms5611_read_adc();
}
static int32_t ms5611_calculate(void)
{
int32_t temperature, off2 = 0, sens2 = 0, delt;
int32_t pressure;
int32_t dT = ms5611_ut - ((uint32_t)ms5611_c[5] << 8);
int64_t off = ((uint32_t)ms5611_c[2] << 16) + (((int64_t)dT * ms5611_c[4]) >> 7);
int64_t sens = ((uint32_t)ms5611_c[1] << 15) + (((int64_t)dT * ms5611_c[3]) >> 8);
temperature = 2000 + (((int64_t)dT * ms5611_c[6]) >> 23);
if (temperature < 2000) { // temperature lower than 20degC
delt = temperature - 2000;
delt = delt * delt;
off2 = (5 * delt) >> 1;
sens2 = (5 * delt) >> 2;
if (temperature < -1500) { // temperature lower than -15degC
delt = temperature + 1500;
delt = delt * delt;
off2 += 7 * delt;
sens2 += (11 * delt) >> 1;
}
}
off -= off2;
sens -= sens2;
pressure = (((ms5611_up * sens ) >> 21) - off) >> 15;
return pressure;
}

3
src/drv_ms5611.h Normal file
View file

@ -0,0 +1,3 @@
#pragma once
bool ms5611Detect(baro_t *baro);

65
src/drv_pwm.c
View file

@ -139,15 +139,6 @@ enum {
TYPE_S = 0x80, TYPE_S = 0x80,
}; };
typedef struct {
uint8_t port;
uint8_t type;
} pwmPortConfig_t;
#define MAX_MOTORS 12
#define MAX_SERVOS 8
#define MAX_INPUTS 8
static pwmPortData_t pwmPorts[MAX_PORTS]; static pwmPortData_t pwmPorts[MAX_PORTS];
static uint16_t captures[MAX_INPUTS]; static uint16_t captures[MAX_INPUTS];
static pwmPortData_t *motors[MAX_MOTORS]; static pwmPortData_t *motors[MAX_MOTORS];
@ -159,68 +150,68 @@ static uint8_t numInputs = 0;
extern int16_t failsafeCnt; extern int16_t failsafeCnt;
static const uint8_t multiPPM[] = { static const uint8_t multiPPM[] = {
PWM1 | TYPE_IP, PWM1 | TYPE_IP, // PPM input
PWM9 | TYPE_M, // Swap to servo if needed PWM9 | TYPE_M, // Swap to servo if needed
PWM10 | TYPE_M, // Swap to servo if needed PWM10 | TYPE_M, // Swap to servo if needed
PWM11 | TYPE_M, PWM11 | TYPE_M,
PWM12 | TYPE_M, PWM12 | TYPE_M,
PWM13 | TYPE_M, PWM13 | TYPE_M,
PWM14 | TYPE_M, PWM14 | TYPE_M,
PWM5 | TYPE_M, // Swap to servo if needed PWM5 | TYPE_M, // Swap to servo if needed
PWM6 | TYPE_M, // Swap to servo if needed PWM6 | TYPE_M, // Swap to servo if needed
PWM7 | TYPE_M, // Swap to servo if needed PWM7 | TYPE_M, // Swap to servo if needed
PWM8 | TYPE_M, // Swap to servo if needed PWM8 | TYPE_M, // Swap to servo if needed
0xFF 0xFF
}; };
static const uint8_t multiPWM[] = { static const uint8_t multiPWM[] = {
PWM1 | TYPE_IW, PWM1 | TYPE_IW, // input #1
PWM2 | TYPE_IW, PWM2 | TYPE_IW,
PWM3 | TYPE_IW, PWM3 | TYPE_IW,
PWM4 | TYPE_IW, PWM4 | TYPE_IW,
PWM5 | TYPE_IW, PWM5 | TYPE_IW,
PWM6 | TYPE_IW, PWM6 | TYPE_IW,
PWM7 | TYPE_IW, PWM7 | TYPE_IW,
PWM8 | TYPE_IW, PWM8 | TYPE_IW, // input #8
PWM9 | TYPE_M, // Swap to servo if needed PWM9 | TYPE_M, // motor #1 or servo #1 (swap to servo if needed)
PWM10 | TYPE_M, // Swap to servo if needed PWM10 | TYPE_M, // motor #2 or servo #2 (swap to servo if needed)
PWM11 | TYPE_M, PWM11 | TYPE_M, // motor #1 or #3
PWM12 | TYPE_M, PWM12 | TYPE_M,
PWM13 | TYPE_M, PWM13 | TYPE_M,
PWM14 | TYPE_M, PWM14 | TYPE_M, // motor #4 or #6
0xFF 0xFF
}; };
static const uint8_t airPPM[] = { static const uint8_t airPPM[] = {
PWM1 | TYPE_IP, PWM1 | TYPE_IP, // PPM input
PWM5 | TYPE_S, PWM9 | TYPE_M, // motor #1
PWM6 | TYPE_S, PWM10 | TYPE_M, // motor #2
PWM7 | TYPE_S, PWM11 | TYPE_S, // servo #1
PWM8 | TYPE_S,
PWM9 | TYPE_M,
PWM10 | TYPE_M,
PWM11 | TYPE_S,
PWM12 | TYPE_S, PWM12 | TYPE_S,
PWM13 | TYPE_S, PWM13 | TYPE_S,
PWM14 | TYPE_S, PWM14 | TYPE_S, // servo #4
PWM5 | TYPE_S, // servo #5
PWM6 | TYPE_S,
PWM7 | TYPE_S,
PWM8 | TYPE_S, // servo #8
0xFF 0xFF
}; };
static const uint8_t airPWM[] = { static const uint8_t airPWM[] = {
PWM1 | TYPE_IW, PWM1 | TYPE_IW, // input #1
PWM2 | TYPE_IW, PWM2 | TYPE_IW,
PWM3 | TYPE_IW, PWM3 | TYPE_IW,
PWM4 | TYPE_IW, PWM4 | TYPE_IW,
PWM5 | TYPE_IW, PWM5 | TYPE_IW,
PWM6 | TYPE_IW, PWM6 | TYPE_IW,
PWM7 | TYPE_IW, PWM7 | TYPE_IW,
PWM8 | TYPE_IW, PWM8 | TYPE_IW, // input #8
PWM9 | TYPE_M, PWM9 | TYPE_M, // motor #1
PWM10 | TYPE_M, PWM10 | TYPE_M, // motor #2
PWM11 | TYPE_S, PWM11 | TYPE_S, // servo #1
PWM12 | TYPE_S, PWM12 | TYPE_S,
PWM13 | TYPE_S, PWM13 | TYPE_S,
PWM14 | TYPE_S, PWM14 | TYPE_S, // servo #4
0xFF 0xFF
}; };

4
src/drv_pwm.h
View file

@ -1,5 +1,9 @@
#pragma once #pragma once
#define MAX_MOTORS 12
#define MAX_SERVOS 8
#define MAX_INPUTS 8
typedef struct drv_pwm_config_t { typedef struct drv_pwm_config_t {
bool enableInput; bool enableInput;
bool usePPM; bool usePPM;

1
src/main.c
View file

@ -26,7 +26,6 @@ int main(void)
#endif #endif
#if 0 #if 0
// using this to write asm for bootloader :) // using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO

142
src/mixer.c
View file

@ -3,13 +3,27 @@
static uint8_t numberMotor = 4; static uint8_t numberMotor = 4;
uint8_t useServo = 0; uint8_t useServo = 0;
int16_t motor[8];
int16_t motor[MAX_MOTORS];
int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
typedef struct {
uint8_t enabled; // is this mix channel enabled
float throttle; // proportion of throttle affect
float pitch;
float roll;
float yaw;
} mixerPower_t;
static mixerPower_t customMixer[12];
void mixerInit(void) void mixerInit(void)
{ {
int i;
// enable servos for mixes that require them. note, this shifts motor counts. // enable servos for mixes that require them. note, this shifts motor counts.
if (cfg.mixerConfiguration == MULTITYPE_BI || cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_GIMBAL || cfg.mixerConfiguration == MULTITYPE_FLYING_WING) if (cfg.mixerConfiguration == MULTITYPE_BI || cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_FLYING_WING ||
cfg.mixerConfiguration == MULTITYPE_AIRPLANE || cfg.mixerConfiguration == MULTITYPE_GIMBAL)
useServo = 1; useServo = 1;
// if we want camstab/trig, that also enabled servos. this is kinda lame. maybe rework feature bits later. // if we want camstab/trig, that also enabled servos. this is kinda lame. maybe rework feature bits later.
if (feature(FEATURE_SERVO_TILT)) if (feature(FEATURE_SERVO_TILT))
@ -19,12 +33,16 @@ void mixerInit(void)
case MULTITYPE_GIMBAL: case MULTITYPE_GIMBAL:
numberMotor = 0; numberMotor = 0;
break; break;
case MULTITYPE_AIRPLANE:
case MULTITYPE_FLYING_WING: case MULTITYPE_FLYING_WING:
numberMotor = 1; numberMotor = 1;
break; break;
case MULTITYPE_BI: case MULTITYPE_BI:
numberMotor = 2; numberMotor = 2;
break; break;
case MULTITYPE_TRI: case MULTITYPE_TRI:
numberMotor = 3; numberMotor = 3;
break; break;
@ -47,6 +65,14 @@ void mixerInit(void)
case MULTITYPE_OCTOFLATX: case MULTITYPE_OCTOFLATX:
numberMotor = 8; numberMotor = 8;
break; break;
case MULTITYPE_CUSTOM:
numberMotor = 0;
for (i = 0; i < MAX_MOTORS; i++) {
if (customMixer[i].enabled)
numberMotor++;
}
break;
} }
} }
@ -55,15 +81,31 @@ void writeServos(void)
if (!useServo) if (!useServo)
return; return;
if (cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_BI) { switch (cfg.mixerConfiguration) {
/* One servo on Motor #4 */ case MULTITYPE_BI:
pwmWriteServo(0, servo[4]); pwmWriteServo(0, servo[4]);
if (cfg.mixerConfiguration == MULTITYPE_BI)
pwmWriteServo(1, servo[5]); pwmWriteServo(1, servo[5]);
} else { break;
/* Two servos for camstab or FLYING_WING */
pwmWriteServo(0, servo[0]); case MULTITYPE_TRI:
pwmWriteServo(1, servo[1]); pwmWriteServo(0, servo[5]);
break;
case MULTITYPE_AIRPLANE:
break;
case MULTITYPE_FLYING_WING:
break;
default:
// Two servos for SERVO_TILT, if enabled
if (feature(FEATURE_SERVO_TILT)) {
pwmWriteServo(0, servo[0]);
pwmWriteServo(1, servo[1]);
}
break;
} }
} }
@ -87,7 +129,57 @@ void writeAllMotors(int16_t mc)
writeMotors(); writeMotors();
} }
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL] * X + axisPID[PITCH] * Y + cfg.yaw_direction * axisPID[YAW] * Z #define PIDMIX(R, P, Y) rcCommand[THROTTLE] + axisPID[ROLL] * R + axisPID[PITCH] * P + cfg.yaw_direction * axisPID[YAW] * Y
static void airplaneMixer(void)
{
#if 0
uint16_t servomid[8];
int16_t flaperons[2] = { 0, 0 };
for (i = 0; i < 8; i++) {
servomid[i] = 1500 + cfg.servotrim[i]; // servo center is 1500?
}
if (!f.ARMED)
motor[0] = cfg.mincommand; // Kill throttle when disarmed
else
motor[0] = rcData[THROTTLE];
if (cfg.flaperons) {
}
if (cfg.flaps) {
int16_t flap = 1500 - constrain(rcData[cfg.flaps], cfg.servoendpoint_low[2], cfg.servoendpoint_high[2]);
static int16_t slowFlaps = flap;
if (cfg.flapspeed) {
if (slowFlaps < flap) {
slowFlaps += cfg.flapspeed;
} else if (slowFlaps > flap) {
slowFlaps -= cfg.flapspeed;
}
} else {
slowFlaps = flap;
}
servo[2] = servomid[2] + (slowFlaps * cfg.servoreverse[2]);
}
if (f.PASSTHRU_MODE) { // Direct passthru from RX
servo[3] = servomid[3] + ((rcCommand[ROLL] + flapperons[0]) * cfg.servoreverse[3]); // Wing 1
servo[4] = servomid[4] + ((rcCommand[ROLL] + flapperons[1]) * cfg.servoreverse[4]); // Wing 2
servo[5] = servomid[5] + (rcCommand[YAW] * cfg.servoreverse[5]); // Rudder
servo[6] = servomid[6] + (rcCommand[PITCH] * cfg.servoreverse[6]); // Elevator
} else { // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
servo[3] = (servomid[3] + ((axisPID[ROLL] + flapperons[0]) * cfg.servoreverse[3])); // Wing 1
servo[4] = (servomid[4] + ((axisPID[ROLL] + flapperons[1]) * cfg.servoreverse[4])); // Wing 2
servo[5] = (servomid[5] + (axisPID[YAW] * cfg.servoreverse[5])); // Rudder
servo[6] = (servomid[6] + (axisPID[PITCH] * cfg.servoreverse[6])); // Elevator
}
#endif
}
void mixTable(void) void mixTable(void)
{ {
@ -112,7 +204,7 @@ void mixTable(void)
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT
servo[4] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR servo[5] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
break; break;
case MULTITYPE_QUADP: case MULTITYPE_QUADP:
@ -155,21 +247,12 @@ void mixTable(void)
break; break;
case MULTITYPE_HEX6X: case MULTITYPE_HEX6X:
#if 0
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
motor[1] = PIDMIX(-1 / 2, -1 / 2, +1); //FRONT_R
motor[2] = PIDMIX(+1 / 2, +1 / 2, -1); //REAR_L
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
motor[4] = PIDMIX(-1, +0, -1); //RIGHT
motor[5] = PIDMIX(+1, +0, +1); //LEFT
#else
motor[0] = PIDMIX(-4/5,+9/10,+1); //REAR_R motor[0] = PIDMIX(-4/5,+9/10,+1); //REAR_R
motor[1] = PIDMIX(-4/5,-9/10,+1); //FRONT_R motor[1] = PIDMIX(-4/5,-9/10,+1); //FRONT_R
motor[2] = PIDMIX(+4/5,+9/10,-1); //REAR_L motor[2] = PIDMIX(+4/5,+9/10,-1); //REAR_L
motor[3] = PIDMIX(+4/5,-9/10,-1); //FRONT_L motor[3] = PIDMIX(+4/5,-9/10,-1); //FRONT_L
motor[4] = PIDMIX(-4/5 ,+0 ,-1); //RIGHT motor[4] = PIDMIX(-4/5 ,+0 ,-1); //RIGHT
motor[5] = PIDMIX(+4/5 ,+0 ,+1); //LEFT motor[5] = PIDMIX(+4/5 ,+0 ,+1); //LEFT
#endif
break; break;
case MULTITYPE_OCTOX8: case MULTITYPE_OCTOX8:
@ -216,6 +299,23 @@ void mixTable(void)
servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max); servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max); servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
break; break;
case MULTITYPE_AIRPLANE:
airplaneMixer();
break;
case MULTITYPE_FLYING_WING:
motor[0] = rcCommand[THROTTLE];
if (f.PASSTHRU_MODE) { // do not use sensors for correction, simple 2 channel mixing
int p = 0, r = 0;
servo[0] = p * (rcData[PITCH] - cfg.midrc) + r * (rcData[ROLL] - cfg.midrc);
servo[1] = p * (rcData[PITCH] - cfg.midrc) + r * (rcData[ROLL] - cfg.midrc);
} else { // use sensors to correct (gyro only or gyro+acc)
int p = 0, r = 0;
servo[0] = p * axisPID[PITCH] + r * axisPID[ROLL];
servo[1] = p * axisPID[PITCH] + r * axisPID[ROLL];
}
break;
} }
// do camstab // do camstab

27
src/mw.h
View file

@ -30,14 +30,15 @@ typedef enum MultiType
MULTITYPE_FLYING_WING = 8, // UNSUPPORTED, do not select! MULTITYPE_FLYING_WING = 8, // UNSUPPORTED, do not select!
MULTITYPE_Y4 = 9, MULTITYPE_Y4 = 9,
MULTITYPE_HEX6X = 10, MULTITYPE_HEX6X = 10,
MULTITYPE_OCTOX8 = 11, MULTITYPE_OCTOX8 = 11, // Java GUI is same for the next 3 configs
MULTITYPE_OCTOFLATP = 12, // the GUI is the same for all 8 motor configs MULTITYPE_OCTOFLATP = 12, // MultiWinGui shows this differently
MULTITYPE_OCTOFLATX = 13, // the GUI is the same for all 8 motor configs MULTITYPE_OCTOFLATX = 13, // MultiWinGui shows this differently
MULTITYPE_AIRPLANE = 14, MULTITYPE_AIRPLANE = 14, // airplane / singlecopter / dualcopter
MULTITYPE_HELI_120_CCPM = 15, // simple model MULTITYPE_HELI_120_CCPM = 15,
MULTITYPE_HELI_90_DEG = 16, // simple model MULTITYPE_HELI_90_DEG = 16,
MULTITYPE_VTAIL4 = 17, MULTITYPE_VTAIL4 = 17,
MULTITYPE_LAST = 18 MULTITYPE_CUSTOM = 18, // no current GUI displays this
MULTITYPE_LAST = 19
} MultiType; } MultiType;
typedef enum GimbalFlags { typedef enum GimbalFlags {
@ -148,6 +149,8 @@ typedef struct config_t {
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz) uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz)
uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz) uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz)
int16_t servotrim[8]; // Adjust Servo MID Offset & Swash angles
int8_t servoreverse[8]; // Invert servos by setting -1
// mixer-related configuration // mixer-related configuration
int8_t yaw_direction; int8_t yaw_direction;
@ -215,17 +218,16 @@ extern uint16_t calibratingA;
extern uint16_t calibratingG; extern uint16_t calibratingG;
extern int16_t heading; extern int16_t heading;
extern int16_t annex650_overrun_count; extern int16_t annex650_overrun_count;
extern int32_t pressure;
extern int32_t BaroAlt; extern int32_t BaroAlt;
extern int16_t sonarAlt; extern int16_t sonarAlt;
extern int32_t EstAlt; extern int32_t EstAlt;
extern int32_t AltHold; extern int32_t AltHold;
extern int16_t errorAltitudeI; extern int16_t errorAltitudeI;
extern int16_t BaroPID; extern int16_t BaroPID;
extern int16_t headFreeModeHold; extern int16_t headFreeModeHold;
extern int16_t zVelocity; extern int16_t zVelocity;
extern int16_t heading, magHold; extern int16_t heading, magHold;
extern int16_t motor[8]; extern int16_t motor[MAX_MOTORS];
extern int16_t servo[8]; extern int16_t servo[8];
extern int16_t rcData[8]; extern int16_t rcData[8];
extern uint8_t vbat; extern uint8_t vbat;
@ -255,6 +257,7 @@ extern config_t cfg;
extern flags_t f; extern flags_t f;
extern sensor_t acc; extern sensor_t acc;
extern sensor_t gyro; extern sensor_t gyro;
extern baro_t baro;
// main // main
void loop(void); void loop(void);

55
src/sensors.c
View file

@ -17,6 +17,7 @@ extern float magneticDeclination;
sensor_t acc; // acc access functions sensor_t acc; // acc access functions
sensor_t gyro; // gyro access functions sensor_t gyro; // gyro access functions
baro_t baro; // barometer access functions
uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected
#ifdef FY90Q #ifdef FY90Q
@ -85,17 +86,25 @@ retry:
} }
} }
// Detect what else is available #ifdef BARO
if (!bmp085Init()) // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
sensorsClear(SENSOR_BARO); if (!ms5611Detect(&baro)) {
// ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro
if (!bmp085Detect(&baro)) {
// if both failed, we don't have anything
sensorsClear(SENSOR_BARO);
}
}
#endif
#ifdef MAG
if (!hmc5883lDetect()) if (!hmc5883lDetect())
sensorsClear(SENSOR_MAG); sensorsClear(SENSOR_MAG);
#endif
// Now time to init them, acc first // Now time to init things, acc first
if (sensors(SENSOR_ACC)) if (sensors(SENSOR_ACC))
acc.init(); acc.init();
if (sensors(SENSOR_BARO))
bmp085Init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init(); gyro.init();
@ -234,13 +243,10 @@ void ACC_getADC(void)
} }
#ifdef BARO #ifdef BARO
static uint32_t baroDeadline = 0;
static uint8_t baroState = 0;
static uint16_t baroUT = 0;
static uint32_t baroUP = 0;
void Baro_update(void) void Baro_update(void)
{ {
static uint32_t baroDeadline = 0;
static uint8_t state = 0;
int32_t pressure; int32_t pressure;
if ((int32_t)(currentTime - baroDeadline) < 0) if ((int32_t)(currentTime - baroDeadline) < 0)
@ -248,28 +254,27 @@ void Baro_update(void)
baroDeadline = currentTime; baroDeadline = currentTime;
switch (baroState) { switch (state) {
case 0: case 0:
bmp085_start_ut(); baro.start_ut();
baroState++; state++;
baroDeadline += 4600; baroDeadline += baro.ut_delay;
break; break;
case 1: case 1:
baroUT = bmp085_get_ut(); baro.get_ut();
baroState++; state++;
break; break;
case 2: case 2:
bmp085_start_up(); baro.start_up();
baroState++; state++;
baroDeadline += 26000; baroDeadline += baro.up_delay;
break; break;
case 3: case 3:
baroUP = bmp085_get_up(); baro.get_up();
bmp085_get_temperature(baroUT); pressure = baro.calculate();
pressure = bmp085_get_pressure(baroUP);
BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f; // centimeter BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f; // centimeter
baroState = 0; state = 0;
baroDeadline += 5000; baroDeadline += baro.repeat_delay;
break; break;
} }
} }