Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge branch 'baro_peripherals'

Browse Source 
closes #515
This commit is contained in:
Felix Ruess
2013-08-28 16:19:18 +02:00
parent f7d26c37a0 c959335b2c
commit 11b48b413e
30 changed files with 1729 additions and 1197 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/rotorcraft.makefile
+17 -5
View File
@@ -172,11 +172,16 @@ LISA_M_BARO ?= BARO_BOARD_BMP085
ifeq ($(LISA_M_BARO), BARO_MS5611_SPI)
include $(CFG_SHARED)/spi_master.makefile
ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3
ap.srcs += $(SRC_BOARD)/baro_ms5611_spi.c
ap.srcs += peripherals/ms5611.c
ap.srcs += peripherals/ms5611_spi.c
ap.srcs += subsystems/sensors/baro_ms5611_spi.c
else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C)
ap.CFLAGS += -DUSE_I2C2
ap.srcs += $(SRC_BOARD)/baro_ms5611_i2c.c
ap.srcs += peripherals/ms5611.c
ap.srcs += peripherals/ms5611_i2c.c
ap.srcs += subsystems/sensors/baro_ms5611_i2c.c
else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085)
ap.srcs += peripherals/bmp085.c
ap.srcs += $(SRC_BOARD)/baro_board.c
ap.CFLAGS += -DUSE_I2C2
endif
@@ -189,10 +194,14 @@ LIA_BARO ?= BARO_MS5611_SPI
ifeq ($(LIA_BARO), BARO_MS5611_SPI)
include $(CFG_SHARED)/spi_master.makefile
ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3
ap.srcs += boards/lisa_m/baro_ms5611_spi.c
ap.srcs += peripherals/ms5611.c
ap.srcs += peripherals/ms5611_spi.c
ap.srcs += subsystems/sensors/baro_ms5611_spi.c
else ifeq ($(LIA_BARO), BARO_MS5611_I2C)
ap.CFLAGS += -DUSE_I2C2
ap.srcs += boards/lisa_m/baro_ms5611_i2c.c
ap.srcs += peripherals/ms5611.c
ap.srcs += peripherals/ms5611_i2c.c
ap.srcs += subsystems/sensors/baro_ms5611_i2c.c
endif
ap.CFLAGS += -D$(LIA_BARO)
@@ -206,7 +215,10 @@ ap.srcs += $(SRC_BOARD)/baro_board.c
# krooz baro
else ifeq ($(BOARD), krooz)
ap.srcs += $(SRC_BOARD)/baro_board.c
ap.CFLAGS += -DMS5611_I2C_DEV=i2c2 -DMS5611_SLAVE_ADDR=0xEC
ap.srcs += peripherals/ms5611.c
ap.srcs += peripherals/ms5611_i2c.c
ap.srcs += subsystems/sensors/baro_ms5611_i2c.c
# apogee baro
else ifeq ($(BOARD), apogee)
conf/firmwares/subsystems/shared/imu_krooz_sd.makefile
-2
View File
@@ -14,11 +14,9 @@ IMU_KROOZ_I2C_DEV=i2c2
IMU_KROOZ_CFLAGS += -DUSE_I2C -DUSE_I2C2 -DI2C2_CLOCK_SPEED=400000
IMU_KROOZ_CFLAGS += -DIMU_KROOZ_I2C_DEV=$(IMU_KROOZ_I2C_DEV)
IMU_KROOZ_CFLAGS += -DMS5611_I2C_DEV=$(IMU_KROOZ_I2C_DEV) -DMS5611_SLAVE_ADDR=0xEC
IMU_KROOZ_SRCS += peripherals/mpu60x0.c
IMU_KROOZ_SRCS += peripherals/mpu60x0_i2c.c
IMU_KROOZ_SRCS += peripherals/hmc58xx.c
IMU_KROOZ_SRCS += modules/sensors/baro_ms5611_i2c.c
AHRS_PROPAGATE_FREQUENCY ?= 512
AHRS_CORRECT_FREQUENCY ?= 512
conf/modules/baro_bmp.xml
+3 -2
View File
@@ -10,10 +10,11 @@
<file name="baro_bmp.h"/>
</header>
<init fun="baro_bmp_init()"/>
<periodic fun="baro_bmp_periodic()" freq="8"/>
<periodic fun="baro_bmp_periodic()" freq="15"/>
<event fun="baro_bmp_event()"/>
<makefile>
<makefile target="ap">
<file name="baro_bmp.c"/>
<file name="bmp085.c" dir="peripherals"/>
</makefile>
</module>
conf/modules/baro_ms5611_i2c.xml
+4 -3
View File
@@ -13,12 +13,13 @@
<file name="baro_ms5611_i2c.h"/>
</header>
<init fun="baro_ms5611_init()"/>
<periodic fun="baro_ms5611_periodic()" freq="4" delay="0"/>
<periodic fun="baro_ms5611_d1()" freq="4" delay="1"/>
<periodic fun="baro_ms5611_d2()" freq="4" delay="2"/>
<periodic fun="baro_ms5611_read()" freq="4"/>
<periodic fun="baro_ms5611_periodic_check()" freq="40"/>
<event fun="baro_ms5611_event()"/>
<makefile target="ap">
<file name="baro_ms5611_i2c.c"/>
<file name="ms5611.c" dir="peripherals"/>
<file name="ms5611_i2c.c" dir="peripherals"/>
</makefile>
</module>
conf/modules/baro_ms5611_spi.xml
+25
View File
@@ -0,0 +1,25 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="baro_ms5611_spi" dir="sensors">
<doc>
<description>
Baro MS5611 (SPI)
Measurement Specialties MS5611-01BA pressure sensor (SPI)
</description>
<define name="MS5611_SPI_DEV" value="spiX" description="select spi peripheral to use (default spi1)"/>
<define name="MS5611_SLAVE_DEV" value="SPI_SLAVE0" description="SPI slave select index"/>
<define name="SENSOR_SYNC_SEND" description="flag to enable sending BARO_MS5611 message on every new measurement"/>
</doc>
<header>
<file name="baro_ms5611_spi.h"/>
</header>
<init fun="baro_ms5611_init()"/>
<periodic fun="baro_ms5611_read()" freq="4"/>
<periodic fun="baro_ms5611_periodic_check()" freq="40"/>
<event fun="baro_ms5611_event()"/>
<makefile target="ap">
<file name="baro_ms5611_spi.c"/>
<file name="ms5611.c" dir="peripherals"/>
<file name="ms5611_spi.c" dir="peripherals"/>
</makefile>
</module>
sw/airborne/boards/krooz/baro_board.c
-30
View File
@@ -1,30 +0,0 @@
#include "subsystems/sensors/baro.h"
#include "baro_board.h"
struct Baro baro;
void baro_init(void) {
baro_ms5611_init();
}
void baro_periodic(void) {
static uint8_t cnt;
switch(cnt) {
case 0:
baro_ms5611_periodic();
cnt++;
break;
case 1:
baro_ms5611_d1();
cnt++;
break;
case 2:
baro_ms5611_d2();
cnt = 0;
break;
default:
cnt = 0;
break;
}
}
sw/airborne/boards/krooz/baro_board.h
+4 -20
View File
@@ -1,30 +1,14 @@
/*
* board specific fonctions for the KroozSD board
* board specific interface for the KroozSD board
*
* It uses the subsystems/sensors/baro_ms5611_i2c.c driver
*/
#ifndef BOARDS_KROOZ_BARO_H
#define BOARDS_KROOZ_BARO_H
#include "std.h"
#include "mcu_periph/i2c.h"
#include "modules/sensors/baro_ms5611_i2c.h"
#include "math/pprz_algebra_int.h"
//#include "led.h"
static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void))
{
baro_ms5611_event();
if(baro_ms5611_valid) {
baro.status = BS_RUNNING;
baro.absolute = (int32_t)baroms;
b_abs_handler();
baro_ms5611_valid = FALSE;
}
}
#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler)
extern void baro_event(void (*b_abs_handler)(void));
#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler)
#endif /* BOARDS_KROOZ_SD_BARO_H */
sw/airborne/boards/lia/baro_board.h
+2 -2
View File
@@ -9,8 +9,8 @@
#include "std.h"
extern void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void));
extern void baro_event(void (*b_abs_handler)(void));
#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler)
#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler)
#endif /* BOARDS_LIA_BARO_H */
sw/airborne/boards/lisa_m/baro_board.c
+50 -164
View File
@@ -1,195 +1,81 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** @file boards/lisa_m/baro_board.c
* Baro board interface for Bosch BMP085 on LisaM I2C2 with EOC check.
*/
#include "subsystems/sensors/baro.h"
#include <libopencm3/stm32/f1/gpio.h>
#include "peripherals/bmp085_regs.h"
#include <libopencm3/stm32/gpio.h>
#include "led.h"
struct Baro baro;
struct BaroBoard baro_board;
struct i2c_transaction baro_trans;
struct bmp085_baro_calibration calibration;
struct Bmp085 baro_bmp085;
#define BMP085_SAMPLE_PERIOD_MS (3 + (2 << BMP085_OSS) * 3)
#define BMP085_SAMPLE_PERIOD (BMP075_SAMPLE_PERIOD_MS >> 1)
// FIXME: BARO DRDY connected to PB0 for lisa/m
static inline void bmp085_write_reg(uint8_t addr, uint8_t value)
static bool_t baro_eoc(void)
{
baro_trans.buf[0] = addr;
baro_trans.buf[1] = value;
i2c_transmit(&i2c2, &baro_trans, BMP085_ADDR, 2);
// FIXME, no while loops without timeout!!
while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning);
}
static inline void bmp085_read_reg16(uint8_t addr)
{
baro_trans.buf[0] = addr;
i2c_transceive(&i2c2, &baro_trans, BMP085_ADDR, 1, 2);
}
static inline int16_t bmp085_read_reg16_blocking(uint8_t addr, uint32_t timeout)
{
uint32_t time = 0;
bmp085_read_reg16(addr);
while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning) {
if ((time == timeout) && (timeout != 0)) {
return 0; /* Timeout of the i2c read */
} else {
time++;
}
}
return ((baro_trans.buf[0] << 8) | baro_trans.buf[1]);
}
static inline void bmp085_read_reg24(uint8_t addr)
{
baro_trans.buf[0] = addr;
i2c_transceive(&i2c2, &baro_trans, BMP085_ADDR, 1, 3);
}
static void bmp085_baro_read_calibration(void)
{
calibration.ac1 = bmp085_read_reg16_blocking(0xAA, 10000); // AC1
calibration.ac2 = bmp085_read_reg16_blocking(0xAC, 10000); // AC2
calibration.ac3 = bmp085_read_reg16_blocking(0xAE, 10000); // AC3
calibration.ac4 = bmp085_read_reg16_blocking(0xB0, 10000); // AC4
calibration.ac5 = bmp085_read_reg16_blocking(0xB2, 10000); // AC5
calibration.ac6 = bmp085_read_reg16_blocking(0xB4, 10000); // AC6
calibration.b1 = bmp085_read_reg16_blocking(0xB6, 10000); // B1
calibration.b2 = bmp085_read_reg16_blocking(0xB8, 10000); // B2
calibration.mb = bmp085_read_reg16_blocking(0xBA, 10000); // MB
calibration.mc = bmp085_read_reg16_blocking(0xBC, 10000); // MC
calibration.md = bmp085_read_reg16_blocking(0xBE, 10000); // MD
return gpio_get(GPIOB, GPIO0);
}
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?) */
bmp085_init(&baro_bmp085, &i2c2, BMP085_SLAVE_ADDR);
/* setup eoc check function */
baro_bmp085.eoc = &baro_eoc;
gpio_clear(GPIOB, GPIO0);
gpio_set_mode(GPIOB, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_PULL_UPDOWN, GPIO0);
}
static inline int baro_eoc(void)
{
return gpio_get(GPIOB, GPIO0);
}
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 bmp085_read_temp(void)
{
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;
if (baro_bmp085.initialized) {
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;
bmp085_periodic(&baro_bmp085);
}
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;
}
else
bmp085_read_eeprom_calib(&baro_bmp085);
}
void baro_board_send_reset(void) {
// This is a NOP at the moment
}
// Apply temp calibration and sensor calibration to raw measurement to get Pa (from BMP085 datasheet)
static int32_t baro_apply_calibration(int32_t raw)
void baro_event(void (*b_abs_handler)(void))
{
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);
}
bmp085_event(&baro_bmp085);
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);
if (baro_bmp085.data_available) {
baro.absolute = baro_bmp085.pressure;
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();
}
baro_bmp085.data_available = FALSE;
#ifdef ROTORCRAFT_BARO_LED
RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
#endif
}
}
sw/airborne/boards/lisa_m/baro_board.h
+4 -45
View File
@@ -12,56 +12,15 @@
// for right now we abuse this file for the ms5611 baro on aspirin as well
#if !BARO_MS5611_I2C && !BARO_MS5611
#include "mcu_periph/i2c.h"
#include "peripherals/bmp085.h"
// absolute addr
#define BMP085_ADDR 0xEE
// Over sample setting (0-3)
#define BMP085_OSS 3
extern struct Bmp085 baro_bmp085;
enum LisaBaroStatus {
LBS_UNINITIALIZED,
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);
#endif // !BARO_MS5611_xx
extern void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void));
extern void baro_event(void (*b_abs_handler)(void));
#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler)
#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler)
#endif /* BOARDS_LISA_M_BARO_H */
sw/airborne/boards/lisa_m/baro_ms5611_i2c.c
-206
View File
@@ -1,206 +0,0 @@
/**
* Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C
*
* Edit by: Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
* Utah State University, http://aggieair.usu.edu/
*/
#include "subsystems/sensors/baro.h"
#include "peripherals/ms5611.h"
#include "led.h"
#include "std.h"
#include "mcu_periph/sys_time.h"
#include "mcu_periph/i2c.h"
#ifndef MS5611_I2C_DEV
#define MS5611_I2C_DEV i2c2
#endif
#ifdef DEBUG
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#endif
struct Baro baro;
struct i2c_transaction ms5611_trans;
uint8_t ms5611_status;
int32_t prom_cnt;
uint16_t ms5611_c[PROM_NB];
uint32_t ms5611_d1, ms5611_d2;
float fbaroms, ftempms;
static int8_t baro_ms5611_crc(uint16_t* prom) {
int32_t i, j;
uint32_t res = 0;
uint8_t crc = prom[7] & 0xF;
prom[7] &= 0xFF00;
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;
else return -1;
}
void baro_init(void) {
ms5611_status = MS5611_UNINIT;
baro.status = BS_UNINITIALIZED;
prom_cnt = 0;
}
void baro_periodic(void) {
if (sys_time.nb_sec > 1) {
if (ms5611_status == MS5611_IDLE) {
/* start D1 conversion */
ms5611_status = MS5611_CONV_D1;
ms5611_trans.buf[0] = MS5611_START_CONV_D1;
i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
#ifdef DEBUG
RunOnceEvery(60, { DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
&ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
&ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]);});
#endif
}
else if (ms5611_status == MS5611_CONV_D1) {
/* assume D1 conversion is done */
ms5611_status = MS5611_CONV_D1_OK;
}
else if (ms5611_status == MS5611_CONV_D1_OK) {
/* read D1 adc */
ms5611_status = MS5611_ADC_D1;
ms5611_trans.buf[0] = MS5611_ADC_READ;
i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 3);
}
else if (ms5611_status == MS5611_CONV_D2) {
/* assume D2 conversion is done */
ms5611_status = MS5611_CONV_D2_OK;
}
else if (ms5611_status == MS5611_CONV_D2_OK) {
/* read D2 adc */
ms5611_status = MS5611_ADC_D2;
ms5611_trans.buf[0] = MS5611_ADC_READ;
i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 3);
}
else if (ms5611_status == MS5611_UNINIT) {
/* reset sensor */
ms5611_status = MS5611_RESET;
ms5611_trans.buf[0] = MS5611_SOFT_RESET;
i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
}
else if (ms5611_status == MS5611_RESET_OK) {
/* start getting prom data */
ms5611_status = MS5611_PROM;
ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 2);
}
}
}
void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){
if (ms5611_trans.status == I2CTransSuccess) {
#ifdef ROTORCRAFT_BARO_LED
RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
#endif
switch (ms5611_status) {
case MS5611_RESET:
ms5611_status = MS5611_RESET_OK;
ms5611_trans.status = I2CTransDone;
break;
case MS5611_PROM:
/* read prom data */
ms5611_c[prom_cnt++] = (ms5611_trans.buf[0] << 8) | ms5611_trans.buf[1];
if (prom_cnt < PROM_NB) {//8 bytes at PROM
/* get next prom data */
ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 2);
}
else {
/* done reading prom */
ms5611_trans.status = I2CTransDone;
/* check prom crc */
if (baro_ms5611_crc(ms5611_c) == 0) {
ms5611_status = MS5611_IDLE;
baro.status = BS_RUNNING;
}
else {
/* checksum error, try again */
baro_init();
}
}
break;
case MS5611_ADC_D1:
/* read D1 (pressure) */
ms5611_d1 = (ms5611_trans.buf[0] << 16) |
(ms5611_trans.buf[1] << 8) |
ms5611_trans.buf[2];
/* start D2 conversion */
ms5611_status = MS5611_CONV_D2;
ms5611_trans.buf[0] = MS5611_START_CONV_D2;
i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
break;
case MS5611_ADC_D2: {
int64_t dt, baroms, tempms, off, sens, t2, off2, sens2;
/* read D2 (temperature) */
ms5611_d2 = (ms5611_trans.buf[0] << 16) |
(ms5611_trans.buf[1] << 8) |
ms5611_trans.buf[2];
ms5611_status = MS5611_IDLE;
ms5611_trans.status = I2CTransDone;
/* difference between actual and ref temperature */
dt = ms5611_d2 - (int64_t)ms5611_c[5] * (1<<8);
/* actual temperature */
tempms = 2000 + ((int64_t)dt * ms5611_c[6]) / (1<<23);
/* offset at actual temperature */
off = ((int64_t)ms5611_c[2] * (1<<16)) + ((int64_t)ms5611_c[4] * dt) / (1<<7);
/* sensitivity at actual temperature */
sens = ((int64_t)ms5611_c[1] * (1<<15)) + ((int64_t)ms5611_c[3] * dt) / (1<<8);
/* second order temperature compensation */
if (tempms < 2000) {
t2 = (dt*dt) / (1<<31);
off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1);
sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2);
if (tempms < -1500) {
off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500);
sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1);
}
tempms = tempms - t2;
off = off - off2;
sens = sens - sens2;
}
/* temperature compensated pressure */
baroms = (((int64_t)ms5611_d1 * sens) / (1<<21) - off) / (1<<15);
/* Update baro structure */
baro.absolute = (int32_t)baroms;
b_abs_handler();
b_diff_handler();
#ifdef DEBUG
ftempms = tempms / 100.;
fbaroms = baroms / 100.;
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
&ms5611_d1, &ms5611_d2, &fbaroms, &ftempms, &baro.status);
#endif
break;
}
default:
ms5611_trans.status = I2CTransDone;
break;
}
}
}
sw/airborne/boards/lisa_m/baro_ms5611_spi.c
-226
View File
@@ -1,226 +0,0 @@
/**
* Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C
*
* Edit by: Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
* Utah State University, http://aggieair.usu.edu/
*/
#include "subsystems/sensors/baro.h"
#include "peripherals/ms5611.h"
#include "led.h"
#include "std.h"
#include "mcu_periph/sys_time.h"
#include "mcu_periph/spi.h"
#ifndef MS5611_SPI_DEV
#define MS5611_SPI_DEV spi2
#endif
#define MS5611_BUFFER_LENGTH 4
#ifdef DEBUG
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#endif
struct Baro baro;
struct spi_transaction ms5611_trans;
uint8_t ms5611_status;
int32_t prom_cnt;
uint16_t ms5611_c[PROM_NB];
uint32_t ms5611_d1, ms5611_d2;
float fbaroms, ftempms;
volatile uint8_t input_buf_ms5611[MS5611_BUFFER_LENGTH];
volatile uint8_t output_buf_ms5611[MS5611_BUFFER_LENGTH];
static void trans_cb_ms5611( struct spi_transaction *trans );
static int8_t baro_ms5611_crc(uint16_t* prom) {
int32_t i, j;
uint32_t res = 0;
uint8_t crc = prom[7] & 0xF;
prom[7] &= 0xFF00;
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;
else return -1;
}
static void trans_cb_ms5611( struct spi_transaction *trans ) {
#ifdef ROTORCRAFT_BARO_LED
RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
#endif
}
void baro_init(void) {
ms5611_trans.select = SPISelectUnselect;
ms5611_trans.cpol = SPICpolIdleHigh;
ms5611_trans.cpha = SPICphaEdge2;
ms5611_trans.dss = SPIDss8bit;
ms5611_trans.bitorder = SPIMSBFirst;
ms5611_trans.cdiv = SPIDiv64;
ms5611_trans.slave_idx = MS5611_SLAVE_DEV;
ms5611_trans.output_length = MS5611_BUFFER_LENGTH;
ms5611_trans.input_length = MS5611_BUFFER_LENGTH;
ms5611_trans.after_cb = trans_cb_ms5611;
ms5611_trans.input_buf = &input_buf_ms5611[0];
ms5611_trans.output_buf = &input_buf_ms5611[0];
ms5611_status = MS5611_UNINIT;
baro.status = BS_UNINITIALIZED;
prom_cnt = 0;
}
void baro_periodic(void) {
if (sys_time.nb_sec > 1) {
if (ms5611_status == MS5611_IDLE) {
/* start D1 conversion */
ms5611_status = MS5611_CONV_D1;
ms5611_trans.output_buf[0] = MS5611_START_CONV_D1;
spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
#ifdef DEBUG
RunOnceEvery(300, { DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
&ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
&ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]);});
#endif
}
else if (ms5611_status == MS5611_CONV_D1) {
/* assume D1 conversion is done */
ms5611_status = MS5611_CONV_D1_OK;
}
else if (ms5611_status == MS5611_CONV_D1_OK) {
/* read D1 adc */
ms5611_status = MS5611_ADC_D1;
ms5611_trans.output_buf[0] = MS5611_ADC_READ;
spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
}
else if (ms5611_status == MS5611_CONV_D2) {
/* assume D2 conversion is done */
ms5611_status = MS5611_CONV_D2_OK;
}
else if (ms5611_status == MS5611_CONV_D2_OK) {
/* read D2 adc */
ms5611_status = MS5611_ADC_D2;
ms5611_trans.output_buf[0] = MS5611_ADC_READ;
spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
}
else if (ms5611_status == MS5611_UNINIT) {
/* reset sensor */
ms5611_status = MS5611_RESET;
ms5611_trans.output_buf[0] = MS5611_SOFT_RESET;
spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
}
else if (ms5611_status == MS5611_RESET_OK) {
/* start getting prom data */
ms5611_status = MS5611_PROM;
ms5611_trans.output_buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
}
}
}
void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){
if (ms5611_trans.status == SPITransSuccess) {
switch (ms5611_status) {
case MS5611_RESET:
ms5611_status = MS5611_RESET_OK;
ms5611_trans.status = SPITransDone;
break;
case MS5611_PROM:
/* read prom data */
ms5611_c[prom_cnt++] = (ms5611_trans.input_buf[1] << 8) | ms5611_trans.input_buf[2];
if (prom_cnt < PROM_NB) {//8 bytes at PROM
/* get next prom data */
ms5611_trans.output_buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
}
else {
/* done reading prom */
ms5611_trans.status = SPITransDone;
/* check prom crc */
if (baro_ms5611_crc(ms5611_c) == 0) {
ms5611_status = MS5611_IDLE;
baro.status = BS_RUNNING;
}
else {
/* checksum error, try again */
baro_init();
}
}
break;
case MS5611_ADC_D1:
/* read D1 (pressure) */
ms5611_d1 = (ms5611_trans.input_buf[1] << 16) |
(ms5611_trans.input_buf[2] << 8) |
ms5611_trans.input_buf[3];
/* start D2 conversion */
ms5611_status = MS5611_CONV_D2;
ms5611_trans.output_buf[0] = MS5611_START_CONV_D2;
spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
break;
case MS5611_ADC_D2: {
int64_t dt, baroms, tempms, off, sens, t2, off2, sens2;
/* read D2 (temperature) */
ms5611_d2 = (ms5611_trans.input_buf[1] << 16) |
(ms5611_trans.input_buf[2] << 8) |
ms5611_trans.input_buf[3];
ms5611_status = MS5611_IDLE;
ms5611_trans.status = SPITransDone;
/* difference between actual and ref temperature */
dt = ms5611_d2 - (int64_t)ms5611_c[5] * (1<<8);
/* actual temperature */
tempms = 2000 + ((int64_t)dt * ms5611_c[6]) / (1<<23);
/* offset at actual temperature */
off = ((int64_t)ms5611_c[2] * (1<<16)) + ((int64_t)ms5611_c[4] * dt) / (1<<7);
/* sensitivity at actual temperature */
sens = ((int64_t)ms5611_c[1] * (1<<15)) + ((int64_t)ms5611_c[3] * dt) / (1<<8);
/* second order temperature compensation */
if (tempms < 2000) {
t2 = (dt*dt) / (1<<31);
off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1);
sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2);
if (tempms < -1500) {
off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500);
sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1);
}
tempms = tempms - t2;
off = off - off2;
sens = sens - sens2;
}
/* temperature compensated pressure */
baroms = (((int64_t)ms5611_d1 * sens) / (1<<21) - off) / (1<<15);
/* Update baro structure */
baro.absolute = (int32_t)baroms;
b_abs_handler();
b_diff_handler();
#ifdef DEBUG
ftempms = tempms / 100.;
fbaroms = baroms / 100.;
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
&ms5611_d1, &ms5611_d2, &fbaroms, &ftempms, &baro.status);
#endif
break;
}
default:
ms5611_trans.status = SPITransDone;
break;
}
}
}
sw/airborne/modules/sensors/baro_bmp.c
+28 -171
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2010 Martin Mueller
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
@@ -20,14 +21,16 @@
*
*/
/** \file baro_bmp.c
* \brief Bosch BMP085 I2C sensor interface
/**
* @file modules/sensors/baro_bmp.c
* Bosch BMP085 I2C sensor interface.
*
* This reads the values for pressure and temperature from the Bosch BMP085 sensor through I2C.
*/
#include "baro_bmp.h"
#include "peripherals/bmp085_regs.h"
#include "mcu_periph/sys_time.h"
#include "mcu_periph/i2c.h"
@@ -35,12 +38,6 @@
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#include "state.h"
#include "subsystems/nav.h"
#ifdef SITL
#include "subsystems/gps.h"
#endif
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -54,196 +51,56 @@
#define BMP_I2C_DEV i2c0
#endif
#define BMP085_SLAVE_ADDR 0xEE
#define BARO_BMP_OFFSET_MAX 30000
#define BARO_BMP_OFFSET_MIN 10
#define BARO_BMP_OFFSET_NBSAMPLES_INIT 2
#define BARO_BMP_OFFSET_NBSAMPLES_AVRG 4
#define BARO_BMP_R 0.5
#define BARO_BMP_SIGMA2 0.1
struct i2c_transaction bmp_trans;
struct Bmp085 baro_bmp;
bool_t baro_bmp_enabled;
float baro_bmp_r;
float baro_bmp_sigma2;
int32_t baro_bmp_alt;
// Global variables
uint8_t baro_bmp_status;
bool_t baro_bmp_valid;
uint32_t baro_bmp_pressure;
uint16_t baro_bmp_temperature;
int32_t baro_bmp_altitude, baro_bmp,baro_bmp_temp,baro_bmp_offset;
double tmp_float;
void baro_bmp_init(void) {
int16_t bmp_ac1, bmp_ac2, bmp_ac3;
uint16_t bmp_ac4, bmp_ac5, bmp_ac6;
int16_t bmp_b1, bmp_b2;
int16_t bmp_mb, bmp_mc, bmp_md;
int32_t bmp_up, bmp_ut;
bmp085_init(&baro_bmp, &BMP_I2C_DEV, BMP085_SLAVE_ADDR);
// Local variables
bool_t baro_bmp_offset_init;
int32_t baro_bmp_offset_tmp;
uint16_t baro_bmp_cnt;
void baro_bmp_init( void ) {
baro_bmp_status = BARO_BMP_UNINIT;
baro_bmp_valid = FALSE;
baro_bmp_r = BARO_BMP_R;
baro_bmp_sigma2 = BARO_BMP_SIGMA2;
baro_bmp_enabled = TRUE;
baro_bmp_offset_init = FALSE;
baro_bmp_cnt = BARO_BMP_OFFSET_NBSAMPLES_INIT + BARO_BMP_OFFSET_NBSAMPLES_AVRG;
/* read calibration values */
bmp_trans.buf[0] = BMP085_EEPROM_AC1;
i2c_transceive(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 1, 22);
}
void baro_bmp_periodic( void ) {
#ifndef SITL
if (baro_bmp_status == BARO_BMP_IDLE) {
/* start temp measurement (once) */
bmp_trans.buf[0] = BMP085_CTRL_REG;
bmp_trans.buf[1] = BMP085_START_TEMP;
i2c_transmit(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 2);
baro_bmp_status = BARO_BMP_START_TEMP;
}
else if (baro_bmp_status == BARO_BMP_START_TEMP) {
/* read temp measurement */
bmp_trans.buf[0] = BMP085_DAT_MSB;
i2c_transceive(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 1, 2);
baro_bmp_status = BARO_BMP_READ_TEMP;
}
else if (baro_bmp_status == BARO_BMP_START_PRESS) {
/* read press measurement */
bmp_trans.buf[0] = BMP085_DAT_MSB;
i2c_transceive(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 1, 3);
baro_bmp_status = BARO_BMP_READ_PRESS;
}
#else // SITL
baro_bmp_altitude = gps.hmsl / 1000.0;
baro_bmp_pressure = baro_bmp_altitude; //FIXME do a proper scaling here
baro_bmp_valid = TRUE;
#endif
}
void baro_bmp_event( void ) {
void baro_bmp_periodic(void) {
if (bmp_trans.status == I2CTransSuccess) {
if (baro_bmp_status == BARO_BMP_UNINIT) {
/* get calibration data */
bmp_ac1 = (bmp_trans.buf[0] << 8) | bmp_trans.buf[1];
bmp_ac2 = (bmp_trans.buf[2] << 8) | bmp_trans.buf[3];
bmp_ac3 = (bmp_trans.buf[4] << 8) | bmp_trans.buf[5];
bmp_ac4 = (bmp_trans.buf[6] << 8) | bmp_trans.buf[7];
bmp_ac5 = (bmp_trans.buf[8] << 8) | bmp_trans.buf[9];
bmp_ac6 = (bmp_trans.buf[10] << 8) | bmp_trans.buf[11];
bmp_b1 = (bmp_trans.buf[12] << 8) | bmp_trans.buf[13];
bmp_b2 = (bmp_trans.buf[14] << 8) | bmp_trans.buf[15];
bmp_mb = (bmp_trans.buf[16] << 8) | bmp_trans.buf[17];
bmp_mc = (bmp_trans.buf[18] << 8) | bmp_trans.buf[19];
bmp_md = (bmp_trans.buf[20] << 8) | bmp_trans.buf[21];
baro_bmp_status = BARO_BMP_IDLE;
}
else if (baro_bmp_status == BARO_BMP_READ_TEMP) {
/* get uncompensated temperature */
bmp_ut = (bmp_trans.buf[0] << 8) | bmp_trans.buf[1];
/* start high res pressure measurement */
bmp_trans.buf[0] = BMP085_CTRL_REG;
bmp_trans.buf[1] = BMP085_START_P3;
i2c_transmit(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 2);
baro_bmp_status = BARO_BMP_START_PRESS;
}
else if (baro_bmp_status == BARO_BMP_READ_PRESS) {
int32_t bmp_p, bmp_t;
int32_t bmp_x1, bmp_x2, bmp_x3;
int32_t bmp_b3, bmp_b5, bmp_b6;
uint32_t bmp_b4, bmp_b7;
/* get uncompensated pressure, oss=3 */
bmp_up = (bmp_trans.buf[0] << 11) |
(bmp_trans.buf[1] << 3) |
(bmp_trans.buf[2] >> 5);
/* start temp measurement */
bmp_trans.buf[0] = BMP085_CTRL_REG;
bmp_trans.buf[1] = BMP085_START_TEMP;
i2c_transmit(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 2);
baro_bmp_status = BARO_BMP_START_TEMP;
/* compensate temperature */
bmp_x1 = (bmp_ut - bmp_ac6) * bmp_ac5 / (1<<15);
bmp_x2 = bmp_mc * (1<<11) / (bmp_x1 + bmp_md);
bmp_b5 = bmp_x1 + bmp_x2;
bmp_t = (bmp_b5 + 8) / (1<<4);
/* compensate pressure */
bmp_b6 = bmp_b5 - 4000;
bmp_x1 = (bmp_b2 * (bmp_b6 * bmp_b6 / (1<<12))) / (1<<11);
bmp_x2 = bmp_ac2 *bmp_b6 / (1<<11);
bmp_x3 = bmp_x1 + bmp_x2;
bmp_b3 = (((bmp_ac1 * 4 + bmp_x3) << 3) + 2) / 4;
bmp_x1 = bmp_ac3 * bmp_b6 / (1<<13);
bmp_x2 = (bmp_b1 * (bmp_b6 * bmp_b6 / (1<<12))) / (1<<16);
bmp_x3 = ((bmp_x1 + bmp_x2) +2) / (1<<2);
bmp_b4 = bmp_ac4 * (uint32_t)(bmp_x3 + 32768) / (1<<15);
bmp_b7 = ((uint32_t)bmp_up - bmp_b3) * (50000>>3);
if (bmp_b7 < 0x80000000)
bmp_p = (bmp_b7 * 2) / bmp_b4;
if (baro_bmp.initialized)
bmp085_periodic(&baro_bmp);
else
bmp_p = (bmp_b7 / bmp_b4) * 2;
bmp_x1 = (bmp_p / (1<<8)) * (bmp_p / (1<<8));
bmp_x1 = (bmp_x1 * 3038) / (1<<16);
bmp_x2 = (-7357 * bmp_p) / (1<<16);
bmp_p = bmp_p + (bmp_x1 + bmp_x2 + 3791) / (1<<4);
bmp085_read_eeprom_calib(&baro_bmp);
baro_bmp_temperature = bmp_t;
baro_bmp_pressure = bmp_p;
}
tmp_float = bmp_p/101325.0; //pressao nivel mar
tmp_float = pow(tmp_float,0.190295); //eleva pressao ao expoente
baro_bmp = 44330*(1.0-tmp_float);
void baro_bmp_event(void) {
if (!baro_bmp_offset_init) {
baro_bmp_offset = baro_bmp;
baro_bmp_offset_init = TRUE;
#if 0
--baro_bmp_cnt;
// Check if averaging completed
if (baro_bmp_cnt == 0) {
// Calculate average
baro_bmp_offset = (baro_bmp_offset_tmp / BARO_BMP_OFFSET_NBSAMPLES_AVRG);
// Limit offset
if (baro_bmp_offset < BARO_BMP_OFFSET_MIN)
baro_bmp_offset = BARO_BMP_OFFSET_MIN;
if (baro_bmp_offset > BARO_BMP_OFFSET_MAX)
baro_bmp_offset = BARO_BMP_OFFSET_MAX;
baro_bmp_offset_init = TRUE;
}
// Check if averaging needs to continue
else if (baro_bmp_cnt <= BARO_BMP_OFFSET_NBSAMPLES_AVRG)
baro_bmp_offset_tmp += baro_bmp;
#endif
} //baro offset init
bmp085_event(&baro_bmp);
baro_bmp_temp = (baro_bmp - baro_bmp_offset);
if (baro_bmp.data_available) {
if (baro_bmp_offset_init) {
baro_bmp_altitude = ground_alt + baro_bmp_temp;
// New value available
baro_bmp_valid = TRUE;
float tmp = baro_bmp.pressure / 101325.0; // pressure at sea level
tmp = pow(tmp, 0.190295);
baro_bmp_alt = 44330 * (1.0 - tmp);
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &bmp_up, &bmp_ut, &bmp_p, &bmp_t);
DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &baro_bmp.up,
&baro_bmp.ut, &baro_bmp.pressure,
&baro_bmp.temperature);
#else
RunOnceEvery(10, DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &baro_bmp_temp, &bmp_ut, &bmp_p, &bmp_t));
RunOnceEvery(10, DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice,
&baro_bmp.up, &baro_bmp.ut,
&baro_bmp.pressure,
&baro_bmp.temperature));
#endif
} else {
baro_bmp_altitude = 0.0;
}
}
}
}
sw/airborne/modules/sensors/baro_bmp.h
+35 -41
View File
@@ -1,42 +1,43 @@
/*
* Copyright (C) 2010 Martin Mueller
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/**
* @file modules/sensors/baro_bmp.h
* Bosch BMP085 I2C sensor interface.
*
* This reads the values for pressure and temperature from the Bosch BMP085 sensor through I2C.
*/
#ifndef BARO_BMP_H
#define BARO_BMP_H
#include "std.h"
#include "peripherals/bmp085.h"
#define BMP085_EEPROM_AC1 0xAA
#define BMP085_EEPROM_AC2 0xAC
#define BMP085_EEPROM_AC3 0xAE
#define BMP085_EEPROM_AC4 0xB0
#define BMP085_EEPROM_AC5 0xB2
#define BMP085_EEPROM_AC6 0xB4
#define BMP085_EEPROM_B1 0xB6
#define BMP085_EEPROM_B2 0xB8
#define BMP085_EEPROM_MB 0xBA
#define BMP085_EEPROM_MC 0xBC
#define BMP085_EEPROM_MD 0xBE
#define BMP085_CTRL_REG 0xF4
#define BMP085_START_TEMP 0x2E
#define BMP085_START_P0 0x34
#define BMP085_START_P1 0x74
#define BMP085_START_P2 0xB4
#define BMP085_START_P3 0xF4
#define BMP085_DAT_MSB 0xF6
#define BMP085_DAT_LSB 0xF7
#define BMP085_DAT_XLSB 0xF8
#define BARO_BMP_UNINIT 0
#define BARO_BMP_IDLE 1
#define BARO_BMP_START_TEMP 2
#define BARO_BMP_READ_TEMP 3
#define BARO_BMP_START_PRESS 4
#define BARO_BMP_READ_PRESS 5
extern struct Bmp085 baro_bmp;
/// new measurement every 3rd baro_bmp_periodic
#ifndef SITL
#define BARO_BMP_DT (BARO_BMP_PERIODIC_PERIOID / 3)
#define BARO_BMP_DT (BARO_BMP_PERIODIC_PERIOD / 3)
#else
#define BARO_BMP_DT BARO_BMP_PERIODIC_PERIOID
#endif
@@ -44,19 +45,12 @@
extern bool_t baro_bmp_enabled;
extern float baro_bmp_r;
extern float baro_bmp_sigma2;
extern uint8_t baro_bmp_status;
extern bool_t baro_bmp_valid;
extern uint32_t baro_bmp_pressure;
extern uint16_t baro_bmp_temperature;
extern int32_t baro_bmp_altitude;
extern int32_t baro_bmp;
extern int32_t baro_bmp_offset;
extern int32_t baro_bmp_alt;
void baro_bmp_init(void);
void baro_bmp_periodic(void);
void baro_bmp_event(void);
#define BaroBmpUpdate(_b, _h) { if (baro_bmp_valid) { _b = baro_bmp_pressure; _h(); baro_bmp_valid = FALSE; } }
#define BaroBmpUpdate(_b, _h) { if (baro_bmp.data_available) { _b = baro_bmp.pressure; _h(); baro_bmp.data_available = FALSE; } }
#endif
sw/airborne/modules/sensors/baro_ms5611_i2c.c
+33 -171
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2011 Martin Mueller <martinmm@pfump.org>
* Copyright (C) 2011-2013 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -30,11 +30,9 @@
#include "modules/sensors/baro_ms5611_i2c.h"
#include "mcu_periph/sys_time.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#include "subsystems/nav.h"
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -49,202 +47,66 @@
#define MS5611_SLAVE_ADDR 0xEE
#endif
struct Ms5611_I2c baro_ms5611;
struct i2c_transaction ms5611_trans;
uint8_t ms5611_status;
uint16_t ms5611_c[PROM_NB];
uint32_t ms5611_d1, ms5611_d2;
int32_t prom_cnt;
int64_t baroms;
float fbaroms, ftempms;
float baro_ms5611_alt;
bool_t baro_ms5611_alt_valid;
bool_t baro_ms5611_enabled;
bool_t baro_ms5611_valid;
float baro_ms5611_r;
float baro_ms5611_sigma2;
static int8_t baro_ms5611_crc(uint16_t* prom) {
int32_t i, j;
uint32_t res = 0;
uint8_t crc = prom[7] & 0xF;
prom[7] &= 0xFF00;
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;
else return -1;
}
void baro_ms5611_init(void) {
ms5611_i2c_init(&baro_ms5611, &MS5611_I2C_DEV, MS5611_SLAVE_ADDR);
baro_ms5611_enabled = TRUE;
baro_ms5611_valid = FALSE;
ms5611_status = MS5611_UNINIT;
baro_ms5611_alt_valid = FALSE;
baro_ms5611_r = BARO_MS5611_R;
baro_ms5611_sigma2 = BARO_MS5611_SIGMA2;
prom_cnt = 0;
}
void baro_ms5611_periodic( void ) {
if (sys_time.nb_sec > 1) {
if (ms5611_status >= MS5611_IDLE) {
/* start D1 conversion */
ms5611_status = MS5611_CONV_D1;
ms5611_trans.buf[0] = MS5611_START_CONV_D1;
i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
void baro_ms5611_periodic_check( void ) {
ms5611_i2c_periodic_check(&baro_ms5611);
if (baro_ms5611.initialized) {
RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
&ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
&ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]));
}
else if (ms5611_status == MS5611_UNINIT) {
/* reset sensor */
ms5611_status = MS5611_RESET;
ms5611_trans.buf[0] = MS5611_SOFT_RESET;
i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
}
else if (ms5611_status == MS5611_RESET_OK) {
/* start getting prom data */
ms5611_status = MS5611_PROM;
ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 2);
}
&baro_ms5611.data.c[0],
&baro_ms5611.data.c[1],
&baro_ms5611.data.c[2],
&baro_ms5611.data.c[3],
&baro_ms5611.data.c[4],
&baro_ms5611.data.c[5],
&baro_ms5611.data.c[6],
&baro_ms5611.data.c[7]));
}
}
void baro_ms5611_d1( void ) {
/// trigger new measurement or initialize if needed
void baro_ms5611_read(void) {
if (sys_time.nb_sec > 1) {
if (ms5611_status == MS5611_CONV_D1_OK) {
/* read D1 adc */
ms5611_status = MS5611_ADC_D1;
ms5611_trans.buf[0] = MS5611_ADC_READ;
i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 3);
}
}
}
void baro_ms5611_d2( void ) {
if (sys_time.nb_sec > 1) {
if (ms5611_status == MS5611_CONV_D2_OK) {
/* read D2 adc */
ms5611_status = MS5611_ADC_D2;
ms5611_trans.buf[0] = MS5611_ADC_READ;
i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 3);
}
ms5611_i2c_read(&baro_ms5611);
}
}
void baro_ms5611_event( void ) {
if (ms5611_trans.status == I2CTransSuccess) {
switch (ms5611_status) {
case MS5611_RESET:
ms5611_status = MS5611_RESET_OK;
ms5611_trans.status = I2CTransDone;
break;
ms5611_i2c_event(&baro_ms5611);
case MS5611_PROM:
/* read prom data */
ms5611_c[prom_cnt++] = (ms5611_trans.buf[0] << 8) | ms5611_trans.buf[1];
if (prom_cnt < PROM_NB) {
/* get next prom data */
ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 2);
}
else {
/* done reading prom */
ms5611_trans.status = I2CTransDone;
/* check prom crc */
if (baro_ms5611_crc(ms5611_c) == 0) {
DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
&ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
&ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]);
ms5611_status = MS5611_IDLE;
}
else {
/* checksum error, try again */
baro_ms5611_init();
}
}
break;
case MS5611_CONV_D1:
ms5611_status = MS5611_CONV_D1_OK;
ms5611_trans.status = I2CTransDone;
break;
case MS5611_ADC_D1:
/* read D1 (pressure) */
ms5611_d1 = (ms5611_trans.buf[0] << 16) |
(ms5611_trans.buf[1] << 8) |
ms5611_trans.buf[2];
/* start D2 conversion */
ms5611_status = MS5611_CONV_D2;
ms5611_trans.buf[0] = MS5611_START_CONV_D2;
i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
break;
case MS5611_CONV_D2:
ms5611_status = MS5611_CONV_D2_OK;
ms5611_trans.status = I2CTransDone;
break;
case MS5611_ADC_D2: {
int64_t dt, tempms, off, sens, t2, off2, sens2;
/* read D2 (temperature) */
ms5611_d2 = (ms5611_trans.buf[0] << 16) |
(ms5611_trans.buf[1] << 8) |
ms5611_trans.buf[2];
ms5611_status = MS5611_IDLE;
ms5611_trans.status = I2CTransDone;
/* difference between actual and ref temperature */
dt = ms5611_d2 - (int64_t)ms5611_c[5] * (1<<8);
/* actual temperature */
tempms = 2000 + ((int64_t)dt * ms5611_c[6]) / (1<<23);
/* offset at actual temperature */
off = ((int64_t)ms5611_c[2] * (1<<16)) + ((int64_t)ms5611_c[4] * dt) / (1<<7);
/* sensitivity at actual temperature */
sens = ((int64_t)ms5611_c[1] * (1<<15)) + ((int64_t)ms5611_c[3] * dt) / (1<<8);
/* second order temperature compensation */
if (tempms < 2000) {
t2 = (dt*dt) / (1<<31);
off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1);
sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2);
if (tempms < -1500) {
off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500);
sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1);
}
tempms = tempms - t2;
off = off - off2;
sens = sens - sens2;
}
/* temperature compensated pressure */
baroms = (((int64_t)ms5611_d1 * sens) / (1<<21) - off) / (1<<15);
float tmp_float = baroms/101325.0; //pressure at sea level
tmp_float = pow(tmp_float,0.190295); //eleva pressao ao expoente
baro_ms5611_alt = 44330*(1.0-tmp_float); //altitude above MSL
baro_ms5611_valid = TRUE;
if (baro_ms5611.data_available) {
float tmp_float = baro_ms5611.data.pressure / 101325.0; //pressure at sea level
tmp_float = pow(tmp_float, 0.190295);
baro_ms5611_alt = 44330 * (1.0 - tmp_float); //altitude above MSL
baro_ms5611_alt_valid = TRUE;
#ifdef SENSOR_SYNC_SEND
ftempms = tempms / 100.;
fbaroms = baroms / 100.;
ftempms = baro_ms5611.data.temperature / 100.;
fbaroms = baro_ms5611.data.pressure / 100.;
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
&ms5611_d1, &ms5611_d2, &fbaroms, &ftempms);
&baro_ms5611.data.d1, &baro_ms5611.data.d2, &fbaroms, &ftempms);
#endif
break;
}
default:
ms5611_trans.status = I2CTransDone;
break;
}
}
}
sw/airborne/modules/sensors/baro_ms5611_i2c.h
+13 -33
View File
@@ -2,48 +2,28 @@
#define BARO_MS56111_I2C_H
#include "std.h"
#include "peripherals/ms5611_i2c.h"
/* we use OSR=4096 for maximum resolution */
#define MS5611_SOFT_RESET 0x1E
#define MS5611_PROM_READ 0xA0
#define MS5611_START_CONV_D1 0x48
#define MS5611_START_CONV_D2 0x58
#define MS5611_ADC_READ 0x00
#define PROM_NB 8
/// new measurement every baro_ms5611_periodic
#define BARO_MS5611_DT BARO_MS5611_PERIODIC_PERIOID
/// new measurement with every baro_ms5611_read() call
#define BARO_MS5611_DT BARO_MS5611_READ_PERIOD
#define BARO_MS5611_R 20
#define BARO_MS5611_SIGMA2 1
extern float baro_ms5611_alt;
extern bool_t baro_ms5611_valid;
extern bool_t baro_ms5611_enabled;
extern float baro_ms5611_r;
extern float baro_ms5611_sigma2;
extern int64_t baroms;
enum ms5611_stat{
MS5611_UNINIT,
MS5611_RESET,
MS5611_RESET_OK,
MS5611_PROM,
MS5611_IDLE,
MS5611_CONV_D1,
MS5611_CONV_D1_OK,
MS5611_ADC_D1,
MS5611_CONV_D2,
MS5611_CONV_D2_OK,
MS5611_ADC_D2
};
extern float baro_ms5611_alt;
extern bool_t baro_ms5611_alt_valid;
extern bool_t baro_ms5611_enabled;
extern struct Ms5611_I2c baro_ms5611;
extern void baro_ms5611_init(void);
extern void baro_ms5611_periodic(void);
extern void baro_ms5611_d1(void);
extern void baro_ms5611_d2(void);
extern void baro_ms5611_read(void);
extern void baro_ms5611_periodic_check(void);
extern void baro_ms5611_event(void);
#define BaroMs5611Update(_b, _h) { if (baro_ms5611_valid) { _b = baro_ms5611_alt; _h(); baro_ms5611_valid = FALSE; } }
#define BaroMs5611UpdatePressure(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; _h(); baro_ms5611.data_available = FALSE; } }
#define BaroMs5611UpdateAlt(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611_alt; _h(); baro_ms5611.data_available = FALSE; } }
#endif
sw/airborne/modules/sensors/baro_ms5611_spi.c
+112
View File
@@ -0,0 +1,112 @@
/*
* Copyright (C) 2011-2013 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/**
* @file modules/sensors/baro_ms5611_spi.c
* Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for SPI.
*
*/
#include "modules/sensors/baro_ms5611_spi.h"
#include "mcu_periph/sys_time.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#ifndef MS5611_SPI_DEV
#define MS5611_SPI_DEV spi1
#endif
#ifndef MS5611_SLAVE_DEV
#define MS5611_SLAVE_DEV SPI_SLAVE0
#endif
struct Ms5611_Spi baro_ms5611;
float fbaroms, ftempms;
float baro_ms5611_alt;
bool_t baro_ms5611_alt_valid;
bool_t baro_ms5611_enabled;
float baro_ms5611_r;
float baro_ms5611_sigma2;
void baro_ms5611_init(void) {
ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_DEV);
baro_ms5611_enabled = TRUE;
baro_ms5611_alt_valid = FALSE;
baro_ms5611_r = BARO_MS5611_R;
baro_ms5611_sigma2 = BARO_MS5611_SIGMA2;
}
void baro_ms5611_periodic_check( void ) {
ms5611_spi_periodic_check(&baro_ms5611);
if (baro_ms5611.initialized) {
RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
&baro_ms5611.data.c[0],
&baro_ms5611.data.c[1],
&baro_ms5611.data.c[2],
&baro_ms5611.data.c[3],
&baro_ms5611.data.c[4],
&baro_ms5611.data.c[5],
&baro_ms5611.data.c[6],
&baro_ms5611.data.c[7]));
}
}
/// trigger new measurement or initialize if needed
void baro_ms5611_read(void) {
if (sys_time.nb_sec > 1) {
ms5611_spi_read(&baro_ms5611);
}
}
void baro_ms5611_event( void ) {
ms5611_spi_event(&baro_ms5611);
if (baro_ms5611.data_available) {
float tmp_float = baro_ms5611.data.pressure / 101325.0; //pressure at sea level
tmp_float = pow(tmp_float, 0.190295);
baro_ms5611_alt = 44330 * (1.0 - tmp_float); //altitude above MSL
baro_ms5611_alt_valid = TRUE;
#ifdef SENSOR_SYNC_SEND
ftempms = baro_ms5611.data.temperature / 100.;
fbaroms = baro_ms5611.data.pressure / 100.;
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
&baro_ms5611.data.d1, &baro_ms5611.data.d2, &fbaroms, &ftempms);
#endif
}
}
sw/airborne/modules/sensors/baro_ms5611_spi.h
+57
View File
@@ -0,0 +1,57 @@
/*
* Copyright (C) 2011 Martin Mueller <martinmm@pfump.org>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/**
* @file modules/sensors/baro_ms5611_spi.h
* Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for SPI.
*
*/
#ifndef BARO_MS56111_SPI_H
#define BARO_MS56111_SPI_H
#include "std.h"
#include "peripherals/ms5611_spi.h"
/// new measurement with every baro_ms5611_read() call
#define BARO_MS5611_DT BARO_MS5611_READ_PERIOD
#define BARO_MS5611_R 20
#define BARO_MS5611_SIGMA2 1
extern float baro_ms5611_r;
extern float baro_ms5611_sigma2;
extern float baro_ms5611_alt;
extern bool_t baro_ms5611_alt_valid;
extern bool_t baro_ms5611_enabled;
extern struct Ms5611_Spi baro_ms5611;
extern void baro_ms5611_init(void);
extern void baro_ms5611_read(void);
extern void baro_ms5611_periodic_check(void);
extern void baro_ms5611_event(void);
#define BaroMs5611UpdatePressure(_b) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; baro_ms5611.data_available = FALSE; } }
#define BaroMs5611UpdateAlt(_b) { if (baro_ms5611.data_available) { _b = baro_ms5611_alt; baro_ms5611.data_available = FALSE; } }
#endif
sw/airborne/peripherals/bmp085.c
+196
View File
@@ -0,0 +1,196 @@
/*
* Copyright (C) 2010 Martin Mueller
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** @file peripherals/bmp085.c
* Bosch BMP085 driver interface.
*/
#include "peripherals/bmp085.h"
#include "peripherals/bmp085_regs.h"
static int32_t bmp085_compensated_temperature(struct Bmp085Calib* calib, int32_t raw)
{
int32_t x1 = (raw - calib->ac6) * calib->ac5 / (1<<15);
int32_t x2 = (calib->mc << 11) / (x1 + calib->md);
calib->b5 = x1 + x2;
return (calib->b5 + 8) >> 4;
}
/** Apply temp calibration and sensor calibration to raw measurement to get Pa
* (from BMP085 datasheet)
*/
static int32_t bmp085_compensated_pressure(struct Bmp085Calib* calib, int32_t raw)
{
int32_t b6 = calib->b5 - 4000;
int32_t x1 = (calib->b2 * (b6 * b6 >> 12)) >> 11;
int32_t x2 = calib->ac2 * b6 >> 11;
int32_t x3 = x1 + x2;
int32_t b3 = (((calib->ac1 * 4 + x3) << BMP085_OSS) + 2)/4;
x1 = calib->ac3 * b6 >> 13;
x2 = (calib->b1 * (b6 * b6 >> 12)) >> 16;
x3 = ((x1 + x2) + 2) >> 2;
uint32_t b4 = (calib->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);
}
/**
* Dummy function to always return TRUE on EndOfConversion check.
* Ensure proper timing trough frequency of bmp085_periodic instead!
*/
static bool_t bmp085_eoc_true(void)
{
return TRUE;
}
void bmp085_read_eeprom_calib(struct Bmp085* bmp)
{
if (bmp->status == BMP085_STATUS_UNINIT) {
bmp->initialized = FALSE;
bmp->i2c_trans.buf[0] = BMP085_EEPROM_AC1;
i2c_transceive(bmp->i2c_p, &(bmp->i2c_trans), bmp->i2c_trans.slave_addr, 1, 22);
}
}
void bmp085_init(struct Bmp085* bmp, struct i2c_periph *i2c_p, uint8_t addr)
{
/* set i2c_peripheral */
bmp->i2c_p = i2c_p;
/* slave address */
bmp->i2c_trans.slave_addr = addr;
/* set initial status: Success or Done */
bmp->i2c_trans.status = I2CTransDone;
bmp->data_available = FALSE;
bmp->initialized = FALSE;
bmp->status = BMP085_STATUS_UNINIT;
/* by default assign EOC function that always returns TRUE
* ensure proper timing through frequeny of bmp_periodic!
*/
bmp->eoc = &bmp085_eoc_true;
}
/**
* Start new measurement if idle or read temp/pressure.
* Should run at < 40Hz unless eoc check function is provided.
* At ultra high resolution (oss = 3) conversion time is max 25.5ms.
*/
void bmp085_periodic(struct Bmp085* bmp)
{
switch (bmp->status) {
case BMP085_STATUS_IDLE:
/* start temp measurement */
bmp->i2c_trans.buf[0] = BMP085_CTRL_REG;
bmp->i2c_trans.buf[1] = BMP085_START_TEMP;
i2c_transmit(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 2);
bmp->status = BMP085_STATUS_START_TEMP;
break;
case BMP085_STATUS_START_TEMP:
/* read temp measurement */
if (bmp->eoc()) {
bmp->i2c_trans.buf[0] = BMP085_DAT_MSB;
i2c_transceive(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 1, 2);
bmp->status = BMP085_STATUS_READ_TEMP;
}
break;
case BMP085_STATUS_START_PRESS:
/* read press measurement */
if (bmp->eoc()) {
bmp->i2c_trans.buf[0] = BMP085_DAT_MSB;
i2c_transceive(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 1, 3);
bmp->status = BMP085_STATUS_READ_PRESS;
}
break;
default:
break;
}
}
void bmp085_event(struct Bmp085* bmp)
{
if (bmp->i2c_trans.status == I2CTransSuccess) {
switch (bmp->status) {
case BMP085_STATUS_UNINIT:
bmp->calib.ac1 = (bmp->i2c_trans.buf[0] << 8) | bmp->i2c_trans.buf[1];
bmp->calib.ac2 = (bmp->i2c_trans.buf[2] << 8) | bmp->i2c_trans.buf[3];
bmp->calib.ac3 = (bmp->i2c_trans.buf[4] << 8) | bmp->i2c_trans.buf[5];
bmp->calib.ac4 = (bmp->i2c_trans.buf[6] << 8) | bmp->i2c_trans.buf[7];
bmp->calib.ac5 = (bmp->i2c_trans.buf[8] << 8) | bmp->i2c_trans.buf[9];
bmp->calib.ac6 = (bmp->i2c_trans.buf[10] << 8) | bmp->i2c_trans.buf[11];
bmp->calib.b1 = (bmp->i2c_trans.buf[12] << 8) | bmp->i2c_trans.buf[13];
bmp->calib.b2 = (bmp->i2c_trans.buf[14] << 8) | bmp->i2c_trans.buf[15];
bmp->calib.mb = (bmp->i2c_trans.buf[16] << 8) | bmp->i2c_trans.buf[17];
bmp->calib.mc = (bmp->i2c_trans.buf[18] << 8) | bmp->i2c_trans.buf[19];
bmp->calib.md = (bmp->i2c_trans.buf[20] << 8) | bmp->i2c_trans.buf[21];
bmp->status = BMP085_STATUS_IDLE;
bmp->initialized = TRUE;
break;
case BMP085_STATUS_READ_TEMP:
/* get uncompensated temperature */
bmp->ut = (bmp->i2c_trans.buf[0] << 8) | bmp->i2c_trans.buf[1];
bmp->temperature = bmp085_compensated_temperature(&(bmp->calib), bmp->ut);
/* start high res pressure measurement */
bmp->i2c_trans.buf[0] = BMP085_CTRL_REG;
bmp->i2c_trans.buf[1] = BMP085_START_P3;
i2c_transmit(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 2);
bmp->status = BMP085_STATUS_START_PRESS;
break;
case BMP085_STATUS_READ_PRESS:
/* get uncompensated pressure */
bmp->up = (bmp->i2c_trans.buf[0]<<16) |
(bmp->i2c_trans.buf[1] << 8) |
bmp->i2c_trans.buf[2];
bmp->up = bmp->up >> (8 - BMP085_OSS);
bmp->pressure = bmp085_compensated_pressure(&(bmp->calib), bmp->up);
bmp->data_available = TRUE;
bmp->status = BMP085_STATUS_IDLE;
break;
default:
break;
}
}
else if (bmp->i2c_trans.status == I2CTransFailed) {
/* try again */
if (bmp->initialized)
bmp->status = BMP085_STATUS_IDLE;
else
bmp->status = BMP085_STATUS_UNINIT;
bmp->i2c_trans.status = I2CTransDone;
}
}
sw/airborne/peripherals/bmp085.h
+81
View File
@@ -0,0 +1,81 @@
/*
* Copyright (C) 2010 Martin Mueller
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** @file peripherals/bmp085.h
* Bosch BMP085 driver interface.
*/
#ifndef BMP085_H
#define BMP085_H
#include "mcu_periph/i2c.h"
#include "std.h"
enum Bmp085Status {
BMP085_STATUS_UNINIT,
BMP085_STATUS_IDLE,
BMP085_STATUS_START_TEMP,
BMP085_STATUS_READ_TEMP,
BMP085_STATUS_START_PRESS,
BMP085_STATUS_READ_PRESS
};
struct Bmp085Calib {
// 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;
};
typedef bool_t (*Bmp085EOC)(void);
struct Bmp085 {
struct i2c_periph *i2c_p;
struct i2c_transaction i2c_trans;
Bmp085EOC eoc; ///< function to check End Of Conversion
enum Bmp085Status status; ///< state machine status
bool_t initialized; ///< config done flag
volatile bool_t data_available; ///< data ready flag
struct Bmp085Calib calib;
int32_t ut; ///< uncompensated temperature
int32_t up; ///< uncompensated pressure
int32_t temperature; ///< temperature in 0.1 deg Celcius
int32_t pressure; ///< pressure in Pascal
};
extern void bmp085_read_eeprom_calib(struct Bmp085* bmp);
extern void bmp085_init(struct Bmp085* bmp, struct i2c_periph *i2c_p, uint8_t addr);
extern void bmp085_periodic(struct Bmp085* bmp);
extern void bmp085_event(struct Bmp085* bmp);
#endif
sw/airborne/peripherals/bmp085_regs.h
+59
View File
@@ -0,0 +1,59 @@
/*
* Copyright (C) 2010 Martin Mueller
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** @file peripherals/bmp085_regs.h
* Bosch BMP085 register definitions.
*/
#ifndef BMP085_REGS_H
#define BMP085_REGS_H
#define BMP085_EEPROM_AC1 0xAA
#define BMP085_EEPROM_AC2 0xAC
#define BMP085_EEPROM_AC3 0xAE
#define BMP085_EEPROM_AC4 0xB0
#define BMP085_EEPROM_AC5 0xB2
#define BMP085_EEPROM_AC6 0xB4
#define BMP085_EEPROM_B1 0xB6
#define BMP085_EEPROM_B2 0xB8
#define BMP085_EEPROM_MB 0xBA
#define BMP085_EEPROM_MC 0xBC
#define BMP085_EEPROM_MD 0xBE
#define BMP085_CTRL_REG 0xF4
#define BMP085_START_TEMP 0x2E
#define BMP085_START_P0 0x34
#define BMP085_START_P1 0x74
#define BMP085_START_P2 0xB4
#define BMP085_START_P3 0xF4
#define BMP085_DAT_MSB 0xF6
#define BMP085_DAT_LSB 0xF7
#define BMP085_DAT_XLSB 0xF8
/// Over sample setting (0-3)
#define BMP085_OSS 3
#define BMP085_SLAVE_ADDR 0xEE
#endif
sw/airborne/peripherals/ms5611.c
+91
View File
@@ -0,0 +1,91 @@
/*
* Copyright (C) 2011 Martin Mueller <martinmm@pfump.org>
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file peripherals/ms5611.c
*
* MS5611 barometer driver common functions (I2C and SPI).
*/
#include "peripherals/ms5611.h"
#include "std.h"
/**
* Check if CRC of PROM data is OK.
* @return TRUE if OK, FALSE otherwise
*/
bool_t ms5611_prom_crc_ok(uint16_t* prom) {
int32_t i, j;
uint32_t res = 0;
uint8_t crc = prom[7] & 0xF;
prom[7] &= 0xFF00;
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 TRUE;
else
return FALSE;
}
/**
* Calculate temperature and compensated pressure.
*/
void ms5611_calc(struct Ms5611Data* ms) {
int64_t dt, tempms, off, sens, t2, off2, sens2;
/* difference between actual and ref temperature */
dt = ms->d2 - (int64_t)ms->c[5] * (1<<8);
/* actual temperature */
tempms = 2000 + ((int64_t)dt * ms->c[6]) / (1<<23);
/* offset at actual temperature */
off = ((int64_t)ms->c[2] * (1<<16)) + ((int64_t)ms->c[4] * dt) / (1<<7);
/* sensitivity at actual temperature */
sens = ((int64_t)ms->c[1] * (1<<15)) + ((int64_t)ms->c[3] * dt) / (1<<8);
/* second order temperature compensation */
if (tempms < 2000) {
t2 = (dt*dt) / (1<<31);
off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1);
sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2);
if (tempms < -1500) {
off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500);
sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1);
}
tempms = tempms - t2;
off = off - off2;
sens = sens - sens2;
}
/* temperature in deg Celsius with 0.01 degC resolultion */
ms->temperature = (int32_t)tempms;
/* temperature compensated pressure in Pascal (0.01mbar) */
ms->pressure = (uint32_t)((((int64_t)ms->d1 * sens) / (1<<21) - off) / (1<<15));
}
sw/airborne/peripherals/ms5611.h
+31 -70
View File
@@ -1,6 +1,5 @@
/*
*
* Copyright (C) 2012 Piotr Esden-Tempski
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
@@ -20,82 +19,44 @@
* Boston, MA 02111-1307, USA.
*/
/* Register definition for MS5611
/**
* @file peripherals/ms5611.h
*
* MS5611 barometer driver common interface (I2C and SPI).
*/
#ifndef MS5611_H
#define MS5611_H
/* default i2c address
* when CSB is set to GND addr is 0xEE
* when CSB is set to VCC addr is 0xEC
*
* Note: Aspirin 2.1 has CSB bound to GND.
*/
#define MS5611_SLAVE_ADDR 0xEE
#include "std.h"
/* FIXME: For backwards compatibility with Aspirin driver (it doesnt talk to baro either) */
#define MS5611_ADDR0 0x77
#define MS5611_ADDR1 0x76
/* Include address and register definition */
#include "peripherals/ms5611_regs.h"
/* SPI SLAVE3 is on pin PC13
* Aspirin 2.2 has ms5611 on SPI bus
*/
#ifndef MS5611_SLAVE_DEV
#define MS5611_SLAVE_DEV SPI_SLAVE3
#endif
/* Number of 16bit calibration coefficients */
#define PROM_NB 8
/* OSR definitions */
#define MS5611_OSR256 0x02
#define MS5611_OSR512 0x02
#define MS5611_OSR1024 0x04
#define MS5611_OSR2048 0x06
#define MS5611_OSR4096 0x08
/* D1 Register defines */
#define MS5611_REG_D1R 0x40 // Request D1 (pressure) conversion
#define MS5611_REG_D1(_osr) (MS5611_REG_D1R | _osr)
#define MS5611_REG_D1OSR256 MS5611_REG_D1(MS5611_ORS256)
#define MS5611_REG_D1OSR512 MS5611_REG_D1(MS5611_OSR512)
#define MS5611_REG_D1OSR1024 MS5611_REG_D1(MS5611_OSR1024)
#define MS5611_REG_D1OSR2048 MS5611_REG_D1(MS5611_OSR2048)
#define MS5611_REG_D1OSR4096 MS5611_REG_D1(MS5611_OSR4096)
/* D2 register defines */
#define MS5611_REG_D2R 0x50 // Request D2 (temperature) conversion
#define MS5611_REG_D2(_osr) (MS5611_REG_D2R | _osr)
#define MS5611_REG_D2OSR256 MS5611_REG_D2(MS5611_ORS256)
#define MS5611_REG_D2OSR512 MS5611_REG_D2(MS5611_OSR512)
#define MS5611_REG_D2OSR1024 MS5611_REG_D2(MS5611_OSR1024)
#define MS5611_REG_D2OSR2048 MS5611_REG_D2(MS5611_OSR2048)
#define MS5611_REG_D2OSR4096 MS5611_REG_D2(MS5611_OSR4096)
/* Commands */
#define MS5611_ADC_READ 0x00 // Read converted value
#define MS5611_SOFT_RESET 0x1E // Reset command
#define MS5611_PROM_READ 0xA0 // Start reading PROM
#define MS5611_START_CONV_D1 MS5611_REG_D1OSR4096 /* we use OSR=4096 for maximum resolution */
#define MS5611_START_CONV_D2 MS5611_REG_D2OSR4096 /* we use OSR=4096 for maximum resolution */
/* FIXME: backwards compatibility with Aspirin driver */
#define MS5611_REG_RESET MS5611_SOFT_RESET
#define MS5611_REG_ADCREAD MS5611_ADC_READ
enum ms5611_stat{
MS5611_UNINIT,
MS5611_RESET,
MS5611_RESET_OK,
MS5611_PROM,
MS5611_IDLE,
MS5611_CONV_D1,
MS5611_CONV_D1_OK,
MS5611_ADC_D1,
MS5611_CONV_D2,
MS5611_CONV_D2_OK,
MS5611_ADC_D2
enum Ms5611Status {
MS5611_STATUS_UNINIT,
MS5611_STATUS_RESET,
MS5611_STATUS_RESET_OK,
MS5611_STATUS_PROM,
MS5611_STATUS_IDLE,
MS5611_STATUS_CONV_D1,
MS5611_STATUS_CONV_D1_OK,
MS5611_STATUS_ADC_D1,
MS5611_STATUS_CONV_D2,
MS5611_STATUS_CONV_D2_OK,
MS5611_STATUS_ADC_D2
};
struct Ms5611Data {
uint32_t pressure; ///< pressure in Pascal (0.01mbar)
int32_t temperature; ///< temperature with 0.01 degrees Celsius resolution
uint16_t c[PROM_NB];
uint32_t d1;
uint32_t d2;
};
extern bool_t ms5611_prom_crc_ok(uint16_t* prom);
extern void ms5611_calc(struct Ms5611Data* ms);
#endif /* MS5611_H */
sw/airborne/peripherals/ms5611_i2c.c
+205
View File
@@ -0,0 +1,205 @@
/*
* Copyright (C) 2011 Martin Mueller <martinmm@pfump.org>
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file peripherals/ms5611_i2c.c
* Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C.
*
*/
#include "peripherals/ms5611_i2c.h"
void ms5611_i2c_init(struct Ms5611_I2c *ms, struct i2c_periph *i2c_p, uint8_t addr)
{
/* set i2c_peripheral */
ms->i2c_p = i2c_p;
/* slave address */
ms->i2c_trans.slave_addr = addr;
/* set initial status: Success or Done */
ms->i2c_trans.status = I2CTransDone;
ms->data_available = FALSE;
ms->initialized = FALSE;
ms->status = MS5611_STATUS_UNINIT;
ms->prom_cnt = 0;
}
void ms5611_i2c_start_configure(struct Ms5611_I2c *ms)
{
if (ms->status == MS5611_STATUS_UNINIT) {
ms->initialized = FALSE;
ms->prom_cnt = 0;
ms->i2c_trans.buf[0] = MS5611_SOFT_RESET;
i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1);
ms->status = MS5611_STATUS_RESET;
}
}
void ms5611_i2c_start_conversion(struct Ms5611_I2c *ms)
{
if (ms->status == MS5611_STATUS_IDLE &&
ms->i2c_trans.status == I2CTransDone) {
/* start D1 conversion */
ms->i2c_trans.buf[0] = MS5611_START_CONV_D1;
i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1);
ms->status = MS5611_STATUS_CONV_D1;
}
}
/**
* Periodic function to ensure proper delay after triggering reset or conversion.
* Should run at 100Hz max.
* Typical conversion time is 8.22ms at max resolution.
*/
void ms5611_i2c_periodic_check(struct Ms5611_I2c *ms)
{
switch (ms->status) {
case MS5611_STATUS_RESET:
ms->status = MS5611_STATUS_RESET_OK;
break;
case MS5611_STATUS_RESET_OK:
if (ms->i2c_trans.status == I2CTransDone) {
/* start getting prom data */
ms->i2c_trans.buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 2);
ms->status = MS5611_STATUS_PROM;
}
break;
case MS5611_STATUS_CONV_D1:
ms->status = MS5611_STATUS_CONV_D1_OK;
break;
case MS5611_STATUS_CONV_D1_OK:
if (ms->i2c_trans.status == I2CTransDone) {
/* read D1 adc */
ms->i2c_trans.buf[0] = MS5611_ADC_READ;
i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 3);
ms->status = MS5611_STATUS_ADC_D1;
}
break;
case MS5611_STATUS_CONV_D2:
ms->status = MS5611_STATUS_CONV_D2_OK;
break;
case MS5611_STATUS_CONV_D2_OK:
if (ms->i2c_trans.status == I2CTransDone) {
/* read D2 adc */
ms->i2c_trans.buf[0] = MS5611_ADC_READ;
i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 3);
ms->status = MS5611_STATUS_ADC_D2;
}
break;
default:
break;
}
}
void ms5611_i2c_event(struct Ms5611_I2c *ms) {
if (ms->initialized) {
if (ms->i2c_trans.status == I2CTransFailed) {
ms->status = MS5611_STATUS_IDLE;
ms->i2c_trans.status = I2CTransDone;
}
else if (ms->i2c_trans.status == I2CTransSuccess) {
// Successfull reading
switch (ms->status) {
case MS5611_STATUS_ADC_D1:
/* read D1 (pressure) */
ms->data.d1 = (ms->i2c_trans.buf[0] << 16) |
(ms->i2c_trans.buf[1] << 8) |
ms->i2c_trans.buf[2];
if (ms->data.d1 == 0) {
/* if value is zero, it was read to soon and is invalid, back to idle */
ms->status = MS5611_STATUS_IDLE;
}
else {
/* start D2 conversion */
ms->i2c_trans.buf[0] = MS5611_START_CONV_D2;
i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1);
ms->status = MS5611_STATUS_CONV_D2;
}
break;
case MS5611_STATUS_ADC_D2:
/* read D2 (temperature) */
ms->data.d2 = (ms->i2c_trans.buf[0] << 16) |
(ms->i2c_trans.buf[1] << 8) |
ms->i2c_trans.buf[2];
if (ms->data.d2 == 0) {
/* if value is zero, it was read to soon and is invalid, back to idle */
ms->status = MS5611_STATUS_IDLE;
}
else {
/* calculate temp and pressure from measurements */
ms5611_calc(&(ms->data));
ms->status = MS5611_STATUS_IDLE;
ms->data_available = TRUE;
}
break;
default:
break;
}
ms->i2c_trans.status = I2CTransDone;
}
}
else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized
switch (ms->i2c_trans.status) {
case I2CTransFailed:
/* try again */
ms->status = MS5611_STATUS_UNINIT;
ms->i2c_trans.status = I2CTransDone;
break;
case I2CTransSuccess:
if (ms->status == MS5611_STATUS_PROM) {
/* read prom data */
ms->data.c[ms->prom_cnt++] = (ms->i2c_trans.buf[0] << 8) |
ms->i2c_trans.buf[1];
if (ms->prom_cnt < PROM_NB) {
/* get next prom data */
ms->i2c_trans.buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 2);
}
else {
/* done reading prom, check prom crc */
if (ms5611_prom_crc_ok(ms->data.c)) {
ms->initialized = TRUE;
ms->status = MS5611_STATUS_IDLE;
}
else {
/* checksum error, try again */
ms->status = MS5611_STATUS_UNINIT;
}
}
}
ms->i2c_trans.status = I2CTransDone;
break;
default:
break;
}
}
}
sw/airborne/peripherals/ms5611_i2c.h
+71
View File
@@ -0,0 +1,71 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file peripherals/ms5611_i2c.h
*
* Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C.
*/
#ifndef MS5611_I2C_H
#define MS5611_I2C_H
#include "mcu_periph/i2c.h"
/* Include common MS5611 definitions */
#include "peripherals/ms5611.h"
struct Ms5611_I2c {
struct i2c_periph *i2c_p;
struct i2c_transaction i2c_trans;
enum Ms5611Status status;
bool_t initialized; ///< config done flag
volatile bool_t data_available; ///< data ready flag
struct Ms5611Data data;
int32_t prom_cnt; ///< number of bytes read from PROM
};
// Functions
extern void ms5611_i2c_init(struct Ms5611_I2c* ms, struct i2c_periph* i2c_p, uint8_t addr);
extern void ms5611_i2c_start_configure(struct Ms5611_I2c* ms);
extern void ms5611_i2c_start_conversion(struct Ms5611_I2c* ms);
extern void ms5611_i2c_periodic_check(struct Ms5611_I2c* ms);
extern void ms5611_i2c_event(struct Ms5611_I2c* ms);
/** convenience function to trigger new measurement.
* (or start configuration if not already initialized)
* Still need to regularly run ms5611_i2c_periodic_check to complete the measurement.
*/
static inline void ms5611_i2c_read(struct Ms5611_I2c* ms) {
if (ms->initialized)
ms5611_i2c_start_conversion(ms);
else
ms5611_i2c_start_configure(ms);
}
/// convenience function
static inline void ms5611_i2c_periodic(struct Ms5611_I2c* ms) {
ms5611_i2c_read(ms);
ms5611_i2c_periodic_check(ms);
}
#endif /* MS5611_I2C_H */
sw/airborne/peripherals/ms5611_regs.h
+72
View File
@@ -0,0 +1,72 @@
/*
* Copyright (C) 2012 Piotr Esden-Tempski
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file peripherals/ms5611_regs.h
* Register definitions for MS5611 barometer.
*/
#ifndef MS5611_REGS_H
#define MS5611_REGS_H
/** default i2c address
* when CSB is set to GND addr is 0xEE
* when CSB is set to VCC addr is 0xEC
*/
#define MS5611_I2C_SLAVE_ADDR 0xEE
#define MS5611_I2C_SLAVE_ADDR_ALT 0xEC
/* Number of 16bit calibration coefficients */
#define PROM_NB 8
/* OSR definitions */
#define MS5611_OSR256 0x02
#define MS5611_OSR512 0x02
#define MS5611_OSR1024 0x04
#define MS5611_OSR2048 0x06
#define MS5611_OSR4096 0x08
/* D1 Register defines */
#define MS5611_REG_D1R 0x40 // Request D1 (pressure) conversion
#define MS5611_REG_D1(_osr) (MS5611_REG_D1R | _osr)
#define MS5611_REG_D1OSR256 MS5611_REG_D1(MS5611_ORS256)
#define MS5611_REG_D1OSR512 MS5611_REG_D1(MS5611_OSR512)
#define MS5611_REG_D1OSR1024 MS5611_REG_D1(MS5611_OSR1024)
#define MS5611_REG_D1OSR2048 MS5611_REG_D1(MS5611_OSR2048)
#define MS5611_REG_D1OSR4096 MS5611_REG_D1(MS5611_OSR4096)
/* D2 register defines */
#define MS5611_REG_D2R 0x50 // Request D2 (temperature) conversion
#define MS5611_REG_D2(_osr) (MS5611_REG_D2R | _osr)
#define MS5611_REG_D2OSR256 MS5611_REG_D2(MS5611_ORS256)
#define MS5611_REG_D2OSR512 MS5611_REG_D2(MS5611_OSR512)
#define MS5611_REG_D2OSR1024 MS5611_REG_D2(MS5611_OSR1024)
#define MS5611_REG_D2OSR2048 MS5611_REG_D2(MS5611_OSR2048)
#define MS5611_REG_D2OSR4096 MS5611_REG_D2(MS5611_OSR4096)
/* Commands */
#define MS5611_ADC_READ 0x00 // Read converted value
#define MS5611_SOFT_RESET 0x1E // Reset command
#define MS5611_PROM_READ 0xA0 // Start reading PROM
#define MS5611_START_CONV_D1 MS5611_REG_D1OSR4096 /* we use OSR=4096 for maximum resolution */
#define MS5611_START_CONV_D2 MS5611_REG_D2OSR4096 /* we use OSR=4096 for maximum resolution */
#endif /* MS5611_REGS_H */
sw/airborne/peripherals/ms5611_spi.c
+219
View File
@@ -0,0 +1,219 @@
/*
* Copyright (C) 2011 Martin Mueller <martinmm@pfump.org>
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file peripherals/ms5611_spi.c
* Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for SPI.
*
*/
#include "peripherals/ms5611_spi.h"
void ms5611_spi_init(struct Ms5611_Spi *ms, struct spi_periph *spi_p, uint8_t slave_idx)
{
/* set spi_peripheral */
ms->spi_p = spi_p;
/* configure spi transaction */
ms->spi_trans.cpol = SPICpolIdleHigh;
ms->spi_trans.cpha = SPICphaEdge2;
ms->spi_trans.dss = SPIDss8bit;
ms->spi_trans.bitorder = SPIMSBFirst;
ms->spi_trans.cdiv = SPIDiv64;
ms->spi_trans.select = SPISelectUnselect;
ms->spi_trans.slave_idx = slave_idx;
ms->spi_trans.output_length = 1;
ms->spi_trans.input_length = 4;
ms->spi_trans.before_cb = NULL;
ms->spi_trans.after_cb = NULL;
ms->spi_trans.input_buf = ms->rx_buf;
ms->spi_trans.output_buf = ms->tx_buf;
/* set initial status: Success or Done */
ms->spi_trans.status = SPITransDone;
ms->data_available = FALSE;
ms->initialized = FALSE;
ms->status = MS5611_STATUS_UNINIT;
ms->prom_cnt = 0;
}
void ms5611_spi_start_configure(struct Ms5611_Spi *ms)
{
if (ms->status == MS5611_STATUS_UNINIT) {
ms->initialized = FALSE;
ms->prom_cnt = 0;
ms->tx_buf[0] = MS5611_SOFT_RESET;
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_RESET;
}
}
void ms5611_spi_start_conversion(struct Ms5611_Spi *ms)
{
if (ms->status == MS5611_STATUS_IDLE &&
ms->spi_trans.status == SPITransDone) {
/* start D1 conversion */
ms->tx_buf[0] = MS5611_START_CONV_D1;
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_CONV_D1;
}
}
/**
* Periodic function to ensure proper delay after triggering reset or conversion.
* Should run at 100Hz max.
* Typical conversion time is 8.22ms at max resolution.
*/
void ms5611_spi_periodic_check(struct Ms5611_Spi *ms)
{
switch (ms->status) {
case MS5611_STATUS_RESET:
ms->status = MS5611_STATUS_RESET_OK;
break;
case MS5611_STATUS_RESET_OK:
if (ms->spi_trans.status == SPITransDone) {
/* start getting prom data */
ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_PROM;
}
break;
case MS5611_STATUS_CONV_D1:
ms->status = MS5611_STATUS_CONV_D1_OK;
break;
case MS5611_STATUS_CONV_D1_OK:
if (ms->spi_trans.status == SPITransDone) {
/* read D1 adc */
ms->tx_buf[0] = MS5611_ADC_READ;
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_ADC_D1;
}
break;
case MS5611_STATUS_CONV_D2:
ms->status = MS5611_STATUS_CONV_D2_OK;
break;
case MS5611_STATUS_CONV_D2_OK:
if (ms->spi_trans.status == SPITransDone) {
/* read D2 adc */
ms->tx_buf[0] = MS5611_ADC_READ;
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_ADC_D2;
}
break;
default:
break;
}
}
void ms5611_spi_event(struct Ms5611_Spi *ms) {
if (ms->initialized) {
if (ms->spi_trans.status == SPITransFailed) {
ms->status = MS5611_STATUS_IDLE;
ms->spi_trans.status = SPITransDone;
}
else if (ms->spi_trans.status == SPITransSuccess) {
// Successfull reading
switch (ms->status) {
case MS5611_STATUS_ADC_D1:
/* read D1 (pressure) */
ms->data.d1 = (ms->rx_buf[1] << 16) |
(ms->rx_buf[2] << 8) |
ms->rx_buf[3];
if (ms->data.d1 == 0) {
/* if value is zero, it was read to soon and is invalid, back to idle */
ms->status = MS5611_STATUS_IDLE;
}
else {
/* start D2 conversion */
ms->tx_buf[0] = MS5611_START_CONV_D2;
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_CONV_D2;
}
break;
case MS5611_STATUS_ADC_D2:
/* read D2 (temperature) */
ms->data.d2 = (ms->rx_buf[1] << 16) |
(ms->rx_buf[2] << 8) |
ms->rx_buf[3];
if (ms->data.d2 == 0) {
/* if value is zero, it was read to soon and is invalid, back to idle */
ms->status = MS5611_STATUS_IDLE;
}
else {
/* calculate temp and pressure from measurements */
ms5611_calc(&(ms->data));
ms->status = MS5611_STATUS_IDLE;
ms->data_available = TRUE;
}
break;
default:
break;
}
ms->spi_trans.status = SPITransDone;
}
}
else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized
switch (ms->spi_trans.status) {
case SPITransFailed:
/* try again */
ms->status = MS5611_STATUS_UNINIT;
ms->spi_trans.status = SPITransDone;
break;
case SPITransSuccess:
if (ms->status == MS5611_STATUS_PROM) {
/* read prom data */
ms->data.c[ms->prom_cnt++] = (ms->rx_buf[1] << 8) |
ms->rx_buf[2];
if (ms->prom_cnt < PROM_NB) {
/* get next prom data */
ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
spi_submit(ms->spi_p, &(ms->spi_trans));
}
else {
/* done reading prom, check prom crc */
if (ms5611_prom_crc_ok(ms->data.c)) {
ms->initialized = TRUE;
ms->status = MS5611_STATUS_IDLE;
}
else {
/* checksum error, try again */
ms->status = MS5611_STATUS_UNINIT;
}
}
}
ms->spi_trans.status = SPITransDone;
break;
default:
break;
}
}
}
sw/airborne/peripherals/ms5611_spi.h
+73
View File
@@ -0,0 +1,73 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file peripherals/ms5611_spi.h
*
* Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for SPI.
*/
#ifndef MS5611_SPI_H
#define MS5611_SPI_H
#include "mcu_periph/spi.h"
/* Include common MS5611 definitions */
#include "peripherals/ms5611.h"
struct Ms5611_Spi {
struct spi_periph *spi_p;
struct spi_transaction spi_trans;
volatile uint8_t tx_buf[1];
volatile uint8_t rx_buf[4];
enum Ms5611Status status;
bool_t initialized; ///< config done flag
volatile bool_t data_available; ///< data ready flag
struct Ms5611Data data;
int32_t prom_cnt; ///< number of bytes read from PROM
};
// Functions
extern void ms5611_spi_init(struct Ms5611_Spi* ms, struct spi_periph* spi_p, uint8_t addr);
extern void ms5611_spi_start_configure(struct Ms5611_Spi* ms);
extern void ms5611_spi_start_conversion(struct Ms5611_Spi* ms);
extern void ms5611_spi_periodic_check(struct Ms5611_Spi* ms);
extern void ms5611_spi_event(struct Ms5611_Spi* ms);
/** convenience function to trigger new measurement.
* (or start configuration if not already initialized)
* Still need to regularly run ms5611_spi_periodic_check to complete the measurement.
*/
static inline void ms5611_spi_read(struct Ms5611_Spi* ms) {
if (ms->initialized)
ms5611_spi_start_conversion(ms);
else
ms5611_spi_start_configure(ms);
}
/// convenience function
static inline void ms5611_spi_periodic(struct Ms5611_Spi* ms) {
ms5611_spi_read(ms);
ms5611_spi_periodic_check(ms);
}
#endif /* MS5611_SPI_H */
sw/airborne/subsystems/sensors/baro_ms5611_i2c.c
+120
View File
@@ -0,0 +1,120 @@
/*
* Copyright (C) 2011-2013 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/**
* @file boards/lisa_m/baro_ms5611_i2c.c
*
* Driver for MS5611 baro via I2C.
*
*/
#include "subsystems/sensors/baro.h"
#include "peripherals/ms5611_i2c.h"
#include "mcu_periph/sys_time.h"
#include "led.h"
#include "std.h"
#ifndef MS5611_I2C_DEV
#define MS5611_I2C_DEV i2c2
#endif
/* default i2c address
* when CSB is set to GND addr is 0xEE
* when CSB is set to VCC addr is 0xEC
*
* Note: Aspirin 2.1 has CSB bound to GND.
*/
#ifndef MS5611_SLAVE_ADDR
#define MS5611_SLAVE_ADDR 0xEE
#endif
#ifdef DEBUG
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
float fbaroms, ftempms;
#endif
struct Baro baro;
struct Ms5611_I2c baro_ms5611;
void baro_init(void) {
baro.status = BS_UNINITIALIZED;
baro.absolute = 0;
baro.differential = 0;
ms5611_i2c_init(&baro_ms5611, &MS5611_I2C_DEV, MS5611_SLAVE_ADDR);
}
void baro_periodic(void) {
if (sys_time.nb_sec > 1) {
/* call the convenience periodic that initializes the sensor and starts reading*/
ms5611_i2c_periodic(&baro_ms5611);
if (baro_ms5611.initialized) {
baro.status = BS_RUNNING;
#if DEBUG
RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
&baro_ms5611.data.c[0],
&baro_ms5611.data.c[1],
&baro_ms5611.data.c[2],
&baro_ms5611.data.c[3],
&baro_ms5611.data.c[4],
&baro_ms5611.data.c[5],
&baro_ms5611.data.c[6],
&baro_ms5611.data.c[7]));
#endif
}
}
}
void baro_event(void (*b_abs_handler)(void)){
if (sys_time.nb_sec > 1) {
ms5611_i2c_event(&baro_ms5611);
if (baro_ms5611.data_available) {
baro.absolute = baro_ms5611.data.pressure;
b_abs_handler();
baro_ms5611.data_available = FALSE;
#ifdef ROTORCRAFT_BARO_LED
RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
#endif
#if DEBUG
ftempms = baro_ms5611.data.temperature / 100.;
fbaroms = baro_ms5611.data.pressure / 100.;
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
&baro_ms5611.data.d1, &baro_ms5611.data.d2,
&fbaroms, &ftempms);
#endif
}
}
}
sw/airborne/subsystems/sensors/baro_ms5611_spi.c
+118
View File
@@ -0,0 +1,118 @@
/*
* Copyright (C) 2011-2013 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/**
* @file boards/lisa_m/baro_ms5611_spi.c
*
* Driver for MS5611 baro on LisaM/Aspirin2.2 via SPI.
*
*/
#include "subsystems/sensors/baro.h"
#include "peripherals/ms5611_spi.h"
#include "mcu_periph/sys_time.h"
#include "led.h"
#include "std.h"
#ifndef MS5611_SPI_DEV
#define MS5611_SPI_DEV spi2
#endif
/* SPI SLAVE3 is on pin PC13
* Aspirin 2.2 has ms5611 on SPI bus
*/
#ifndef MS5611_SLAVE_DEV
#define MS5611_SLAVE_DEV SPI_SLAVE3
#endif
#ifdef DEBUG
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
float fbaroms, ftempms;
#endif
struct Baro baro;
struct Ms5611_Spi baro_ms5611;
void baro_init(void) {
baro.status = BS_UNINITIALIZED;
baro.absolute = 0;
baro.differential = 0;
ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_DEV);
}
void baro_periodic(void) {
if (sys_time.nb_sec > 1) {
/* call the convenience periodic that initializes the sensor and starts reading*/
ms5611_spi_periodic(&baro_ms5611);
if (baro_ms5611.initialized) {
baro.status = BS_RUNNING;
#if DEBUG
RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
&baro_ms5611.data.c[0],
&baro_ms5611.data.c[1],
&baro_ms5611.data.c[2],
&baro_ms5611.data.c[3],
&baro_ms5611.data.c[4],
&baro_ms5611.data.c[5],
&baro_ms5611.data.c[6],
&baro_ms5611.data.c[7]));
#endif
}
}
}
void baro_event(void (*b_abs_handler)(void)){
if (sys_time.nb_sec > 1) {
ms5611_spi_event(&baro_ms5611);
if (baro_ms5611.data_available) {
baro.absolute = baro_ms5611.data.pressure;
b_abs_handler();
baro_ms5611.data_available = FALSE;
#ifdef ROTORCRAFT_BARO_LED
RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
#endif
#if DEBUG
ftempms = baro_ms5611.data.temperature / 100.;
fbaroms = baro_ms5611.data.pressure / 100.;
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
&baro_ms5611.data.d1, &baro_ms5611.data.d2,
&fbaroms, &ftempms);
#endif
}
}
}