mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
Update Olimexo to support a 10DOF board.
Sensors on it are: MPU6050 HMC5883L BMP085. BMP085 not connected to GPIO pins.
This commit is contained in:
parent
e17048a2f6
commit
ad1b7dd216
4 changed files with 31 additions and 14 deletions
2
Makefile
2
Makefile
|
@ -198,8 +198,10 @@ OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \
|
||||||
drivers/accgyro_mpu6050.c \
|
drivers/accgyro_mpu6050.c \
|
||||||
drivers/adc.c \
|
drivers/adc.c \
|
||||||
drivers/adc_stm32f10x.c \
|
drivers/adc_stm32f10x.c \
|
||||||
|
drivers/barometer_bmp085.c \
|
||||||
drivers/bus_i2c_stm32f10x.c \
|
drivers/bus_i2c_stm32f10x.c \
|
||||||
drivers/bus_spi.c \
|
drivers/bus_spi.c \
|
||||||
|
drivers/compass_hmc5883l.c \
|
||||||
drivers/gpio_stm32f10x.c \
|
drivers/gpio_stm32f10x.c \
|
||||||
drivers/light_led_stm32f10x.c \
|
drivers/light_led_stm32f10x.c \
|
||||||
drivers/pwm_mapping.c \
|
drivers/pwm_mapping.c \
|
||||||
|
|
|
@ -30,8 +30,10 @@
|
||||||
static bool convDone = false;
|
static bool convDone = false;
|
||||||
static uint16_t convOverrun = 0;
|
static uint16_t convOverrun = 0;
|
||||||
|
|
||||||
|
#ifdef BARO_GPIO
|
||||||
#define BARO_OFF digitalLo(BARO_GPIO, BARO_PIN);
|
#define BARO_OFF digitalLo(BARO_GPIO, BARO_PIN);
|
||||||
#define BARO_ON digitalHi(BARO_GPIO, BARO_PIN);
|
#define BARO_ON digitalHi(BARO_GPIO, BARO_PIN);
|
||||||
|
#endif
|
||||||
|
|
||||||
// EXTI14 for BMP085 End of Conversion Interrupt
|
// EXTI14 for BMP085 End of Conversion Interrupt
|
||||||
void EXTI15_10_IRQHandler(void)
|
void EXTI15_10_IRQHandler(void)
|
||||||
|
@ -111,9 +113,6 @@ static void bmp085_calculate(int32_t *pressure, int32_t *temperature);
|
||||||
|
|
||||||
bool bmp085Detect(baro_t *baro)
|
bool bmp085Detect(baro_t *baro)
|
||||||
{
|
{
|
||||||
gpio_config_t gpio;
|
|
||||||
EXTI_InitTypeDef EXTI_InitStructure;
|
|
||||||
NVIC_InitTypeDef NVIC_InitStructure;
|
|
||||||
uint8_t data;
|
uint8_t data;
|
||||||
|
|
||||||
// Not supported with this frequency
|
// Not supported with this frequency
|
||||||
|
@ -123,6 +122,11 @@ bool bmp085Detect(baro_t *baro)
|
||||||
if (bmp085InitDone)
|
if (bmp085InitDone)
|
||||||
return true;
|
return true;
|
||||||
|
|
||||||
|
#if defined(BARO) && defined(BARO_GPIO)
|
||||||
|
EXTI_InitTypeDef EXTI_InitStructure;
|
||||||
|
NVIC_InitTypeDef NVIC_InitStructure;
|
||||||
|
gpio_config_t gpio;
|
||||||
|
|
||||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
|
||||||
|
|
||||||
// PC13, PC14 (Barometer XCLR reset output, EOC input)
|
// PC13, PC14 (Barometer XCLR reset output, EOC input)
|
||||||
|
@ -133,9 +137,7 @@ bool bmp085Detect(baro_t *baro)
|
||||||
gpio.pin = Pin_14;
|
gpio.pin = Pin_14;
|
||||||
gpio.mode = Mode_IN_FLOATING;
|
gpio.mode = Mode_IN_FLOATING;
|
||||||
gpioInit(GPIOC, &gpio);
|
gpioInit(GPIOC, &gpio);
|
||||||
#ifdef BARO
|
|
||||||
BARO_ON;
|
BARO_ON;
|
||||||
#endif
|
|
||||||
|
|
||||||
// EXTI interrupt for barometer EOC
|
// EXTI interrupt for barometer EOC
|
||||||
gpioExtiLineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
|
gpioExtiLineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
|
||||||
|
@ -153,6 +155,7 @@ bool bmp085Detect(baro_t *baro)
|
||||||
NVIC_Init(&NVIC_InitStructure);
|
NVIC_Init(&NVIC_InitStructure);
|
||||||
|
|
||||||
delay(20); // datasheet says 10ms, we'll be careful and do 20. this is after ms5611 driver kills us, so longer the better.
|
delay(20); // datasheet says 10ms, we'll be careful and do 20. this is after ms5611 driver kills us, so longer the better.
|
||||||
|
#endif
|
||||||
|
|
||||||
i2cRead(BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
|
i2cRead(BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
|
||||||
bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
|
bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
|
||||||
|
@ -173,7 +176,8 @@ bool bmp085Detect(baro_t *baro)
|
||||||
baro->calculate = bmp085_calculate;
|
baro->calculate = bmp085_calculate;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#ifdef BARO
|
|
||||||
|
#if defined(BARO) && defined(BARO_GPIO)
|
||||||
BARO_OFF;
|
BARO_OFF;
|
||||||
#endif
|
#endif
|
||||||
return false;
|
return false;
|
||||||
|
|
|
@ -63,6 +63,8 @@
|
||||||
#define USE_ACC_MMA8452
|
#define USE_ACC_MMA8452
|
||||||
#define USE_ACC_LSM303DLHC
|
#define USE_ACC_LSM303DLHC
|
||||||
#define USE_ACC_MPU6050
|
#define USE_ACC_MPU6050
|
||||||
|
#define USE_BARO_MS5611
|
||||||
|
#define USE_BARO_BMP085
|
||||||
|
|
||||||
#ifdef NAZE
|
#ifdef NAZE
|
||||||
#undef USE_ACC_LSM303DLHC
|
#undef USE_ACC_LSM303DLHC
|
||||||
|
@ -92,6 +94,7 @@
|
||||||
#undef USE_ACC_BMA280
|
#undef USE_ACC_BMA280
|
||||||
#undef USE_ACC_MMA8452
|
#undef USE_ACC_MMA8452
|
||||||
#undef USE_ACC_ADXL345
|
#undef USE_ACC_ADXL345
|
||||||
|
#undef USE_BARO_MS5611
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef CHEBUZZF3
|
#ifdef CHEBUZZF3
|
||||||
|
@ -281,15 +284,21 @@ retry:
|
||||||
static void detectBaro()
|
static void detectBaro()
|
||||||
{
|
{
|
||||||
#ifdef BARO
|
#ifdef BARO
|
||||||
|
#ifdef USE_BARO_MS5611
|
||||||
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
|
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
|
||||||
if (!ms5611Detect(&baro)) {
|
if (ms5611Detect(&baro)) {
|
||||||
// ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro
|
return;
|
||||||
if (!bmp085Detect(&baro)) {
|
|
||||||
// if both failed, we don't have anything
|
|
||||||
sensorsClear(SENSOR_BARO);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_BARO_BMP085
|
||||||
|
// ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro
|
||||||
|
if (bmp085Detect(&baro)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
sensorsClear(SENSOR_BARO);
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
|
void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
|
||||||
|
|
|
@ -35,7 +35,9 @@
|
||||||
#define LED1
|
#define LED1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define GYRO
|
|
||||||
#define ACC
|
#define ACC
|
||||||
|
#define BARO
|
||||||
|
#define GYRO
|
||||||
|
#define MAG
|
||||||
|
|
||||||
#define SENSORS_SET (SENSOR_ACC)
|
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue