diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index 58b2b6187e..b5cd13d64e 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -172,13 +172,18 @@ 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 + ap.CFLAGS += -DUSE_I2C2 endif ap.CFLAGS += -D$(LISA_M_BARO) @@ -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) diff --git a/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile b/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile index c2a7e83fe0..878dadfac1 100644 --- a/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile +++ b/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile @@ -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 diff --git a/conf/modules/baro_bmp.xml b/conf/modules/baro_bmp.xml index 0fa9cc1edf..1bf6bde874 100644 --- a/conf/modules/baro_bmp.xml +++ b/conf/modules/baro_bmp.xml @@ -10,10 +10,11 @@ - + - + + diff --git a/conf/modules/baro_ms5611_i2c.xml b/conf/modules/baro_ms5611_i2c.xml index 412c725338..027cd4061c 100644 --- a/conf/modules/baro_ms5611_i2c.xml +++ b/conf/modules/baro_ms5611_i2c.xml @@ -13,12 +13,13 @@ - - - + + + + diff --git a/conf/modules/baro_ms5611_spi.xml b/conf/modules/baro_ms5611_spi.xml new file mode 100644 index 0000000000..2ec153e554 --- /dev/null +++ b/conf/modules/baro_ms5611_spi.xml @@ -0,0 +1,25 @@ + + + + + + Baro MS5611 (SPI) + Measurement Specialties MS5611-01BA pressure sensor (SPI) + + + + + +
+ +
+ + + + + + + + + + diff --git a/sw/airborne/boards/krooz/baro_board.c b/sw/airborne/boards/krooz/baro_board.c deleted file mode 100644 index bc042c6cd0..0000000000 --- a/sw/airborne/boards/krooz/baro_board.c +++ /dev/null @@ -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; - } -} diff --git a/sw/airborne/boards/krooz/baro_board.h b/sw/airborne/boards/krooz/baro_board.h index b73f33d90d..5f47ad650e 100644 --- a/sw/airborne/boards/krooz/baro_board.h +++ b/sw/airborne/boards/krooz/baro_board.h @@ -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 */ diff --git a/sw/airborne/boards/lia/baro_board.h b/sw/airborne/boards/lia/baro_board.h index d65c11d266..59782132c8 100644 --- a/sw/airborne/boards/lia/baro_board.h +++ b/sw/airborne/boards/lia/baro_board.h @@ -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 */ diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index b51743c30a..9c09b97d90 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -1,195 +1,81 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * 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 +#include "peripherals/bmp085_regs.h" +#include + +#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; - } - 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; + bmp085_periodic(&baro_bmp085); } - + 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); - 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(); - } + if (baro_bmp085.data_available) { + baro.absolute = baro_bmp085.pressure; + b_abs_handler(); + baro_bmp085.data_available = FALSE; + +#ifdef ROTORCRAFT_BARO_LED + RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED)); +#endif } } diff --git a/sw/airborne/boards/lisa_m/baro_board.h b/sw/airborne/boards/lisa_m/baro_board.h index 42014ddeee..b84a11a7da 100644 --- a/sw/airborne/boards/lisa_m/baro_board.h +++ b/sw/airborne/boards/lisa_m/baro_board.h @@ -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 */ diff --git a/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c b/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c deleted file mode 100644 index 10994d5948..0000000000 --- a/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c +++ /dev/null @@ -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; - } - } -} diff --git a/sw/airborne/boards/lisa_m/baro_ms5611_spi.c b/sw/airborne/boards/lisa_m/baro_ms5611_spi.c deleted file mode 100644 index bd437cd318..0000000000 --- a/sw/airborne/boards/lisa_m/baro_ms5611_spi.c +++ /dev/null @@ -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; - } - } -} diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c index bdfce3a3af..5011b7d075 100644 --- a/sw/airborne/modules/sensors/baro_bmp.c +++ b/sw/airborne/modules/sensors/baro_bmp.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2010 Martin Mueller + * Copyright (C) 2013 Felix Ruess * * 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. + * 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.initialized) + bmp085_periodic(&baro_bmp); + else + bmp085_read_eeprom_calib(&baro_bmp); - 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; +void baro_bmp_event(void) { - /* 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); + bmp085_event(&baro_bmp); - /* 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; - 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); + if (baro_bmp.data_available) { - 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); - - 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 - - baro_bmp_temp = (baro_bmp - baro_bmp_offset); - - 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; - } - } } } diff --git a/sw/airborne/modules/sensors/baro_bmp.h b/sw/airborne/modules/sensors/baro_bmp.h index 860937bfe6..db1039e817 100644 --- a/sw/airborne/modules/sensors/baro_bmp.h +++ b/sw/airborne/modules/sensors/baro_bmp.h @@ -1,42 +1,43 @@ +/* + * Copyright (C) 2010 Martin Mueller + * Copyright (C) 2013 Felix Ruess + * + * 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 diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index e7e2f1c423..d3b3a3d88c 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2011 Martin Mueller + * 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); - 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); - } +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, + &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.; - DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, - &ms5611_d1, &ms5611_d2, &fbaroms, &ftempms); + 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 - - break; - } - - default: - ms5611_trans.status = I2CTransDone; - break; - } } } diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.h b/sw/airborne/modules/sensors/baro_ms5611_i2c.h index 2f34e8fcff..f30b28debc 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.h +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.h @@ -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 diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c new file mode 100644 index 0000000000..c91d713153 --- /dev/null +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -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 + } +} diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.h b/sw/airborne/modules/sensors/baro_ms5611_spi.h new file mode 100644 index 0000000000..7bab9d92e8 --- /dev/null +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2011 Martin Mueller + * + * 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 diff --git a/sw/airborne/peripherals/bmp085.c b/sw/airborne/peripherals/bmp085.c new file mode 100644 index 0000000000..2c123ef592 --- /dev/null +++ b/sw/airborne/peripherals/bmp085.c @@ -0,0 +1,196 @@ +/* + * Copyright (C) 2010 Martin Mueller + * Copyright (C) 2013 Felix Ruess + * + * 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; + } +} diff --git a/sw/airborne/peripherals/bmp085.h b/sw/airborne/peripherals/bmp085.h new file mode 100644 index 0000000000..a335f1e3b5 --- /dev/null +++ b/sw/airborne/peripherals/bmp085.h @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2010 Martin Mueller + * Copyright (C) 2013 Felix Ruess + * + * 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 diff --git a/sw/airborne/peripherals/bmp085_regs.h b/sw/airborne/peripherals/bmp085_regs.h new file mode 100644 index 0000000000..fb94b6a67d --- /dev/null +++ b/sw/airborne/peripherals/bmp085_regs.h @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2010 Martin Mueller + * Copyright (C) 2013 Felix Ruess + * + * 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 diff --git a/sw/airborne/peripherals/ms5611.c b/sw/airborne/peripherals/ms5611.c new file mode 100644 index 0000000000..9956d6583b --- /dev/null +++ b/sw/airborne/peripherals/ms5611.c @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2011 Martin Mueller + * Copyright (C) 2013 Felix Ruess + * + * 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)); +} diff --git a/sw/airborne/peripherals/ms5611.h b/sw/airborne/peripherals/ms5611.h index ad3054e776..85958d5ac5 100644 --- a/sw/airborne/peripherals/ms5611.h +++ b/sw/airborne/peripherals/ms5611.h @@ -1,6 +1,5 @@ /* - * - * Copyright (C) 2012 Piotr Esden-Tempski + * Copyright (C) 2013 Felix Ruess * * 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 */ diff --git a/sw/airborne/peripherals/ms5611_i2c.c b/sw/airborne/peripherals/ms5611_i2c.c new file mode 100644 index 0000000000..1ec14b4611 --- /dev/null +++ b/sw/airborne/peripherals/ms5611_i2c.c @@ -0,0 +1,205 @@ +/* + * Copyright (C) 2011 Martin Mueller + * Copyright (C) 2013 Felix Ruess + * + * 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; + } + } +} diff --git a/sw/airborne/peripherals/ms5611_i2c.h b/sw/airborne/peripherals/ms5611_i2c.h new file mode 100644 index 0000000000..149312ff36 --- /dev/null +++ b/sw/airborne/peripherals/ms5611_i2c.h @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * 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 */ diff --git a/sw/airborne/peripherals/ms5611_regs.h b/sw/airborne/peripherals/ms5611_regs.h new file mode 100644 index 0000000000..6cd3eb1fe2 --- /dev/null +++ b/sw/airborne/peripherals/ms5611_regs.h @@ -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 */ diff --git a/sw/airborne/peripherals/ms5611_spi.c b/sw/airborne/peripherals/ms5611_spi.c new file mode 100644 index 0000000000..d8e9293185 --- /dev/null +++ b/sw/airborne/peripherals/ms5611_spi.c @@ -0,0 +1,219 @@ +/* + * Copyright (C) 2011 Martin Mueller + * Copyright (C) 2013 Felix Ruess + * + * 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; + } + } +} diff --git a/sw/airborne/peripherals/ms5611_spi.h b/sw/airborne/peripherals/ms5611_spi.h new file mode 100644 index 0000000000..59f8f6e8e8 --- /dev/null +++ b/sw/airborne/peripherals/ms5611_spi.h @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * 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 */ diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c new file mode 100644 index 0000000000..a996cee2dc --- /dev/null +++ b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c @@ -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 + } + } +} diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c new file mode 100644 index 0000000000..a86ab818f7 --- /dev/null +++ b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c @@ -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 + } + } +}