Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
baro_ms5611_i2c.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2011 Martin Mueller <martinmm@pfump.org>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
32 
33 #include "mcu_periph/sys_time.h"
34 #include "mcu_periph/i2c.h"
35 #include "mcu_periph/uart.h"
36 #include "messages.h"
38 #include "subsystems/nav.h"
39 
40 #ifndef DOWNLINK_DEVICE
41 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
42 #endif
43 
44 #ifndef MS5611_I2C_DEV
45 #define MS5611_I2C_DEV i2c0
46 #endif
47 
48 /* address can be 0xEC or 0xEE (CSB\ low = 0xEE) */
49 #ifndef MS5611_SLAVE_ADDR
50 #define MS5611_SLAVE_ADDR 0xEE
51 #endif
52 
53 #if PERIODIC_FREQUENCY > 60
54 #error baro_ms5611_i2c assumes a PERIODIC_FREQUENCY of 60Hz
55 #endif
56 
67 
69  int32_t i, j;
70  uint32_t res = 0;
71  uint8_t crc = prom[7] & 0xF;
72  prom[7] &= 0xFF00;
73  for (i = 0; i < 16; i++) {
74  if (i & 1) res ^= ((prom[i>>1]) & 0x00FF);
75  else res ^= (prom[i>>1]>>8);
76  for (j = 8; j > 0; j--) {
77  if (res & 0x8000) res ^= 0x1800;
78  res <<= 1;
79  }
80  }
81  prom[7] |= crc;
82  if (crc == ((res >> 12) & 0xF)) return 0;
83  else return -1;
84 }
85 
86 void baro_ms5611_init(void) {
92  prom_cnt = 0;
93 }
94 
95 void baro_ms5611_periodic( void ) {
96  if (cpu_time_sec > 1) {
97  if (ms5611_status >= MS5611_IDLE) {
98  /* start D1 conversion */
102  RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
103  &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
104  &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]));
105  }
106  else if (ms5611_status == MS5611_UNINIT) {
107  /* reset sensor */
111  }
112  else if (ms5611_status == MS5611_RESET_OK) {
113  /* start getting prom data */
117  }
118  }
119 }
120 
121 void baro_ms5611_d1( void ) {
122  if (cpu_time_sec > 1) {
124  /* read D1 adc */
128  }
129  }
130 }
131 
132 void baro_ms5611_d2( void ) {
133  if (cpu_time_sec > 1) {
135  /* read D2 adc */
139  }
140  }
141 }
142 
143 void baro_ms5611_event( void ) {
145  switch (ms5611_status) {
146 
147  case MS5611_RESET:
150  break;
151 
152  case MS5611_PROM:
153  /* read prom data */
154  ms5611_c[prom_cnt++] = (ms5611_trans.buf[0] << 8) | ms5611_trans.buf[1];
155  if (prom_cnt < PROM_NB) {
156  /* get next prom data */
159  }
160  else {
161  /* done reading prom */
163  /* check prom crc */
164  if (baro_ms5611_crc(ms5611_c) == 0) {
165  DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
166  &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
167  &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]);
169  }
170  else {
171  /* checksum error, try again */
173  }
174  }
175  break;
176 
177  case MS5611_CONV_D1:
180  break;
181 
182  case MS5611_ADC_D1:
183  /* read D1 (pressure) */
184  ms5611_d1 = (ms5611_trans.buf[0] << 16) |
185  (ms5611_trans.buf[1] << 8) |
186  ms5611_trans.buf[2];
187  /* start D2 conversion */
191  break;
192 
193  case MS5611_CONV_D2:
196  break;
197 
198  case MS5611_ADC_D2: {
199  int64_t dt, baroms, tempms, off, sens, t2, off2, sens2;
200  /* read D2 (temperature) */
201  ms5611_d2 = (ms5611_trans.buf[0] << 16) |
202  (ms5611_trans.buf[1] << 8) |
203  ms5611_trans.buf[2];
206 
207  /* difference between actual and ref temperature */
208  dt = ms5611_d2 - (int64_t)ms5611_c[5] * (1<<8);
209  /* actual temperature */
210  tempms = 2000 + ((int64_t)dt * ms5611_c[6]) / (1<<23);
211  /* offset at actual temperature */
212  off = ((int64_t)ms5611_c[2] * (1<<16)) + ((int64_t)ms5611_c[4] * dt) / (1<<7);
213  /* sensitivity at actual temperature */
214  sens = ((int64_t)ms5611_c[1] * (1<<15)) + ((int64_t)ms5611_c[3] * dt) / (1<<8);
215  /* second order temperature compensation */
216  if (tempms < 2000) {
217  t2 = (dt*dt) / (1<<31);
218  off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1);
219  sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2);
220  if (tempms < -1500) {
221  off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500);
222  sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1);
223  }
224  tempms = tempms - t2;
225  off = off - off2;
226  sens = sens - sens2;
227  }
228  /* temperature compensated pressure */
229  baroms = (((int64_t)ms5611_d1 * sens) / (1<<21) - off) / (1<<15);
230 
231  tmp_float = baroms/101325.0; //pressao nivel mar
232  tmp_float = pow(tmp_float,0.190295); //eleva pressao ao expoente
233  baro_alt = 44330*(1.0-tmp_float); //altitude relativa nivel do mar
234 
235  if (!baro_offset_init) {
238  } //baro offset init
239 
241 
242  if (baro_offset_init) { // New value available
244 
245 #if USE_BARO_MS5611
246 #pragma message "USING BARO MS5611"
248 #endif
249 
250  } else {
251  baro_altitude = 0.0;
252  }
253 #ifdef SENSOR_SYNC_SEND
254  ftempms = tempms / 100.;
255  fbaroms = baroms / 100.;
256  DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
258 #endif
259 
260  break;
261  }
262 
263  default:
265  break;
266  }
267  }
268 }
#define PROM_NB
unsigned short uint16_t
Definition: types.h:16
bool_t baro_ms5611_enabled
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float baro_ms5611_sigma2
#define MS5611_SOFT_RESET
#define MS5611_START_CONV_D1
void baro_ms5611_init(void)
#define I2CTransmit(_p, _t, _s_addr, _len)
Definition: i2c.h:148
bool_t baro_offset_init
#define BARO_MS5611_R
float fbaroms
#define cpu_time_sec
Definition: sys_time.h:64
float baro_alt
signed long long int64_t
Definition: types.h:21
#define EstimatorSetAlt(z)
Definition: estimator.h:123
#define FALSE
Definition: imu_chimu.h:141
struct i2c_transaction ms5611_trans
#define MS5611_START_CONV_D2
void baro_ms5611_event(void)
static int8_t baro_ms5611_crc(uint16_t *prom)
float ftempms
Architecture independent timing functions.
float baro_altitude
#define MS5611_PROM_READ
unsigned long uint32_t
Definition: types.h:18
#define MS5611_ADC_READ
uint16_t ms5611_c[PROM_NB]
enum I2CTransactionStatus status
Definition: i2c.h:47
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:46
uint32_t ms5611_d1
unsigned char uint8_t
Definition: types.h:14
float baro_temp
int32_t prom_cnt
#define MS5611_SLAVE_ADDR
float tmp_float
#define I2CTransceive(_p, _t, _s_addr, _len_w, _len_r)
Definition: i2c.h:156
float baro_offset
void baro_ms5611_periodic(void)
void baro_ms5611_d2(void)
const float dt
auto throttle inner loop
Definition: energy_ctrl.c:285
struct i2c_transaction t2
#define MS5611_I2C_DEV
void baro_ms5611_d1(void)
signed char int8_t
Definition: types.h:15
#define BARO_MS5611_SIGMA2
uint32_t ms5611_d2
uint8_t ms5611_status
float baro_ms5611_r