mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
Merge branch 'master' of github.com:paparazzi/paparazzi into CHIMU
This commit is contained in:
Christophe De Wagter
@@ -1,26 +1,16 @@
|
||||
|
||||
#include "subsystems/sensors/baro.h"
|
||||
#include <stm32/gpio.h>
|
||||
|
||||
struct Baro baro;
|
||||
struct BaroBoard baro_board;
|
||||
struct i2c_transaction baro_trans;
|
||||
|
||||
|
||||
static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb);
|
||||
static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr);
|
||||
static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr);
|
||||
static inline void baro_board_read(void);
|
||||
|
||||
struct bmp085_baro_calibration calibration;
|
||||
|
||||
#define BMP085_SAMPLE_PERIOD_MS (3 + (2 << BMP085_OSS) * 3)
|
||||
#define BMP085_SAMPLE_PERIOD (BMP075_SAMPLE_PERIOD_MS >> 1)
|
||||
|
||||
void baro_init(void) {
|
||||
baro.status = BS_UNINITIALIZED;
|
||||
baro.absolute = 0;
|
||||
baro.differential = 0;
|
||||
baro_board.status = LBS_UNINITIALIZED;
|
||||
}
|
||||
// FIXME: BARO DRDY connected to PB0 for lisa/m
|
||||
|
||||
static inline void bmp085_write_reg(uint8_t addr, uint8_t value)
|
||||
{
|
||||
@@ -33,56 +23,23 @@ static inline void bmp085_write_reg(uint8_t addr, uint8_t value)
|
||||
while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning);
|
||||
}
|
||||
|
||||
static inline void bmp085_request_pressure(void)
|
||||
static inline void bmp085_read_reg16(uint8_t addr)
|
||||
{
|
||||
bmp085_write_reg(0xF4, 0x34 + (BMP085_OSS << 6));
|
||||
}
|
||||
|
||||
|
||||
void baro_periodic(void) {
|
||||
// check i2c_done
|
||||
if (!i2c_idle(&i2c2)) return;
|
||||
switch (baro_board.status) {
|
||||
case LBS_UNINITIALIZED:
|
||||
baro_board_send_reset();
|
||||
baro_board.status = LBS_RESETED;
|
||||
break;
|
||||
case LBS_RESETED:
|
||||
baro_board_send_config();
|
||||
baro_board.status = LBS_INITIALIZING;
|
||||
break;
|
||||
case LBS_INITIALIZING:
|
||||
baro_board_set_current_register(BMP085_ADDR, 0x00);
|
||||
baro_board.status = LBS_INITIALIZING_1;
|
||||
break;
|
||||
case LBS_INITIALIZING_1:
|
||||
baro.status = BS_RUNNING;
|
||||
case LBS_REQUEST:
|
||||
bmp085_request_pressure();
|
||||
baro_board.status = LBS_READ;
|
||||
break;
|
||||
case LBS_READ:
|
||||
baro_board_read();
|
||||
baro_board.status = LBS_READING;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void baro_board_send_config(void) {
|
||||
/* maybe we should read factory calibration here */
|
||||
//baro_board_write_to_register(BMP085_ADDR, 0x01, 0x86, 0x83);
|
||||
}
|
||||
|
||||
void baro_board_send_reset(void) {
|
||||
baro_trans.type = I2CTransTx;
|
||||
baro_trans.slave_addr = 0x00;
|
||||
baro_trans.type = I2CTransTxRx;
|
||||
baro_trans.slave_addr = BMP085_ADDR;
|
||||
baro_trans.len_w = 1;
|
||||
baro_trans.buf[0] = 0x06;
|
||||
i2c_submit(&i2c2,&baro_trans);
|
||||
baro_trans.len_r = 2;
|
||||
baro_trans.buf[0] = addr;
|
||||
i2c_submit(&i2c2, &baro_trans);
|
||||
}
|
||||
|
||||
static inline int16_t bmp085_read_reg16_blocking(uint8_t addr)
|
||||
{
|
||||
bmp085_read_reg16(addr);
|
||||
|
||||
while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning);
|
||||
|
||||
return ((baro_trans.buf[0] << 8) | baro_trans.buf[1]);
|
||||
}
|
||||
|
||||
static inline void bmp085_read_reg24(uint8_t addr)
|
||||
@@ -93,51 +50,101 @@ static inline void bmp085_read_reg24(uint8_t addr)
|
||||
baro_trans.len_r = 3;
|
||||
baro_trans.buf[0] = addr;
|
||||
i2c_submit(&i2c2, &baro_trans);
|
||||
//while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning);
|
||||
|
||||
//return (baro_trans.buf[0] << 16) | (baro_trans.buf[1] >> 8) | (baro_trans.buf[2]);
|
||||
}
|
||||
|
||||
static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb) {
|
||||
baro_trans.type = I2CTransTx;
|
||||
baro_trans.slave_addr = baro_addr;
|
||||
baro_trans.len_w = 3;
|
||||
baro_trans.buf[0] = reg_addr;
|
||||
baro_trans.buf[1] = val_msb;
|
||||
baro_trans.buf[2] = val_lsb;
|
||||
i2c_submit(&i2c2,&baro_trans);
|
||||
static void bmp085_baro_read_calibration(void)
|
||||
{
|
||||
calibration.ac1 = bmp085_read_reg16_blocking(0xAA); // AC1
|
||||
calibration.ac2 = bmp085_read_reg16_blocking(0xAC); // AC2
|
||||
calibration.ac3 = bmp085_read_reg16_blocking(0xAE); // AC3
|
||||
calibration.ac4 = bmp085_read_reg16_blocking(0xB0); // AC4
|
||||
calibration.ac5 = bmp085_read_reg16_blocking(0xB2); // AC5
|
||||
calibration.ac6 = bmp085_read_reg16_blocking(0xB4); // AC6
|
||||
calibration.b1 = bmp085_read_reg16_blocking(0xB6); // B1
|
||||
calibration.b2 = bmp085_read_reg16_blocking(0xB8); // B2
|
||||
calibration.mb = bmp085_read_reg16_blocking(0xBA); // MB
|
||||
calibration.mc = bmp085_read_reg16_blocking(0xBC); // MC
|
||||
calibration.md = bmp085_read_reg16_blocking(0xBE); // MD
|
||||
}
|
||||
|
||||
static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr) {
|
||||
baro_trans.type = I2CTransTxRx;
|
||||
baro_trans.slave_addr = baro_addr;
|
||||
baro_trans.len_w = 1;
|
||||
baro_trans.len_r = 2;
|
||||
baro_trans.buf[0] = reg_addr;
|
||||
i2c_submit(&i2c2,&baro_trans);
|
||||
void baro_init(void) {
|
||||
baro.status = BS_UNINITIALIZED;
|
||||
baro.absolute = 0;
|
||||
baro.differential = 0;
|
||||
baro_board.status = LBS_UNINITIALIZED;
|
||||
bmp085_baro_read_calibration();
|
||||
|
||||
/* STM32 specific (maybe this is a LISA/M specific driver anyway?) */
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
}
|
||||
|
||||
static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr) {
|
||||
baro_trans.type = I2CTransTx;
|
||||
baro_trans.slave_addr = baro_addr;
|
||||
baro_trans.len_w = 1;
|
||||
baro_trans.buf[0] = reg_addr;
|
||||
i2c_submit(&i2c2,&baro_trans);
|
||||
static inline int baro_eoc(void)
|
||||
{
|
||||
return GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_0);
|
||||
}
|
||||
|
||||
static inline void bmp085_request_pressure(void)
|
||||
{
|
||||
bmp085_write_reg(0xF4, 0x34 + (BMP085_OSS << 6));
|
||||
}
|
||||
|
||||
static inline void bmp085_request_temp(void)
|
||||
{
|
||||
bmp085_write_reg(0xF4, 0x2E);
|
||||
}
|
||||
|
||||
static inline void bmp085_read_pressure(void)
|
||||
{
|
||||
bmp085_read_reg24(0xF6);
|
||||
}
|
||||
|
||||
static inline void baro_board_read()
|
||||
static inline void bmp085_read_temp(void)
|
||||
{
|
||||
//int32_t x;
|
||||
//bmp085_request_pressure();
|
||||
bmp085_read_pressure();
|
||||
//baro_trans.type = I2CTransRx;
|
||||
//baro_trans.slave_addr = BMP085_ADDR;
|
||||
//baro_trans.len_r = 2;
|
||||
//i2c_submit(&i2c2,&baro_trans);
|
||||
bmp085_read_reg16(0xF6);
|
||||
}
|
||||
|
||||
void baro_periodic(void) {
|
||||
// check that nothing is in progress
|
||||
if (baro_trans.status == I2CTransPending) return;
|
||||
if (baro_trans.status == I2CTransRunning) return;
|
||||
if (!i2c_idle(&i2c2)) return;
|
||||
|
||||
switch (baro_board.status) {
|
||||
case LBS_UNINITIALIZED:
|
||||
baro_board_send_reset();
|
||||
baro_board.status = LBS_REQUEST;
|
||||
baro.status = BS_RUNNING;
|
||||
break;
|
||||
case LBS_REQUEST:
|
||||
bmp085_request_pressure();
|
||||
baro_board.status = LBS_READ;
|
||||
break;
|
||||
case LBS_READ:
|
||||
if (baro_eoc()) {
|
||||
bmp085_read_pressure();
|
||||
baro_board.status = LBS_READING;
|
||||
}
|
||||
break;
|
||||
case LBS_REQUEST_TEMP:
|
||||
bmp085_request_temp();
|
||||
baro_board.status = LBS_READ_TEMP;
|
||||
break;
|
||||
case LBS_READ_TEMP:
|
||||
if (baro_eoc()) {
|
||||
bmp085_read_temp();
|
||||
baro_board.status = LBS_READING_TEMP;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void baro_board_send_reset(void) {
|
||||
// This is a NOP at the moment
|
||||
}
|
||||
|
||||
@@ -10,44 +10,97 @@
|
||||
#include "std.h"
|
||||
#include "mcu_periph/i2c.h"
|
||||
|
||||
// absolute
|
||||
// absolute addr
|
||||
#define BMP085_ADDR 0xEE
|
||||
// Over sample setting (0-3)
|
||||
#define BMP085_OSS 3
|
||||
|
||||
enum LisaBaroStatus {
|
||||
LBS_UNINITIALIZED,
|
||||
LBS_RESETED,
|
||||
LBS_INITIALIZING,
|
||||
LBS_INITIALIZING_1,
|
||||
LBS_IDLE,
|
||||
LBS_REQUEST,
|
||||
LBS_READING,
|
||||
LBS_READ,
|
||||
LBS_REQUEST_TEMP,
|
||||
LBS_READING_TEMP,
|
||||
LBS_READ_TEMP,
|
||||
};
|
||||
|
||||
struct BaroBoard {
|
||||
enum LisaBaroStatus status;
|
||||
};
|
||||
|
||||
struct bmp085_baro_calibration {
|
||||
// These values come from EEPROM on sensor
|
||||
int16_t ac1;
|
||||
int16_t ac2;
|
||||
int16_t ac3;
|
||||
uint16_t ac4;
|
||||
uint16_t ac5;
|
||||
uint16_t ac6;
|
||||
int16_t b1;
|
||||
int16_t b2;
|
||||
int16_t mb;
|
||||
int16_t mc;
|
||||
int16_t md;
|
||||
|
||||
// These values are calculated
|
||||
int32_t b5;
|
||||
};
|
||||
|
||||
extern struct BaroBoard baro_board;
|
||||
extern struct i2c_transaction baro_trans;
|
||||
extern struct bmp085_baro_calibration calibration;
|
||||
|
||||
extern void baro_board_send_reset(void);
|
||||
extern void baro_board_send_config(void);
|
||||
|
||||
#define BaroEvent(_b_abs_handler, _b_diff_handler) { \
|
||||
if (baro_board.status == LBS_READING && \
|
||||
baro_trans.status != I2CTransPending) { \
|
||||
baro_board.status = LBS_REQUEST; \
|
||||
if (baro_trans.status == I2CTransSuccess) { \
|
||||
int32_t tmp = (baro_trans.buf[0]<<16) | (baro_trans.buf[1] << 8) | baro_trans.buf[0]; \
|
||||
baro.absolute = tmp >> ( 8 - BMP085_OSS); \
|
||||
_b_abs_handler(); \
|
||||
} \
|
||||
} \
|
||||
// Apply temp calibration and sensor calibration to raw measurement to get Pa (from BMP085 datasheet)
|
||||
static inline int32_t baro_apply_calibration(int32_t raw)
|
||||
{
|
||||
int32_t b6 = calibration.b5 - 4000;
|
||||
int x1 = (calibration.b2 * (b6 * b6 >> 12)) >> 11;
|
||||
int x2 = calibration.ac2 * b6 >> 11;
|
||||
int32_t x3 = x1 + x2;
|
||||
int32_t b3 = (((calibration.ac1 * 4 + x3) << BMP085_OSS) + 2)/4;
|
||||
x1 = calibration.ac3 * b6 >> 13;
|
||||
x2 = (calibration.b1 * (b6 * b6 >> 12)) >> 16;
|
||||
x3 = ((x1 + x2) + 2) >> 2;
|
||||
uint32_t b4 = (calibration.ac4 * (uint32_t) (x3 + 32768)) >> 15;
|
||||
uint32_t b7 = (raw - b3) * (50000 >> BMP085_OSS);
|
||||
int32_t p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
|
||||
x1 = (p >> 8) * (p >> 8);
|
||||
x1 = (x1 * 3038) >> 16;
|
||||
x2 = (-7357 * p) >> 16;
|
||||
return p + ((x1 + x2 + 3791) >> 4);
|
||||
}
|
||||
|
||||
static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void))
|
||||
{
|
||||
if (baro_board.status == LBS_READING &&
|
||||
baro_trans.status != I2CTransPending && baro_trans.status != I2CTransRunning) {
|
||||
baro_board.status = LBS_REQUEST_TEMP;
|
||||
if (baro_trans.status == I2CTransSuccess) {
|
||||
int32_t tmp = (baro_trans.buf[0]<<16) | (baro_trans.buf[1] << 8) | baro_trans.buf[2];
|
||||
tmp = tmp >> (8 - BMP085_OSS);
|
||||
baro.absolute = baro_apply_calibration(tmp);
|
||||
b_abs_handler();
|
||||
}
|
||||
}
|
||||
if (baro_board.status == LBS_READING_TEMP &&
|
||||
baro_trans.status != I2CTransPending && baro_trans.status != I2CTransRunning) {
|
||||
baro_board.status = LBS_REQUEST;
|
||||
if (baro_trans.status == I2CTransSuccess) {
|
||||
// abuse differential to store temp in 0.1C for now
|
||||
int32_t tmp = (baro_trans.buf[0] << 8) | baro_trans.buf[1];
|
||||
int32_t x1 = ((tmp - calibration.ac6) * calibration.ac5) >> 15;
|
||||
int32_t x2 = (calibration.mc << 11) / (x1 + calibration.md);
|
||||
calibration.b5 = x1 + x2;
|
||||
baro.differential = (calibration.b5 + 8) >> 4;
|
||||
b_diff_handler();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler)
|
||||
|
||||
#endif /* BOARDS_LISA_M_BARO_H */
|
||||
|
||||
Reference in New Issue
Block a user