Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
43  bool allzero = true;
44  for (i = 0; i < 8; i++) {
45  if (prom[i] != 0) {
46  allzero = false;
47  break;
48  }
49  }
50  if (allzero) {
51  return false;
52  }
53 
54  for (i = 0; i < 16; i++) {
55  if (i & 1) {
56  res ^= ((prom[i >> 1]) & 0x00FF);
57 
58  } else {
59  res ^= (prom[i >> 1] >> 8);
60  }
61  for (j = 8; j > 0; j--) {
62  if (res & 0x8000) {
63  res ^= 0x1800;
64  }
65  res <<= 1;
66  }
67  }
68  prom[7] |= crc;
69 
70  if (crc == ((res >> 12) & 0xF)) {
71  return true;
72  } else {
73  return false;
74  }
75 }
76 
81 bool ms5611_calc(struct Ms5611Data *ms)
82 {
83  int64_t dt, tempms, off, sens, t2, off2, sens2;
84 
85  /* difference between actual and ref temperature */
86  dt = ms->d2 - (int64_t)ms->c[5] * (1 << 8);
87  /* actual temperature */
88  tempms = 2000 + ((int64_t)dt * ms->c[6]) / (1 << 23);
89  /* offset at actual temperature */
90  off = ((int64_t)ms->c[2] * (1 << 16)) + ((int64_t)ms->c[4] * dt) / (1 << 7);
91  /* sensitivity at actual temperature */
92  sens = ((int64_t)ms->c[1] * (1 << 15)) + ((int64_t)ms->c[3] * dt) / (1 << 8);
93  /* second order temperature compensation */
94  if (tempms < 2000) {
95  t2 = (dt * dt) / (1 << 31);
96  off2 = 5 * ((int64_t)(tempms - 2000) * (tempms - 2000)) / (1 << 1);
97  sens2 = 5 * ((int64_t)(tempms - 2000) * (tempms - 2000)) / (1 << 2);
98  if (tempms < -1500) {
99  off2 = off2 + 7 * (int64_t)(tempms + 1500) * (tempms + 1500);
100  sens2 = sens2 + 11 * ((int64_t)(tempms + 1500) * (tempms + 1500)) / (1 << 1);
101  }
102  tempms = tempms - t2;
103  off = off - off2;
104  sens = sens - sens2;
105  }
106 
107  /* temperature compensated pressure in Pascal (0.01mbar) */
108  uint32_t p = (((int64_t)ms->d1 * sens) / (1 << 21) - off) / (1 << 15);
109  /* if temp and pressare are in valid bounds, copy and return TRUE (valid) */
110  if ((tempms > -4000) && (tempms < 8500) && (p > 1000) && (p < 120000)) {
111  /* temperature in deg Celsius with 0.01 degC resolultion */
112  ms->temperature = (int32_t)tempms;
113  ms->pressure = p;
114  return true;
115  }
116  return false;
117 }
118 
124 bool ms5607_calc(struct Ms5611Data *ms)
125 {
126  int64_t dt, tempms, off, sens, t2, off2, sens2;
127 
128  /* difference between actual and ref temperature */
129  dt = ms->d2 - (int64_t)ms->c[5] * (1 << 8);
130  /* actual temperature */
131  tempms = 2000 + ((int64_t)dt * ms->c[6]) / (1 << 23);
132  /* offset at actual temperature */
133  off = ((int64_t)ms->c[2] * (1 << 17)) + ((int64_t)ms->c[4] * dt) / (1 << 6);
134  /* sensitivity at actual temperature */
135  sens = ((int64_t)ms->c[1] * (1 << 16)) + ((int64_t)ms->c[3] * dt) / (1 << 7);
136  /* second order temperature compensation */
137  if (tempms < 2000) {
138  t2 = (dt * dt) / (1 << 31);
139  off2 = 61 * ((int64_t)(tempms - 2000) * (tempms - 2000)) / (1 << 4);
140  sens2 = 2 * ((int64_t)(tempms - 2000) * (tempms - 2000));
141  if (tempms < -1500) {
142  off2 = off2 + 15 * (int64_t)(tempms + 1500) * (tempms + 1500);
143  sens2 = sens2 + 8 * ((int64_t)(tempms + 1500) * (tempms + 1500));
144  }
145  tempms = tempms - t2;
146  off = off - off2;
147  sens = sens - sens2;
148  }
149 
150  /* temperature compensated pressure in Pascal (0.01mbar) */
151  uint32_t p = (((int64_t)ms->d1 * sens) / (1 << 21) - off) / (1 << 15);
152  /* if temp and pressare are in valid bounds, copy and return TRUE (valid) */
153  if ((tempms > -4000) && (tempms < 8500) && (p > 1000) && (p < 120000)) {
154  /* temperature in deg Celsius with 0.01 degC resolultion */
155  ms->temperature = (int32_t)tempms;
156  ms->pressure = p;
157  return true;
158  }
159  return false;
160 }
uint16_t
unsigned short uint16_t
Definition: types.h:16
Ms5611Data::temperature
int32_t temperature
temperature with 0.01 degrees Celsius resolution
Definition: ms5611.h:53
ms5611_prom_crc_ok
bool ms5611_prom_crc_ok(uint16_t *prom)
Check if CRC of PROM data is OK.
Definition: ms5611.c:36
ms5611.h
uint32_t
unsigned long uint32_t
Definition: types.h:18
int64_t
signed long long int64_t
Definition: types.h:21
Ms5611Data::d2
uint32_t d2
Definition: ms5611.h:56
ms5611_calc
bool ms5611_calc(struct Ms5611Data *ms)
Calculate temperature and compensated pressure for MS5611.
Definition: ms5611.c:81
std.h
Ms5611Data
Definition: ms5611.h:51
uint8_t
unsigned char uint8_t
Definition: types.h:14
ms5607_calc
bool ms5607_calc(struct Ms5611Data *ms)
Calculate temperature and compensated pressure for MS5607.
Definition: ms5611.c:124
int32_t
signed long int32_t
Definition: types.h:19
Ms5611Data::pressure
uint32_t pressure
pressure in Pascal (0.01mbar)
Definition: ms5611.h:52
Ms5611Data::d1
uint32_t d1
Definition: ms5611.h:55
Ms5611Data::c
uint16_t c[PROM_NB]
Definition: ms5611.h:54
p
static float p[2][2]
Definition: ins_alt_float.c:268