mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +08:00
[boards] use ms5611 peripheral in lisa_m baro
This commit is contained in:
Felix Ruess
@@ -172,9 +172,13 @@ LISA_M_BARO ?= BARO_BOARD_BMP085
|
|||||||
ifeq ($(LISA_M_BARO), BARO_MS5611_SPI)
|
ifeq ($(LISA_M_BARO), BARO_MS5611_SPI)
|
||||||
include $(CFG_SHARED)/spi_master.makefile
|
include $(CFG_SHARED)/spi_master.makefile
|
||||||
ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3
|
ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3
|
||||||
|
ap.srcs += peripherals/ms5611.c
|
||||||
|
ap.srcs += peripherals/ms5611_spi.c
|
||||||
ap.srcs += $(SRC_BOARD)/baro_ms5611_spi.c
|
ap.srcs += $(SRC_BOARD)/baro_ms5611_spi.c
|
||||||
else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C)
|
else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C)
|
||||||
ap.CFLAGS += -DUSE_I2C2
|
ap.CFLAGS += -DUSE_I2C2
|
||||||
|
ap.srcs += peripherals/ms5611.c
|
||||||
|
ap.srcs += peripherals/ms5611_i2c.c
|
||||||
ap.srcs += $(SRC_BOARD)/baro_ms5611_i2c.c
|
ap.srcs += $(SRC_BOARD)/baro_ms5611_i2c.c
|
||||||
else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085)
|
else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085)
|
||||||
ap.srcs += $(SRC_BOARD)/baro_board.c
|
ap.srcs += $(SRC_BOARD)/baro_board.c
|
||||||
@@ -189,9 +193,13 @@ LIA_BARO ?= BARO_MS5611_SPI
|
|||||||
ifeq ($(LIA_BARO), BARO_MS5611_SPI)
|
ifeq ($(LIA_BARO), BARO_MS5611_SPI)
|
||||||
include $(CFG_SHARED)/spi_master.makefile
|
include $(CFG_SHARED)/spi_master.makefile
|
||||||
ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3
|
ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3
|
||||||
|
ap.srcs += peripherals/ms5611.c
|
||||||
|
ap.srcs += peripherals/ms5611_spi.c
|
||||||
ap.srcs += boards/lisa_m/baro_ms5611_spi.c
|
ap.srcs += boards/lisa_m/baro_ms5611_spi.c
|
||||||
else ifeq ($(LIA_BARO), BARO_MS5611_I2C)
|
else ifeq ($(LIA_BARO), BARO_MS5611_I2C)
|
||||||
ap.CFLAGS += -DUSE_I2C2
|
ap.CFLAGS += -DUSE_I2C2
|
||||||
|
ap.srcs += peripherals/ms5611.c
|
||||||
|
ap.srcs += peripherals/ms5611_i2c.c
|
||||||
ap.srcs += boards/lisa_m/baro_ms5611_i2c.c
|
ap.srcs += boards/lisa_m/baro_ms5611_i2c.c
|
||||||
endif
|
endif
|
||||||
ap.CFLAGS += -D$(LIA_BARO)
|
ap.CFLAGS += -D$(LIA_BARO)
|
||||||
|
|||||||
@@ -1,16 +1,39 @@
|
|||||||
/**
|
/*
|
||||||
* Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C
|
* 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.
|
||||||
*
|
*
|
||||||
* Edit by: Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
|
|
||||||
* Utah State University, http://aggieair.usu.edu/
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file boards/lisa_m/baro_ms5611_i2c.c
|
||||||
|
*
|
||||||
|
* Driver for MS5611 baro on LisaM/Aspirin2.1 via I2C.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
#include "subsystems/sensors/baro.h"
|
#include "subsystems/sensors/baro.h"
|
||||||
#include "peripherals/ms5611_regs.h"
|
#include "peripherals/ms5611_i2c.h"
|
||||||
|
|
||||||
|
#include "mcu_periph/sys_time.h"
|
||||||
#include "led.h"
|
#include "led.h"
|
||||||
#include "std.h"
|
#include "std.h"
|
||||||
#include "mcu_periph/sys_time.h"
|
|
||||||
|
|
||||||
#include "mcu_periph/i2c.h"
|
|
||||||
#ifndef MS5611_I2C_DEV
|
#ifndef MS5611_I2C_DEV
|
||||||
#define MS5611_I2C_DEV i2c2
|
#define MS5611_I2C_DEV i2c2
|
||||||
#endif
|
#endif
|
||||||
@@ -23,192 +46,73 @@
|
|||||||
*/
|
*/
|
||||||
#define MS5611_SLAVE_ADDR 0xEE
|
#define MS5611_SLAVE_ADDR 0xEE
|
||||||
|
|
||||||
|
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
|
|
||||||
#ifndef DOWNLINK_DEVICE
|
#ifndef DOWNLINK_DEVICE
|
||||||
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
|
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
|
||||||
#endif
|
#endif
|
||||||
#include "mcu_periph/uart.h"
|
#include "mcu_periph/uart.h"
|
||||||
#include "messages.h"
|
#include "messages.h"
|
||||||
#include "subsystems/datalink/downlink.h"
|
#include "subsystems/datalink/downlink.h"
|
||||||
|
|
||||||
|
float fbaroms, ftempms;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
struct Baro baro;
|
struct Baro baro;
|
||||||
struct i2c_transaction ms5611_trans;
|
struct Ms5611_I2c baro_ms5611;
|
||||||
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) {
|
void baro_init(void) {
|
||||||
ms5611_status = MS5611_UNINIT;
|
|
||||||
baro.status = BS_UNINITIALIZED;
|
baro.status = BS_UNINITIALIZED;
|
||||||
prom_cnt = 0;
|
baro.absolute = 0;
|
||||||
|
baro.differential = 0;
|
||||||
|
|
||||||
|
ms5611_i2c_init(&baro_ms5611, &MS5611_I2C_DEV, MS5611_SLAVE_ADDR);
|
||||||
}
|
}
|
||||||
|
|
||||||
void baro_periodic(void) {
|
void baro_periodic(void) {
|
||||||
if (sys_time.nb_sec > 1) {
|
if (sys_time.nb_sec > 1) {
|
||||||
if (ms5611_status == MS5611_IDLE) {
|
|
||||||
/* start D1 conversion */
|
/* call the convenience periodic that initializes the sensor and starts reading*/
|
||||||
ms5611_status = MS5611_CONV_D1;
|
ms5611_i2c_periodic(&baro_ms5611);
|
||||||
ms5611_trans.buf[0] = MS5611_START_CONV_D1;
|
|
||||||
i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
|
if (baro_ms5611.initialized) {
|
||||||
#ifdef DEBUG
|
baro.status = BS_RUNNING;
|
||||||
RunOnceEvery(60, { DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
|
#if DEBUG
|
||||||
&ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
|
RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
|
||||||
&ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]);});
|
&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
|
#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)){
|
void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){
|
||||||
if (ms5611_trans.status == I2CTransSuccess) {
|
if (sys_time.nb_sec > 1) {
|
||||||
|
ms5611_i2c_event(&baro_ms5611);
|
||||||
|
|
||||||
|
if (baro_ms5611.data_available) {
|
||||||
|
b_abs_handler();
|
||||||
|
baro_ms5611.data_available = FALSE;
|
||||||
|
|
||||||
#ifdef ROTORCRAFT_BARO_LED
|
#ifdef ROTORCRAFT_BARO_LED
|
||||||
RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
|
RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
|
||||||
#endif
|
#endif
|
||||||
switch (ms5611_status) {
|
|
||||||
|
|
||||||
case MS5611_RESET:
|
#if DEBUG
|
||||||
ms5611_status = MS5611_RESET_OK;
|
ftempms = baro_ms5611.data.temperature / 100.;
|
||||||
ms5611_trans.status = I2CTransDone;
|
fbaroms = baro_ms5611.data.pressure / 100.;
|
||||||
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,
|
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
|
||||||
&ms5611_d1, &ms5611_d2, &fbaroms, &ftempms, &baro.status);
|
&baro_ms5611.data.d1, &baro_ms5611.data.d2,
|
||||||
|
&fbaroms, &ftempms);
|
||||||
#endif
|
#endif
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
default:
|
|
||||||
ms5611_trans.status = I2CTransDone;
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,16 +1,39 @@
|
|||||||
/**
|
/*
|
||||||
* Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C
|
* 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.
|
||||||
*
|
*
|
||||||
* Edit by: Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
|
|
||||||
* Utah State University, http://aggieair.usu.edu/
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file boards/lisa_m/baro_ms5611_spi.c
|
||||||
|
*
|
||||||
|
* Driver for MS5611 baro on LisaM/Aspirin2.2 via SPI.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
#include "subsystems/sensors/baro.h"
|
#include "subsystems/sensors/baro.h"
|
||||||
#include "peripherals/ms5611_regs.h"
|
#include "peripherals/ms5611_spi.h"
|
||||||
|
|
||||||
|
#include "mcu_periph/sys_time.h"
|
||||||
#include "led.h"
|
#include "led.h"
|
||||||
#include "std.h"
|
#include "std.h"
|
||||||
#include "mcu_periph/sys_time.h"
|
|
||||||
|
|
||||||
#include "mcu_periph/spi.h"
|
|
||||||
#ifndef MS5611_SPI_DEV
|
#ifndef MS5611_SPI_DEV
|
||||||
#define MS5611_SPI_DEV spi2
|
#define MS5611_SPI_DEV spi2
|
||||||
#endif
|
#endif
|
||||||
@@ -23,213 +46,72 @@
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
#define MS5611_BUFFER_LENGTH 4
|
|
||||||
|
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
|
|
||||||
#ifndef DOWNLINK_DEVICE
|
#ifndef DOWNLINK_DEVICE
|
||||||
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
|
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
|
||||||
#endif
|
#endif
|
||||||
#include "mcu_periph/uart.h"
|
#include "mcu_periph/uart.h"
|
||||||
#include "messages.h"
|
#include "messages.h"
|
||||||
#include "subsystems/datalink/downlink.h"
|
#include "subsystems/datalink/downlink.h"
|
||||||
|
|
||||||
|
float fbaroms, ftempms;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
struct Baro baro;
|
struct Baro baro;
|
||||||
struct spi_transaction ms5611_trans;
|
struct Ms5611_Spi baro_ms5611;
|
||||||
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) {
|
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;
|
baro.status = BS_UNINITIALIZED;
|
||||||
prom_cnt = 0;
|
baro.absolute = 0;
|
||||||
|
baro.differential = 0;
|
||||||
|
|
||||||
|
ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_DEV);
|
||||||
}
|
}
|
||||||
|
|
||||||
void baro_periodic(void) {
|
void baro_periodic(void) {
|
||||||
if (sys_time.nb_sec > 1) {
|
if (sys_time.nb_sec > 1) {
|
||||||
if (ms5611_status == MS5611_IDLE) {
|
|
||||||
/* start D1 conversion */
|
/* call the convenience periodic that initializes the sensor and starts reading*/
|
||||||
ms5611_status = MS5611_CONV_D1;
|
ms5611_spi_periodic(&baro_ms5611);
|
||||||
ms5611_trans.output_buf[0] = MS5611_START_CONV_D1;
|
|
||||||
spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
|
if (baro_ms5611.initialized) {
|
||||||
#ifdef DEBUG
|
baro.status = BS_RUNNING;
|
||||||
RunOnceEvery(300, { DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
|
#if DEBUG
|
||||||
&ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
|
RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
|
||||||
&ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]);});
|
&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
|
#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)){
|
void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){
|
||||||
if (ms5611_trans.status == SPITransSuccess) {
|
if (sys_time.nb_sec > 1) {
|
||||||
switch (ms5611_status) {
|
ms5611_spi_event(&baro_ms5611);
|
||||||
|
|
||||||
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;
|
|
||||||
|
|
||||||
|
if (baro_ms5611.data_available) {
|
||||||
b_abs_handler();
|
b_abs_handler();
|
||||||
b_diff_handler();
|
baro_ms5611.data_available = FALSE;
|
||||||
|
|
||||||
#ifdef DEBUG
|
#ifdef ROTORCRAFT_BARO_LED
|
||||||
ftempms = tempms / 100.;
|
RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
|
||||||
fbaroms = baroms / 100.;
|
|
||||||
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
|
|
||||||
&ms5611_d1, &ms5611_d2, &fbaroms, &ftempms, &baro.status);
|
|
||||||
#endif
|
#endif
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
default:
|
#if DEBUG
|
||||||
ms5611_trans.status = SPITransDone;
|
ftempms = baro_ms5611.data.temperature / 100.;
|
||||||
break;
|
fbaroms = baro_ms5611.data.pressure / 100.;
|
||||||
|
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
|
||||||
|
&baro_ms5611.data.d1, &baro_ms5611.data.d2,
|
||||||
|
&fbaroms, &ftempms);
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user