Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ms5611.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 Martin Mueller <martinmm@pfump.org>
3  * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
29 #include "peripherals/ms5611.h"
30 #include "std.h"
31 
37 {
38  int32_t i, j;
39  uint32_t res = 0;
40  uint8_t crc = prom[7] & 0xF;
41  prom[7] &= 0xFF00;
42  for (i = 0; i < 16; i++) {
43  if (i & 1) {
44  res ^= ((prom[i >> 1]) & 0x00FF);
45  } else {
46  res ^= (prom[i >> 1] >> 8);
47  }
48  for (j = 8; j > 0; j--) {
49  if (res & 0x8000) {
50  res ^= 0x1800;
51  }
52  res <<= 1;
53  }
54  }
55  prom[7] |= crc;
56  if (crc == ((res >> 12) & 0xF)) {
57  return true;
58  } else {
59  return false;
60  }
61 }
62 
67 bool ms5611_calc(struct Ms5611Data *ms)
68 {
69  int64_t dt, tempms, off, sens, t2, off2, sens2;
70 
71  /* difference between actual and ref temperature */
72  dt = ms->d2 - (int64_t)ms->c[5] * (1 << 8);
73  /* actual temperature */
74  tempms = 2000 + ((int64_t)dt * ms->c[6]) / (1 << 23);
75  /* offset at actual temperature */
76  off = ((int64_t)ms->c[2] * (1 << 16)) + ((int64_t)ms->c[4] * dt) / (1 << 7);
77  /* sensitivity at actual temperature */
78  sens = ((int64_t)ms->c[1] * (1 << 15)) + ((int64_t)ms->c[3] * dt) / (1 << 8);
79  /* second order temperature compensation */
80  if (tempms < 2000) {
81  t2 = (dt * dt) / (1 << 31);
82  off2 = 5 * ((int64_t)(tempms - 2000) * (tempms - 2000)) / (1 << 1);
83  sens2 = 5 * ((int64_t)(tempms - 2000) * (tempms - 2000)) / (1 << 2);
84  if (tempms < -1500) {
85  off2 = off2 + 7 * (int64_t)(tempms + 1500) * (tempms + 1500);
86  sens2 = sens2 + 11 * ((int64_t)(tempms + 1500) * (tempms + 1500)) / (1 << 1);
87  }
88  tempms = tempms - t2;
89  off = off - off2;
90  sens = sens - sens2;
91  }
92 
93  /* temperature compensated pressure in Pascal (0.01mbar) */
94  uint32_t p = (((int64_t)ms->d1 * sens) / (1 << 21) - off) / (1 << 15);
95  /* if temp and pressare are in valid bounds, copy and return TRUE (valid) */
96  if ((tempms > -4000) && (tempms < 8500) && (p > 1000) && (p < 120000)) {
97  /* temperature in deg Celsius with 0.01 degC resolultion */
98  ms->temperature = (int32_t)tempms;
99  ms->pressure = p;
100  return true;
101  }
102  return false;
103 }
104 
110 bool ms5607_calc(struct Ms5611Data *ms)
111 {
112  int64_t dt, tempms, off, sens, t2, off2, sens2;
113 
114  /* difference between actual and ref temperature */
115  dt = ms->d2 - (int64_t)ms->c[5] * (1 << 8);
116  /* actual temperature */
117  tempms = 2000 + ((int64_t)dt * ms->c[6]) / (1 << 23);
118  /* offset at actual temperature */
119  off = ((int64_t)ms->c[2] * (1 << 17)) + ((int64_t)ms->c[4] * dt) / (1 << 6);
120  /* sensitivity at actual temperature */
121  sens = ((int64_t)ms->c[1] * (1 << 16)) + ((int64_t)ms->c[3] * dt) / (1 << 7);
122  /* second order temperature compensation */
123  if (tempms < 2000) {
124  t2 = (dt * dt) / (1 << 31);
125  off2 = 61 * ((int64_t)(tempms - 2000) * (tempms - 2000)) / (1 << 4);
126  sens2 = 2 * ((int64_t)(tempms - 2000) * (tempms - 2000));
127  if (tempms < -1500) {
128  off2 = off2 + 15 * (int64_t)(tempms + 1500) * (tempms + 1500);
129  sens2 = sens2 + 8 * ((int64_t)(tempms + 1500) * (tempms + 1500));
130  }
131  tempms = tempms - t2;
132  off = off - off2;
133  sens = sens - sens2;
134  }
135 
136  /* temperature compensated pressure in Pascal (0.01mbar) */
137  uint32_t p = (((int64_t)ms->d1 * sens) / (1 << 21) - off) / (1 << 15);
138  /* if temp and pressare are in valid bounds, copy and return TRUE (valid) */
139  if ((tempms > -4000) && (tempms < 8500) && (p > 1000) && (p < 120000)) {
140  /* temperature in deg Celsius with 0.01 degC resolultion */
141  ms->temperature = (int32_t)tempms;
142  ms->pressure = p;
143  return true;
144  }
145  return false;
146 }
uint32_t d1
Definition: ms5611.h:55
unsigned short uint16_t
Definition: types.h:16
bool ms5611_calc(struct Ms5611Data *ms)
Calculate temperature and compensated pressure for MS5611.
Definition: ms5611.c:67
uint16_t c[PROM_NB]
Definition: ms5611.h:54
bool ms5607_calc(struct Ms5611Data *ms)
Calculate temperature and compensated pressure for MS5607.
Definition: ms5611.c:110
uint32_t pressure
pressure in Pascal (0.01mbar)
Definition: ms5611.h:52
float dt
uint32_t d2
Definition: ms5611.h:56
signed long long int64_t
Definition: types.h:21
bool ms5611_prom_crc_ok(uint16_t *prom)
Check if CRC of PROM data is OK.
Definition: ms5611.c:36
MS5611 barometer driver common interface (I2C and SPI).
unsigned long uint32_t
Definition: types.h:18
signed long int32_t
Definition: types.h:19
unsigned char uint8_t
Definition: types.h:14
static float p[2][2]
int32_t temperature
temperature with 0.01 degrees Celsius resolution
Definition: ms5611.h:53