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
2
Makefile
2
Makefile
|
@ -198,8 +198,10 @@ OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \
|
|||
drivers/accgyro_mpu6050.c \
|
||||
drivers/adc.c \
|
||||
drivers/adc_stm32f10x.c \
|
||||
drivers/barometer_bmp085.c \
|
||||
drivers/bus_i2c_stm32f10x.c \
|
||||
drivers/bus_spi.c \
|
||||
drivers/compass_hmc5883l.c \
|
||||
drivers/gpio_stm32f10x.c \
|
||||
drivers/light_led_stm32f10x.c \
|
||||
drivers/pwm_mapping.c \
|
||||
|
|
|
@ -30,8 +30,10 @@
|
|||
static bool convDone = false;
|
||||
static uint16_t convOverrun = 0;
|
||||
|
||||
#ifdef BARO_GPIO
|
||||
#define BARO_OFF digitalLo(BARO_GPIO, BARO_PIN);
|
||||
#define BARO_ON digitalHi(BARO_GPIO, BARO_PIN);
|
||||
#endif
|
||||
|
||||
// EXTI14 for BMP085 End of Conversion Interrupt
|
||||
void EXTI15_10_IRQHandler(void)
|
||||
|
@ -111,9 +113,6 @@ static void bmp085_calculate(int32_t *pressure, int32_t *temperature);
|
|||
|
||||
bool bmp085Detect(baro_t *baro)
|
||||
{
|
||||
gpio_config_t gpio;
|
||||
EXTI_InitTypeDef EXTI_InitStructure;
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
uint8_t data;
|
||||
|
||||
// Not supported with this frequency
|
||||
|
@ -123,6 +122,11 @@ bool bmp085Detect(baro_t *baro)
|
|||
if (bmp085InitDone)
|
||||
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);
|
||||
|
||||
// PC13, PC14 (Barometer XCLR reset output, EOC input)
|
||||
|
@ -133,9 +137,7 @@ bool bmp085Detect(baro_t *baro)
|
|||
gpio.pin = Pin_14;
|
||||
gpio.mode = Mode_IN_FLOATING;
|
||||
gpioInit(GPIOC, &gpio);
|
||||
#ifdef BARO
|
||||
BARO_ON;
|
||||
#endif
|
||||
|
||||
// EXTI interrupt for barometer EOC
|
||||
gpioExtiLineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
|
||||
|
@ -153,6 +155,7 @@ bool bmp085Detect(baro_t *baro)
|
|||
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.
|
||||
#endif
|
||||
|
||||
i2cRead(BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read 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;
|
||||
return true;
|
||||
}
|
||||
#ifdef BARO
|
||||
|
||||
#if defined(BARO) && defined(BARO_GPIO)
|
||||
BARO_OFF;
|
||||
#endif
|
||||
return false;
|
||||
|
|
|
@ -63,6 +63,8 @@
|
|||
#define USE_ACC_MMA8452
|
||||
#define USE_ACC_LSM303DLHC
|
||||
#define USE_ACC_MPU6050
|
||||
#define USE_BARO_MS5611
|
||||
#define USE_BARO_BMP085
|
||||
|
||||
#ifdef NAZE
|
||||
#undef USE_ACC_LSM303DLHC
|
||||
|
@ -92,6 +94,7 @@
|
|||
#undef USE_ACC_BMA280
|
||||
#undef USE_ACC_MMA8452
|
||||
#undef USE_ACC_ADXL345
|
||||
#undef USE_BARO_MS5611
|
||||
#endif
|
||||
|
||||
#ifdef CHEBUZZF3
|
||||
|
@ -281,15 +284,21 @@ retry:
|
|||
static void detectBaro()
|
||||
{
|
||||
#ifdef BARO
|
||||
#ifdef USE_BARO_MS5611
|
||||
// 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);
|
||||
}
|
||||
if (ms5611Detect(&baro)) {
|
||||
return;
|
||||
}
|
||||
#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)
|
||||
|
|
|
@ -35,7 +35,9 @@
|
|||
#define LED1
|
||||
#endif
|
||||
|
||||
#define GYRO
|
||||
#define ACC
|
||||
#define BARO
|
||||
#define GYRO
|
||||
#define MAG
|
||||
|
||||
#define SENSORS_SET (SENSOR_ACC)
|
||||
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
|
||||
|
|
Loading…
Reference in New Issue