Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +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 \
drv_adxl345.c \
drv_bmp085.c \
drv_ms5611.c \
drv_hcsr04.c \
drv_hmc5883l.c \
drv_ledring.c \

15
baseflight.uvproj
View file

@ -630,6 +630,11 @@
<FileType>1</FileType>
<FilePath>.\src\drv_l3g4200d.c</FilePath>
</File>
<File>
<FileName>drv_ms5611.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_ms5611.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1424,6 +1429,11 @@
<FileType>1</FileType>
<FilePath>.\src\drv_l3g4200d.c</FilePath>
</File>
<File>
<FileName>drv_ms5611.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_ms5611.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -2402,6 +2412,11 @@
<FileType>1</FileType>
<FilePath>.\src\drv_l3g4200d.c</FilePath>
</File>
<File>
<FileName>drv_ms5611.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_ms5611.c</FilePath>
</File>
</Files>
</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 (* 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 uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data
@ -62,6 +63,18 @@ typedef struct sensor_t
sensorReadFuncPtr temperature;
} 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 digitalLo(p, i) { p->BRR = i; }
#define digitalToggle(p, i) { p->ODR ^= i; }
@ -133,6 +146,7 @@ typedef struct sensor_t
#include "drv_adc.h"
#include "drv_adxl345.h"
#include "drv_bmp085.h"
#include "drv_ms5611.h"
#include "drv_hmc5883l.h"
#include "drv_i2c.h"
#include "drv_ledring.h"

2
src/cli.c
View file

@ -30,7 +30,7 @@ const char *mixerNames[] = {
"TRI", "QUADP", "QUADX", "BI",
"GIMBAL", "Y6", "HEX6",
"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

150
src/drv_bmp085.c
View file

@ -35,10 +35,8 @@ typedef struct {
uint8_t mode;
uint8_t chip_id, ml_version, al_version;
uint8_t dev_addr;
uint8_t sensortype;
int32_t param_b5;
int16_t oversampling_setting;
int16_t smd500_t_resolution, smd500_masterclock;
} bmp085_t;
#define BMP085_I2C_ADDR 0x77
@ -64,8 +62,6 @@ typedef struct {
#define BMP085_ML_VERSION__MSK 0x0F
#define BMP085_ML_VERSION__REG BMP085_VERSION_REG
#define BMP085_AL_VERSION__POS 4
#define BMP085_AL_VERSION__LEN 4
#define BMP085_AL_VERSION__MSK 0xF0
@ -79,12 +75,20 @@ typedef struct {
#define SMD500_PARAM_MI 3791 //calibration parameter
static bmp085_t bmp085 = { { 0, } };
static bmp085_t *p_bmp085 = &bmp085; /**< pointer to SMD500 / BMP085 device area */
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_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;
EXTI_InitTypeDef EXTI_InitStructure;
@ -119,49 +123,34 @@ bool bmp085Init(void)
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
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;
p_bmp085->dev_addr = BMP085_I2C_ADDR; /* preset BMP085 I2C_addr */
i2cRead(p_bmp085->dev_addr, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
p_bmp085->chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
p_bmp085->oversampling_setting = 3;
i2cRead(BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
bmp085.oversampling_setting = 3;
if (p_bmp085->chip_id == BMP085_CHIP_ID) { /* get bitslice */
p_bmp085->sensortype = BOSCH_PRESSURE_BMP085;
i2cRead(p_bmp085->dev_addr, BMP085_VERSION_REG, 1, &data); /* read Version reg */
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 */
if (bmp085.chip_id == BMP085_CHIP_ID) { /* get bitslice */
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 */
bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
bmp085_get_cal_param(); /* readout bmp085 calibparam structure */
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;
}
BARO_OFF;
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
int16_t bmp085_get_temperature(uint32_t ut)
static int16_t bmp085_get_temperature(uint32_t ut)
{
int16_t temperature;
int32_t x1, x2;
@ -169,12 +158,10 @@ int16_t bmp085_get_temperature(uint32_t ut)
static uint32_t temp;
#endif
if (p_bmp085->sensortype == BOSCH_PRESSURE_BMP085) {
x1 = (((int32_t) ut - (int32_t) p_bmp085->cal_param.ac6) * (int32_t) p_bmp085->cal_param.ac5) >> 15;
x2 = ((int32_t) p_bmp085->cal_param.mc << 11) / (x1 + p_bmp085->cal_param.md);
p_bmp085->param_b5 = x1 + x2;
}
temperature = ((p_bmp085->param_b5 + 8) >> 4); // temperature in 0.1°C
x1 = (((int32_t) ut - (int32_t) bmp085.cal_param.ac6) * (int32_t) bmp085.cal_param.ac5) >> 15;
x2 = ((int32_t) bmp085.cal_param.mc << 11) / (x1 + bmp085.cal_param.md);
bmp085.param_b5 = x1 + x2;
temperature = ((bmp085.param_b5 + 8) >> 4); // temperature in 0.1°C
#ifdef BMP_TEMP_OSS
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
}
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;
uint32_t b4, b7;
b6 = p_bmp085->param_b5 - 4000;
b6 = bmp085.param_b5 - 4000;
// *****calculate B3************
x1 = (b6 * b6) >> 12;
x1 *= p_bmp085->cal_param.b2;
x1 *= bmp085.cal_param.b2;
x1 >>= 11;
x2 = (p_bmp085->cal_param.ac2 * b6);
x2 = (bmp085.cal_param.ac2 * b6);
x2 >>= 11;
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************
x1 = (p_bmp085->cal_param.ac3 * b6) >> 13;
x2 = (p_bmp085->cal_param.b1 * ((b6 * b6) >> 12) ) >> 16;
x1 = (bmp085.cal_param.ac3 * b6) >> 13;
x2 = (bmp085.cal_param.b1 * ((b6 * b6) >> 12) ) >> 16;
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) {
pressure = (b7 << 1) / b4;
} else {
@ -226,15 +213,14 @@ int32_t bmp085_get_pressure(uint32_t up)
return pressure;
}
void bmp085_start_ut(void)
static void bmp085_start_ut(void)
{
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];
uint16_t timeout = 10000;
@ -246,27 +232,25 @@ uint16_t bmp085_get_ut(void)
__NOP();
}
#endif
i2cRead(p_bmp085->dev_addr, BMP085_ADC_OUT_MSB_REG, 2, data);
ut = (data[0] << 8) | data[1];
return ut;
i2cRead(BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 2, data);
bmp085_ut = (data[0] << 8) | data[1];
}
void bmp085_start_up(void)
static void bmp085_start_up(void)
{
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;
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
depending on the oversampling ratio setting up can be 16 to 19 bit
\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];
uint16_t timeout = 10000;
@ -278,31 +262,35 @@ uint32_t bmp085_get_up(void)
__NOP();
}
#endif
i2cRead(p_bmp085->dev_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);
i2cRead(BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data);
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)
{
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*/
p_bmp085->cal_param.ac1 = (data[0] <<8) | data[1];
p_bmp085->cal_param.ac2 = (data[2] <<8) | data[3];
p_bmp085->cal_param.ac3 = (data[4] <<8) | data[5];
p_bmp085->cal_param.ac4 = (data[6] <<8) | data[7];
p_bmp085->cal_param.ac5 = (data[8] <<8) | data[9];
p_bmp085->cal_param.ac6 = (data[10] <<8) | data[11];
bmp085.cal_param.ac1 = (data[0] << 8) | data[1];
bmp085.cal_param.ac2 = (data[2] << 8) | data[3];
bmp085.cal_param.ac3 = (data[4] << 8) | data[5];
bmp085.cal_param.ac4 = (data[6] << 8) | data[7];
bmp085.cal_param.ac5 = (data[8] << 8) | data[9];
bmp085.cal_param.ac6 = (data[10] << 8) | data[11];
/*parameters B1,B2*/
p_bmp085->cal_param.b1 = (data[12] <<8) | data[13];
p_bmp085->cal_param.b2 = (data[14] <<8) | data[15];
bmp085.cal_param.b1 = (data[12] << 8) | data[13];
bmp085.cal_param.b2 = (data[14] << 8) | data[15];
/*parameters MB,MC,MD*/
p_bmp085->cal_param.mb = (data[16] <<8) | data[17];
p_bmp085->cal_param.mc = (data[18] <<8) | data[19];
p_bmp085->cal_param.md = (data[20] <<8) | data[21];
bmp085.cal_param.mb = (data[16] << 8) | data[17];
bmp085.cal_param.mc = (data[18] << 8) | data[19];
bmp085.cal_param.md = (data[20] << 8) | data[21];
}

14
src/drv_bmp085.h
View file

@ -1,15 +1,3 @@
#pragma once
bool bmp085Init(void);
// 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);
bool bmp085Detect(baro_t *baro);

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

53
src/drv_pwm.c
View file

@ -139,15 +139,6 @@ enum {
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 uint16_t captures[MAX_INPUTS];
static pwmPortData_t *motors[MAX_MOTORS];
@ -159,7 +150,7 @@ static uint8_t numInputs = 0;
extern int16_t failsafeCnt;
static const uint8_t multiPPM[] = {
PWM1 | TYPE_IP,
PWM1 | TYPE_IP, // PPM input
PWM9 | TYPE_M, // Swap to servo if needed
PWM10 | TYPE_M, // Swap to servo if needed
PWM11 | TYPE_M,
@ -174,53 +165,53 @@ static const uint8_t multiPPM[] = {
};
static const uint8_t multiPWM[] = {
PWM1 | TYPE_IW,
PWM1 | TYPE_IW, // input #1
PWM2 | TYPE_IW,
PWM3 | TYPE_IW,
PWM4 | TYPE_IW,
PWM5 | TYPE_IW,
PWM6 | TYPE_IW,
PWM7 | TYPE_IW,
PWM8 | TYPE_IW,
PWM9 | TYPE_M, // Swap to servo if needed
PWM10 | TYPE_M, // Swap to servo if needed
PWM11 | TYPE_M,
PWM8 | TYPE_IW, // input #8
PWM9 | TYPE_M, // motor #1 or servo #1 (swap to servo if needed)
PWM10 | TYPE_M, // motor #2 or servo #2 (swap to servo if needed)
PWM11 | TYPE_M, // motor #1 or #3
PWM12 | TYPE_M,
PWM13 | TYPE_M,
PWM14 | TYPE_M,
PWM14 | TYPE_M, // motor #4 or #6
0xFF
};
static const uint8_t airPPM[] = {
PWM1 | TYPE_IP,
PWM5 | TYPE_S,
PWM6 | TYPE_S,
PWM7 | TYPE_S,
PWM8 | TYPE_S,
PWM9 | TYPE_M,
PWM10 | TYPE_M,
PWM11 | TYPE_S,
PWM1 | TYPE_IP, // PPM input
PWM9 | TYPE_M, // motor #1
PWM10 | TYPE_M, // motor #2
PWM11 | TYPE_S, // servo #1
PWM12 | 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
};
static const uint8_t airPWM[] = {
PWM1 | TYPE_IW,
PWM1 | TYPE_IW, // input #1
PWM2 | TYPE_IW,
PWM3 | TYPE_IW,
PWM4 | TYPE_IW,
PWM5 | TYPE_IW,
PWM6 | TYPE_IW,
PWM7 | TYPE_IW,
PWM8 | TYPE_IW,
PWM9 | TYPE_M,
PWM10 | TYPE_M,
PWM11 | TYPE_S,
PWM8 | TYPE_IW, // input #8
PWM9 | TYPE_M, // motor #1
PWM10 | TYPE_M, // motor #2
PWM11 | TYPE_S, // servo #1
PWM12 | TYPE_S,
PWM13 | TYPE_S,
PWM14 | TYPE_S,
PWM14 | TYPE_S, // servo #4
0xFF
};

4
src/drv_pwm.h
View file

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

1
src/main.c
View file

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

136
src/mixer.c
View file

@ -3,13 +3,27 @@
static uint8_t numberMotor = 4;
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 };
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)
{
int i;
// 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;
// if we want camstab/trig, that also enabled servos. this is kinda lame. maybe rework feature bits later.
if (feature(FEATURE_SERVO_TILT))
@ -19,12 +33,16 @@ void mixerInit(void)
case MULTITYPE_GIMBAL:
numberMotor = 0;
break;
case MULTITYPE_AIRPLANE:
case MULTITYPE_FLYING_WING:
numberMotor = 1;
break;
case MULTITYPE_BI:
numberMotor = 2;
break;
case MULTITYPE_TRI:
numberMotor = 3;
break;
@ -47,6 +65,14 @@ void mixerInit(void)
case MULTITYPE_OCTOFLATX:
numberMotor = 8;
break;
case MULTITYPE_CUSTOM:
numberMotor = 0;
for (i = 0; i < MAX_MOTORS; i++) {
if (customMixer[i].enabled)
numberMotor++;
}
break;
}
}
@ -55,16 +81,32 @@ void writeServos(void)
if (!useServo)
return;
if (cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_BI) {
/* One servo on Motor #4 */
switch (cfg.mixerConfiguration) {
case MULTITYPE_BI:
pwmWriteServo(0, servo[4]);
if (cfg.mixerConfiguration == MULTITYPE_BI)
pwmWriteServo(1, servo[5]);
} else {
/* Two servos for camstab or FLYING_WING */
break;
case MULTITYPE_TRI:
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;
}
}
extern uint8_t cliMode;
@ -87,7 +129,57 @@ void writeAllMotors(int16_t mc)
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)
{
@ -112,7 +204,7 @@ void mixTable(void)
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
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;
case MULTITYPE_QUADP:
@ -155,21 +247,12 @@ void mixTable(void)
break;
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[1] = PIDMIX(-4/5,-9/10,+1); //FRONT_R
motor[2] = PIDMIX(+4/5,+9/10,-1); //REAR_L
motor[3] = PIDMIX(+4/5,-9/10,-1); //FRONT_L
motor[4] = PIDMIX(-4/5 ,+0 ,-1); //RIGHT
motor[5] = PIDMIX(+4/5 ,+0 ,+1); //LEFT
#endif
break;
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[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
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

21
src/mw.h
View file

@ -30,14 +30,15 @@ typedef enum MultiType
MULTITYPE_FLYING_WING = 8, // UNSUPPORTED, do not select!
MULTITYPE_Y4 = 9,
MULTITYPE_HEX6X = 10,
MULTITYPE_OCTOX8 = 11,
MULTITYPE_OCTOFLATP = 12, // the GUI is the same for all 8 motor configs
MULTITYPE_OCTOFLATX = 13, // the GUI is the same for all 8 motor configs
MULTITYPE_AIRPLANE = 14,
MULTITYPE_HELI_120_CCPM = 15, // simple model
MULTITYPE_HELI_90_DEG = 16, // simple model
MULTITYPE_OCTOX8 = 11, // Java GUI is same for the next 3 configs
MULTITYPE_OCTOFLATP = 12, // MultiWinGui shows this differently
MULTITYPE_OCTOFLATX = 13, // MultiWinGui shows this differently
MULTITYPE_AIRPLANE = 14, // airplane / singlecopter / dualcopter
MULTITYPE_HELI_120_CCPM = 15,
MULTITYPE_HELI_90_DEG = 16,
MULTITYPE_VTAIL4 = 17,
MULTITYPE_LAST = 18
MULTITYPE_CUSTOM = 18, // no current GUI displays this
MULTITYPE_LAST = 19
} MultiType;
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 motor_pwm_rate; // The update rate of motor 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
int8_t yaw_direction;
@ -215,7 +218,6 @@ extern uint16_t calibratingA;
extern uint16_t calibratingG;
extern int16_t heading;
extern int16_t annex650_overrun_count;
extern int32_t pressure;
extern int32_t BaroAlt;
extern int16_t sonarAlt;
extern int32_t EstAlt;
@ -225,7 +227,7 @@ extern int16_t BaroPID;
extern int16_t headFreeModeHold;
extern int16_t zVelocity;
extern int16_t heading, magHold;
extern int16_t motor[8];
extern int16_t motor[MAX_MOTORS];
extern int16_t servo[8];
extern int16_t rcData[8];
extern uint8_t vbat;
@ -255,6 +257,7 @@ extern config_t cfg;
extern flags_t f;
extern sensor_t acc;
extern sensor_t gyro;
extern baro_t baro;
// main
void loop(void);

53
src/sensors.c
View file

@ -17,6 +17,7 @@ extern float magneticDeclination;
sensor_t acc; // acc 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
#ifdef FY90Q
@ -85,17 +86,25 @@ retry:
}
}
// Detect what else is available
if (!bmp085Init())
#ifdef BARO
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
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())
sensorsClear(SENSOR_MAG);
#endif
// Now time to init them, acc first
// Now time to init things, acc first
if (sensors(SENSOR_ACC))
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.
gyro.init();
@ -234,13 +243,10 @@ void ACC_getADC(void)
}
#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)
{
static uint32_t baroDeadline = 0;
static uint8_t state = 0;
int32_t pressure;
if ((int32_t)(currentTime - baroDeadline) < 0)
@ -248,28 +254,27 @@ void Baro_update(void)
baroDeadline = currentTime;
switch (baroState) {
switch (state) {
case 0:
bmp085_start_ut();
baroState++;
baroDeadline += 4600;
baro.start_ut();
state++;
baroDeadline += baro.ut_delay;
break;
case 1:
baroUT = bmp085_get_ut();
baroState++;
baro.get_ut();
state++;
break;
case 2:
bmp085_start_up();
baroState++;
baroDeadline += 26000;
baro.start_up();
state++;
baroDeadline += baro.up_delay;
break;
case 3:
baroUP = bmp085_get_up();
bmp085_get_temperature(baroUT);
pressure = bmp085_get_pressure(baroUP);
baro.get_up();
pressure = baro.calculate();
BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f; // centimeter
baroState = 0;
baroDeadline += 5000;
state = 0;
baroDeadline += baro.repeat_delay;
break;
}
}