Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
baro_bmp.c
Go to the documentation of this file.
1 /*
2  * $Id: baro_bmp.c $
3  *
4  * Copyright (C) 2010 Martin Mueller
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 #include "baro_bmp.h"
33 
34 #include "mcu_periph/sys_time.h"
35 #include "mcu_periph/i2c.h"
36 #include "led.h"
37 #include "mcu_periph/uart.h"
38 #include "messages.h"
40 #include "estimator.h"
41 #include "subsystems/nav.h"
42 
43 #ifdef SITL
44 #include "subsystems/gps.h"
45 #endif
46 
47 #ifndef DOWNLINK_DEVICE
48 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
49 #endif
50 
51 #if !defined(SENSOR_SYNC_SEND) && !defined(USE_BARO_BMP)
52 #warning set SENSOR_SYNC_SEND to use baro_bmp
53 #endif
54 
55 #ifndef BMP_I2C_DEV
56 #define BMP_I2C_DEV i2c0
57 #endif
58 
59 #define BMP085_SLAVE_ADDR 0xEE
60 
61 #define BARO_BMP_OFFSET_MAX 30000
62 #define BARO_BMP_OFFSET_MIN 10
63 #define BARO_BMP_OFFSET_NBSAMPLES_INIT 2
64 #define BARO_BMP_OFFSET_NBSAMPLES_AVRG 4
65 #define BARO_BMP_R 0.5
66 #define BARO_BMP_SIGMA2 0.1
67 
69 
71 float baro_bmp_r;
73 
74 // Global variables
79 double tmp_float;
80 
86 
87 // Local variables
91 
92 void baro_bmp_init( void ) {
99  /* read calibration values */
102 }
103 
104 void baro_bmp_periodic( void ) {
105 #ifndef SITL
107  /* start temp measurement (once) */
112  }
113  else if (baro_bmp_status == BARO_BMP_START_TEMP) {
114  /* read temp measurement */
118  }
120  /* read press measurement */
124  }
125 #else // SITL
126  baro_bmp_altitude = gps.hmsl / 1000.0;
128 #endif
129 
130 }
131 
132 void baro_bmp_event( void ) {
133 
135 
137  /* get calibration data */
138  bmp_ac1 = (bmp_trans.buf[0] << 8) | bmp_trans.buf[1];
139  bmp_ac2 = (bmp_trans.buf[2] << 8) | bmp_trans.buf[3];
140  bmp_ac3 = (bmp_trans.buf[4] << 8) | bmp_trans.buf[5];
141  bmp_ac4 = (bmp_trans.buf[6] << 8) | bmp_trans.buf[7];
142  bmp_ac5 = (bmp_trans.buf[8] << 8) | bmp_trans.buf[9];
143  bmp_ac6 = (bmp_trans.buf[10] << 8) | bmp_trans.buf[11];
144  bmp_b1 = (bmp_trans.buf[12] << 8) | bmp_trans.buf[13];
145  bmp_b2 = (bmp_trans.buf[14] << 8) | bmp_trans.buf[15];
146  bmp_mb = (bmp_trans.buf[16] << 8) | bmp_trans.buf[17];
147  bmp_mc = (bmp_trans.buf[18] << 8) | bmp_trans.buf[19];
148  bmp_md = (bmp_trans.buf[20] << 8) | bmp_trans.buf[21];
150  }
151  else if (baro_bmp_status == BARO_BMP_READ_TEMP) {
152  /* get uncompensated temperature */
153  bmp_ut = (bmp_trans.buf[0] << 8) | bmp_trans.buf[1];
154  /* start high res pressure measurement */
159  }
160  else if (baro_bmp_status == BARO_BMP_READ_PRESS) {
161  int32_t bmp_p, bmp_t;
162  int32_t bmp_x1, bmp_x2, bmp_x3;
163  int32_t bmp_b3, bmp_b5, bmp_b6;
164  uint32_t bmp_b4, bmp_b7;
165 
166  /* get uncompensated pressure, oss=3 */
167  bmp_up = (bmp_trans.buf[0] << 11) |
168  (bmp_trans.buf[1] << 3) |
169  (bmp_trans.buf[2] >> 5);
170  /* start temp measurement */
175 
176  /* compensate temperature */
177  bmp_x1 = (bmp_ut - bmp_ac6) * bmp_ac5 / (1<<15);
178  bmp_x2 = bmp_mc * (1<<11) / (bmp_x1 + bmp_md);
179  bmp_b5 = bmp_x1 + bmp_x2;
180  bmp_t = (bmp_b5 + 8) / (1<<4);
181 
182  /* compensate pressure */
183  bmp_b6 = bmp_b5 - 4000;
184  bmp_x1 = (bmp_b2 * (bmp_b6 * bmp_b6 / (1<<12))) / (1<<11);
185  bmp_x2 = bmp_ac2 *bmp_b6 / (1<<11);
186  bmp_x3 = bmp_x1 + bmp_x2;
187  bmp_b3 = (((bmp_ac1 * 4 + bmp_x3) << 3) + 2) / 4;
188  bmp_x1 = bmp_ac3 * bmp_b6 / (1<<13);
189  bmp_x2 = (bmp_b1 * (bmp_b6 * bmp_b6 / (1<<12))) / (1<<16);
190  bmp_x3 = ((bmp_x1 + bmp_x2) +2) / (1<<2);
191  bmp_b4 = bmp_ac4 * (uint32_t)(bmp_x3 + 32768) / (1<<15);
192  bmp_b7 = ((uint32_t)bmp_up - bmp_b3) * (50000>>3);
193  if (bmp_b7 < 0x80000000)
194  bmp_p = (bmp_b7 * 2) / bmp_b4;
195  else
196  bmp_p = (bmp_b7 / bmp_b4) * 2;
197  bmp_x1 = (bmp_p / (1<<8)) * (bmp_p / (1<<8));
198  bmp_x1 = (bmp_x1 * 3038) / (1<<16);
199  bmp_x2 = (-7357 * bmp_p) / (1<<16);
200  bmp_p = bmp_p + (bmp_x1 + bmp_x2 + 3791) / (1<<4);
201 
202  baro_bmp_temperature = bmp_t;
203  baro_bmp_pressure = bmp_p;
204 
205  tmp_float = bmp_p/101325.0; //pressao nivel mar
206  tmp_float = pow(tmp_float,0.190295); //eleva pressao ao expoente
207  baro_bmp = 44330*(1.0-tmp_float);
208 
209  if (!baro_bmp_offset_init) {
212 #if 0
213  --baro_bmp_cnt;
214  // Check if averaging completed
215  if (baro_bmp_cnt == 0) {
216  // Calculate average
218  // Limit offset
224  }
225  // Check if averaging needs to continue
228 #endif
229  } //baro offset init
230 
232 
233  if (baro_bmp_offset_init) {
235  // New value available
236 #if USE_BARO_BMP
237 #pragma message "USING BARO BMP"
239 #endif
240 
241 #ifdef SENSOR_SYNC_SEND
242  DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &bmp_up, &bmp_ut, &bmp_p, &bmp_t);
243 #else
244  RunOnceEvery(10, DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &baro_bmp_temp, &bmp_ut, &bmp_p, &bmp_t));
245 #endif
246  } else {
247  baro_bmp_altitude = 0.0;
248  }
249  }
250  }
251 }
unsigned short uint16_t
Definition: types.h:16
int16_t bmp_mb
Definition: baro_bmp.c:84
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t baro_bmp_temp
Definition: baro_bmp.c:78
void baro_bmp_periodic(void)
Definition: baro_bmp.c:104
#define BARO_BMP_SIGMA2
Definition: baro_bmp.c:66
#define BARO_BMP_OFFSET_NBSAMPLES_INIT
Definition: baro_bmp.c:63
#define BARO_BMP_UNINIT
Definition: baro_bmp.h:30
#define BARO_BMP_START_PRESS
Definition: baro_bmp.h:34
int32_t baro_bmp_offset
Definition: baro_bmp.c:78
#define I2CTransmit(_p, _t, _s_addr, _len)
Definition: i2c.h:148
#define BARO_BMP_READ_PRESS
Definition: baro_bmp.h:35
int16_t bmp_b1
Definition: baro_bmp.c:83
#define BARO_BMP_R
Definition: baro_bmp.c:65
#define BMP085_DAT_MSB
Definition: baro_bmp.h:26
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
#define EstimatorSetAlt(z)
Definition: estimator.h:119
uint16_t bmp_ac4
Definition: baro_bmp.c:82
uint16_t bmp_ac5
Definition: baro_bmp.c:82
#define BMP085_EEPROM_AC1
Definition: baro_bmp.h:6
int32_t baro_bmp_altitude
Definition: baro_bmp.c:78
#define BARO_BMP_OFFSET_MIN
Definition: baro_bmp.c:62
bool_t baro_bmp_enabled
Definition: baro_bmp.c:70
uint8_t baro_bmp_status
Definition: baro_bmp.c:75
double tmp_float
Definition: baro_bmp.c:79
#define FALSE
Definition: imu_chimu.h:141
uint32_t baro_bmp_pressure
Definition: baro_bmp.c:76
#define BMP085_START_TEMP
Definition: baro_bmp.h:20
Architecture independent timing functions.
int16_t bmp_b2
Definition: baro_bmp.c:83
Device independent GPS code (interface)
void baro_bmp_init(void)
Definition: baro_bmp.c:92
unsigned long uint32_t
Definition: types.h:18
int16_t bmp_ac1
Definition: baro_bmp.c:81
signed short int16_t
Definition: types.h:17
uint16_t baro_bmp_temperature
Definition: baro_bmp.c:77
uint16_t bmp_ac6
Definition: baro_bmp.c:82
float baro_bmp_sigma2
Definition: baro_bmp.c:72
int16_t bmp_mc
Definition: baro_bmp.c:84
float baro_bmp_r
Definition: baro_bmp.c:71
enum I2CTransactionStatus status
Definition: i2c.h:47
int16_t bmp_ac2
Definition: baro_bmp.c:81
#define BMP085_CTRL_REG
Definition: baro_bmp.h:18
#define BMP085_START_P3
Definition: baro_bmp.h:24
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
#define BMP_I2C_DEV
Definition: baro_bmp.c:56
int32_t baro_bmp_offset_tmp
Definition: baro_bmp.c:89
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:46
uint16_t baro_bmp_cnt
Definition: baro_bmp.c:90
int32_t bmp_up
Definition: baro_bmp.c:85
int16_t bmp_md
Definition: baro_bmp.c:84
unsigned char uint8_t
Definition: types.h:14
#define I2CTransceive(_p, _t, _s_addr, _len_w, _len_r)
Definition: i2c.h:156
int16_t bmp_ac3
Definition: baro_bmp.c:81
#define BARO_BMP_OFFSET_NBSAMPLES_AVRG
Definition: baro_bmp.c:64
State estimation, fusioning sensors.
arch independent LED (Light Emitting Diodes) API
#define BARO_BMP_IDLE
Definition: baro_bmp.h:31
#define BARO_BMP_READ_TEMP
Definition: baro_bmp.h:33
#define BARO_BMP_OFFSET_MAX
Definition: baro_bmp.c:61
#define BARO_BMP_START_TEMP
Definition: baro_bmp.h:32
bool_t baro_bmp_offset_init
Definition: baro_bmp.c:88
struct GpsState gps
global GPS state
Definition: gps.c:31
int32_t baro_bmp
Definition: baro_bmp.c:78
int32_t bmp_ut
Definition: baro_bmp.c:85
struct i2c_transaction bmp_trans
Definition: baro_bmp.c:68
void baro_bmp_event(void)
Definition: baro_bmp.c:132
#define BMP085_SLAVE_ADDR
Definition: baro_bmp.c:59