drv_ms5611.c
Go to the documentation of this file.
1 /*
2  drv_ms5611.c : driver for Measurement Specialties MS5611 barometer
3 
4  Adapted from https://github.com/multiwii/baseflight/blob/master/src/drv_ms5611.c
5 
6  This file is part of BreezySTM32.
7 
8  BreezySTM32 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 3 of the License, or
11  (at your option) any later version.
12 
13  BreezySTM32 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 BreezySTM32. If not, see <http://www.gnu.org/licenses/>.
20  */
21 
22 
23 #include <breezystm32.h>
24 #include <limits.h>
25 
26 #include <math.h>
27 
28 // MS5611, Standard address 0x77
29 static const uint8_t ADDR = 0x77;
30 static const uint8_t PROM_RD = 0xA0; // Prom read command
31 
32 #define CMD_RESET 0x1E // ADC reset command
33 #define CMD_ADC_READ 0x00 // ADC read command
34 #define CMD_ADC_CONV 0x40 // ADC conversion command
35 #define CMD_ADC_D1 0x00 // ADC D1 conversion
36 #define CMD_ADC_D2 0x10 // ADC D2 conversion
37 #define CMD_ADC_256 0x00 // ADC OSR=256
38 #define CMD_ADC_512 0x02 // ADC OSR=512
39 #define CMD_ADC_1024 0x04 // ADC OSR=1024
40 #define CMD_ADC_2048 0x06 // ADC OSR=2048
41 #define CMD_ADC_4096 0x08 // ADC OSR=4096
42 #define CMD_PROM_RD 0xA0 // Prom read command
43 #define PROM_NB 8
44 
45 
46 #define REBOOT_PERIOD_MS 1000 * 60 * 30 // reboot the device every 30 minutes
47 
48 static uint8_t cmd;
49 
50 static void master_cb(uint8_t result);
51 
52 uint8_t pres_buf_[3];
53 uint8_t temp_buf_[3];
54 int32_t pres_raw_;
55 int32_t temp_raw_;
56 float pressure_;
58 uint16_t prom[8];
59 uint32_t next_update_ms_;
60 uint32_t next_reboot_ms_;
61 uint32_t last_update_ms_;
63 bool new_data_;
65 
66 typedef enum
67 {
69  READ_TEMP = 1,
72 } state_t;
73 static state_t state_;
74 
75 
76 typedef enum
77 {
88 
89 static void reset(void)
90 {
91  i2cWrite(ADDR, CMD_RESET, 1);
92  delayMicroseconds(2800);
93 }
94 
95 static bool read_prom()
96 {
97  uint8_t buf[2] = {0, 0};
98 
99  // try a few times
100  for (int i = 0; i < 8; i++)
101  {
102  i2cWrite(ADDR, 0xFF, PROM_RD + 2* i);
103  if (i2cRead(ADDR, 0xFF, 2, buf) == true)
104  prom[i] = (uint16_t)(buf[0] << 8 | buf[1]);
105  else
106  {
107  reset();
108  delay(3);
109  i2cWrite(0, 0, 0);
110  delay(3);
111  // didn't work, try again
112  return false;
113  }
114  }
115  return true;
116 }
117 
118 int8_t calc_crc()
119 {
120  uint32_t res = 0;
121  uint8_t crc = prom[7] & 0xF;
122  prom[7] &= 0xFF00;
123 
124  bool blank = true;
125 
126  for (int i = 0; i < 16; i++)
127  {
128  if (prom[i >> 1])
129  {
130  blank = false;
131  }
132  if (i & 1)
133  res ^= ((prom[i >> 1]) & 0x00FF);
134  else
135  res ^= (prom[i >> 1] >> 8);
136  for (int j = 8; j > 0; j--)
137  {
138  if (res & 0x8000)
139  res ^= 0x1800;
140  res <<= 1;
141  }
142  }
143  prom[7] |= crc;
144  if (!blank && crc == ((res >> 12) & 0xF))
145  return 0;
146 
147  return -1;
148 }
149 
150 
152 {
153  baro_present_ = false;
154  while (millis() < 10); // wait for chip to power on
155 
158 
159  i2cWrite(0, 0, 0); // cycle the I2C clk signal (from the datasheet)
160  delay(1);
161  if (i2cWrite(ADDR, RESET, 1) != true)
162  {
163  baro_present_ = false;
164  return false;
165  }
166  else
167  {
168  baro_present_ = true;
169  }
170  delay(3);
171 
172  // Read the PROM (try a couple of times if it fails)
173  bool got_valid_prom = false;
174  for (int i = 0; i < 5; i++)
175  {
176  if (read_prom() == true)
177  {
178  if (calc_crc() != 0)
179  continue;
180  else
181  {
182  got_valid_prom = true;
183  break;
184  }
185  }
186  }
187 
188  if (got_valid_prom)
189  {
190  state_ = START_TEMP;
191  new_data_ = false;
192  baro_present_ = true;
194  waiting_for_cb_ = false;
195  return true;
196  }
197  else
198  {
199  return false;
200  }
201 }
202 
204 {
205  if (baro_present_ && waiting_for_cb_ && (millis() > last_update_ms_ + 200))
206  baro_present_ = false;
207  return baro_present_;
208 }
209 
210 static void convert()
211 {
212  int32_t press = 0;
213  int32_t temp = 0;
214  int64_t delta = 0;
215  temp_raw_ = (temp_buf_[0] << 16) | (temp_buf_[1] << 8) | temp_buf_[2];
216  pres_raw_ = (pres_buf_[0] << 16) | (pres_buf_[1] << 8) | pres_buf_[2];
217  if(pres_raw_ > 9085466 * 2 / 3 && temp_raw_ > 0)
218  {
219  int32_t dT = temp_raw_ - ((int32_t)(prom[5]) << 8);
220  int64_t off = ((int64_t)(prom[2]) << 16) + (((int64_t)(prom[4]) * dT) >> 7);
221  int64_t sens = ((int64_t)(prom[1]) << 15) + (((int64_t)(prom[3]) * dT) >> 8);
222  temp = 2000 + ((dT * (int64_t)(prom[6])) >> 23);
223 
224  // temperature lower than 20degC
225  if (temp < 2000)
226  {
227  delta = temp - 2000;
228  delta = 5 * delta * delta;
229  off -= delta >> 1;
230  sens -= delta >> 2;
231 
232  // temperature lower than -15degC
233  if (temp < -1500)
234  {
235  delta = temp + 1500;
236  delta = delta * delta;
237  off -= 7 * delta;
238  sens -= (11 * delta) >> 1;
239  }
240 
241  temp -= ((dT * dT) >> 31);
242  }
243 
244  press = ((((uint64_t)(pres_raw_) * sens) >> 21) - off) >> 15;
245 
246  pressure_ = (float)(press); // Pa
247  temperature_ = (float)(temp) / 100.0 + 273.0; // K
248  }
249  new_data_ = false;
250 }
251 
252 void temp_read_cb1(uint8_t result)
253 {
254  (void) result;
255  waiting_for_cb_ = false;
258  i2c_queue_job(READ, ADDR, 0xFF, temp_buf_, 3, NULL, &master_cb);
259 }
260 
261 void pres_read_cb1(uint8_t result)
262 {
263  (void) result;
264  waiting_for_cb_ = false;
267  i2c_queue_job(READ, ADDR, 0xFF, pres_buf_, 3, NULL, &master_cb);
268 }
269 
270 
271 void temp_read_cb2(uint8_t result)
272 {
273  (void) result;
275  waiting_for_cb_ = false;
278  new_data_ = true;
279 }
280 
281 void pres_read_cb2(uint8_t result)
282 {
283  (void) result;
284  state_ = START_TEMP;
285  waiting_for_cb_ = false;
288  new_data_ = true;
289 }
290 
291 void temp_start_cb(uint8_t result)
292 {
293  (void) result;
294  state_ = READ_TEMP;
295  waiting_for_cb_ = false;
298 }
299 
300 void pres_start_cb(uint8_t result)
301 {
302  (void) result;
303  state_ = READ_PRESS;
304  waiting_for_cb_ = false;
307 }
308 
309 void reset_cb(uint8_t result)
310 {
311  (void) result;
315  waiting_for_cb_ = false;
317  uint8_t data = 0;
318  i2c_queue_job(WRITE, 0, 0xFF, &data, 1, NULL, &master_cb);
319 }
320 
321 void write_zero_cb(uint8_t result)
322 {
323  (void) result;
327  waiting_for_cb_ = false;
328  state_ = START_TEMP;
329 }
330 
331 void master_cb(uint8_t result)
332 {
333  if (result != I2C_JOB_ERROR)
334  baro_present_ = true;
335  switch (callback_type_)
336  {
337  case CB_TEMP_READ1:
338  temp_read_cb1(result);
339  break;
340  case CB_TEMP_READ2:
341  temp_read_cb2(result);
342  break;
343  case CB_PRES_READ1:
344  pres_read_cb1(result);
345  break;
346  case CB_PRES_READ2:
347  pres_read_cb2(result);
348  break;
349  case CB_TEMP_START:
350  temp_start_cb(result);
351  break;
352  case CB_PRES_START:
353  pres_start_cb(result);
354  break;
355  case CB_RESET:
356  reset_cb(result);
357  break;
358  case CB_WRITE_ZERO:
359  write_zero_cb(result);
360  break;
361  }
362 }
363 
365 {
366  waiting_for_cb_ = true;
370  i2c_queue_job(WRITE, ADDR, 0xFF, &cmd, 1, NULL, &master_cb);
371  return true;
372 }
373 
375 {
376  waiting_for_cb_ = true;
380  i2c_queue_job(WRITE, ADDR, 0xFF, &cmd, 1, NULL, &master_cb);
381  return true;
382 }
383 
385 {
386  waiting_for_cb_ = true;
389  cmd = CMD_ADC_READ;
390  i2c_queue_job(WRITE, ADDR, 0xFF, &cmd, 1, NULL, &master_cb);
391  return true;
392 }
393 
395 {
396  waiting_for_cb_ = true;
399  cmd = CMD_ADC_READ;
400  i2c_queue_job(WRITE, ADDR, 0xFF, &cmd, 1, NULL, &master_cb);
401  return true;
402 }
403 
404 
406 {
407  uint32_t now_ms = millis();
408 
409  // Sometimes the barometer fails to respond. If this happens, then reset it
410  // the barometer also seems to stop responding after 72 minutes (suspiciously close to a overflow of uint32_t with a microsecond timer)
411  // to avoid that, just reboot periodically
412  if ((waiting_for_cb_ && now_ms) > last_update_ms_ + 20 || (now_ms > next_reboot_ms_))
413  {
414  last_update_ms_ = now_ms;
416  uint8_t command = RESET;
417  i2c_queue_job(READ, ADDR, 0xFF, &command, 1, NULL, &master_cb);
418  }
419 
420  else if (now_ms > next_update_ms_)
421  {
422  switch (state_)
423  {
424  case START_TEMP:
425  if (start_temp_meas())
426  next_update_ms_ += 100;
427  break;
428  case READ_TEMP:
429  if (read_temp_mess())
430  next_update_ms_ += 100;
431  break;
432  case START_PRESS:
433  if (start_pres_meas())
434  next_update_ms_ += 100;
435  break;
436  case READ_PRESS:
437  if (read_pres_mess())
438  next_update_ms_ += 100;
439  break;
440  default:
441  state_ = START_TEMP;
442  break;
443  }
444  }
445 
446  if (new_data_)
447  {
448  convert();
449  }
450 }
451 
453 {
454  (*pressure) = pressure_;
455  (*temperature) = temperature_;
456 }
bool start_temp_meas()
Definition: drv_ms5611.c:364
callback_type_t
Definition: drv_ms5611.c:76
uint8_t * data
Definition: drv_i2c.h:47
uint16_t prom[8]
Definition: drv_ms5611.c:58
static const uint8_t ADDR
Definition: drv_ms5611.c:29
bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t *buf)
Definition: drv_i2c.c:157
int32_t temp_raw_
Definition: drv_ms5611.c:55
volatile uint32_t millis(void)
Definition: system.c:50
static bool read_prom()
Definition: drv_ms5611.c:95
float pressure
Definition: ms4525.c:41
float temperature
Definition: ms4525.c:41
Definition: drv_i2c.h:32
state_t
Definition: drv_ms5611.c:66
void temp_start_cb(uint8_t result)
Definition: drv_ms5611.c:291
void temp_read_cb1(uint8_t result)
Definition: drv_ms5611.c:252
void delayMicroseconds(uint32_t us)
Definition: system.c:91
#define CMD_ADC_4096
Definition: drv_ms5611.c:41
int8_t calc_crc()
Definition: drv_ms5611.c:118
static const uint8_t PROM_RD
Definition: drv_ms5611.c:30
bool ms5611_init()
Definition: drv_ms5611.c:151
void write_zero_cb(uint8_t result)
Definition: drv_ms5611.c:321
#define CMD_ADC_D2
Definition: drv_ms5611.c:36
int32_t pres_raw_
Definition: drv_ms5611.c:54
bool start_pres_meas()
Definition: drv_ms5611.c:374
void pres_read_cb1(uint8_t result)
Definition: drv_ms5611.c:261
#define REBOOT_PERIOD_MS
Definition: drv_ms5611.c:46
uint8_t pres_buf_[3]
Definition: drv_ms5611.c:52
bool i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
Definition: drv_i2c.c:152
ROSLIB_DECL std::string command(const std::string &cmd)
bool read_temp_mess()
Definition: drv_ms5611.c:394
void temp_read_cb2(uint8_t result)
Definition: drv_ms5611.c:271
static volatile int16_t temp
Definition: drv_mpu6050.c:278
uint8_t temp_buf_[3]
Definition: drv_ms5611.c:53
bool waiting_for_cb_
Definition: drv_ms5611.c:62
#define CMD_RESET
Definition: drv_ms5611.c:32
bool new_data_
Definition: drv_ms5611.c:63
void pres_start_cb(uint8_t result)
Definition: drv_ms5611.c:300
#define CMD_ADC_D1
Definition: drv_ms5611.c:35
static state_t state_
Definition: drv_ms5611.c:73
float temperature_
Definition: drv_ms5611.c:57
#define CMD_ADC_READ
Definition: drv_ms5611.c:33
bool read_pres_mess()
Definition: drv_ms5611.c:384
float pressure_
Definition: drv_ms5611.c:56
static void convert()
Definition: drv_ms5611.c:210
void reset_cb(uint8_t result)
Definition: drv_ms5611.c:309
void i2c_queue_job(i2cJobType_t type, uint8_t addr_, uint8_t reg_, uint8_t *data, uint8_t length, volatile uint8_t *status_, void(*CB)(uint8_t))
Definition: drv_i2c.c:579
void pres_read_cb2(uint8_t result)
Definition: drv_ms5611.c:281
static callback_type_t callback_type_
Definition: drv_ms5611.c:87
static uint8_t cmd
Definition: drv_ms5611.c:48
uint32_t last_update_ms_
Definition: drv_ms5611.c:61
Definition: drv_i2c.h:31
uint32_t next_update_ms_
Definition: drv_ms5611.c:59
#define NULL
Definition: usbd_def.h:50
uint32_t next_reboot_ms_
Definition: drv_ms5611.c:60
void ms5611_async_update()
Definition: drv_ms5611.c:405
bool ms5611_present()
Definition: drv_ms5611.c:203
#define CMD_ADC_CONV
Definition: drv_ms5611.c:34
static void master_cb(uint8_t result)
Definition: drv_ms5611.c:331
bool baro_present_
Definition: drv_ms5611.c:64
void delay(uint32_t ms)
Definition: system.c:98
static void reset(void)
Definition: drv_ms5611.c:89
void ms5611_async_read(float *pressure, float *temperature)
Definition: drv_ms5611.c:452


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:18