Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
baro_bmp.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 Martin Mueller
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
30 #include "baro_bmp.h"
31 
32 #include "mcu_periph/sys_time.h"
33 #include "mcu_periph/i2c.h"
34 #include "led.h"
35 #include "mcu_periph/uart.h"
36 #include "messages.h"
38 #include "state.h"
39 #include "subsystems/nav.h"
40 
41 #ifdef SITL
42 #include "subsystems/gps.h"
43 #endif
44 
45 #ifndef DOWNLINK_DEVICE
46 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
47 #endif
48 
49 #if !defined(SENSOR_SYNC_SEND) && !defined(USE_BARO_BMP)
50 #warning set SENSOR_SYNC_SEND to use baro_bmp
51 #endif
52 
53 #ifndef BMP_I2C_DEV
54 #define BMP_I2C_DEV i2c0
55 #endif
56 
57 #define BMP085_SLAVE_ADDR 0xEE
58 
59 #define BARO_BMP_OFFSET_MAX 30000
60 #define BARO_BMP_OFFSET_MIN 10
61 #define BARO_BMP_OFFSET_NBSAMPLES_INIT 2
62 #define BARO_BMP_OFFSET_NBSAMPLES_AVRG 4
63 #define BARO_BMP_R 0.5
64 #define BARO_BMP_SIGMA2 0.1
65 
67 
69 float baro_bmp_r;
71 
72 // Global variables
78 double tmp_float;
79 
85 
86 // Local variables
90 
91 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;
127  baro_bmp_pressure = baro_bmp_altitude; //FIXME do a proper scaling here
129 #endif
130 
131 }
132 
133 void baro_bmp_event( void ) {
134 
136 
138  /* get calibration data */
139  bmp_ac1 = (bmp_trans.buf[0] << 8) | bmp_trans.buf[1];
140  bmp_ac2 = (bmp_trans.buf[2] << 8) | bmp_trans.buf[3];
141  bmp_ac3 = (bmp_trans.buf[4] << 8) | bmp_trans.buf[5];
142  bmp_ac4 = (bmp_trans.buf[6] << 8) | bmp_trans.buf[7];
143  bmp_ac5 = (bmp_trans.buf[8] << 8) | bmp_trans.buf[9];
144  bmp_ac6 = (bmp_trans.buf[10] << 8) | bmp_trans.buf[11];
145  bmp_b1 = (bmp_trans.buf[12] << 8) | bmp_trans.buf[13];
146  bmp_b2 = (bmp_trans.buf[14] << 8) | bmp_trans.buf[15];
147  bmp_mb = (bmp_trans.buf[16] << 8) | bmp_trans.buf[17];
148  bmp_mc = (bmp_trans.buf[18] << 8) | bmp_trans.buf[19];
149  bmp_md = (bmp_trans.buf[20] << 8) | bmp_trans.buf[21];
151  }
152  else if (baro_bmp_status == BARO_BMP_READ_TEMP) {
153  /* get uncompensated temperature */
154  bmp_ut = (bmp_trans.buf[0] << 8) | bmp_trans.buf[1];
155  /* start high res pressure measurement */
160  }
161  else if (baro_bmp_status == BARO_BMP_READ_PRESS) {
162  int32_t bmp_p, bmp_t;
163  int32_t bmp_x1, bmp_x2, bmp_x3;
164  int32_t bmp_b3, bmp_b5, bmp_b6;
165  uint32_t bmp_b4, bmp_b7;
166 
167  /* get uncompensated pressure, oss=3 */
168  bmp_up = (bmp_trans.buf[0] << 11) |
169  (bmp_trans.buf[1] << 3) |
170  (bmp_trans.buf[2] >> 5);
171  /* start temp measurement */
176 
177  /* compensate temperature */
178  bmp_x1 = (bmp_ut - bmp_ac6) * bmp_ac5 / (1<<15);
179  bmp_x2 = bmp_mc * (1<<11) / (bmp_x1 + bmp_md);
180  bmp_b5 = bmp_x1 + bmp_x2;
181  bmp_t = (bmp_b5 + 8) / (1<<4);
182 
183  /* compensate pressure */
184  bmp_b6 = bmp_b5 - 4000;
185  bmp_x1 = (bmp_b2 * (bmp_b6 * bmp_b6 / (1<<12))) / (1<<11);
186  bmp_x2 = bmp_ac2 *bmp_b6 / (1<<11);
187  bmp_x3 = bmp_x1 + bmp_x2;
188  bmp_b3 = (((bmp_ac1 * 4 + bmp_x3) << 3) + 2) / 4;
189  bmp_x1 = bmp_ac3 * bmp_b6 / (1<<13);
190  bmp_x2 = (bmp_b1 * (bmp_b6 * bmp_b6 / (1<<12))) / (1<<16);
191  bmp_x3 = ((bmp_x1 + bmp_x2) +2) / (1<<2);
192  bmp_b4 = bmp_ac4 * (uint32_t)(bmp_x3 + 32768) / (1<<15);
193  bmp_b7 = ((uint32_t)bmp_up - bmp_b3) * (50000>>3);
194  if (bmp_b7 < 0x80000000)
195  bmp_p = (bmp_b7 * 2) / bmp_b4;
196  else
197  bmp_p = (bmp_b7 / bmp_b4) * 2;
198  bmp_x1 = (bmp_p / (1<<8)) * (bmp_p / (1<<8));
199  bmp_x1 = (bmp_x1 * 3038) / (1<<16);
200  bmp_x2 = (-7357 * bmp_p) / (1<<16);
201  bmp_p = bmp_p + (bmp_x1 + bmp_x2 + 3791) / (1<<4);
202 
203  baro_bmp_temperature = bmp_t;
204  baro_bmp_pressure = bmp_p;
205 
206  tmp_float = bmp_p/101325.0; //pressao nivel mar
207  tmp_float = pow(tmp_float,0.190295); //eleva pressao ao expoente
208  baro_bmp = 44330*(1.0-tmp_float);
209 
210  if (!baro_bmp_offset_init) {
213 #if 0
214  --baro_bmp_cnt;
215  // Check if averaging completed
216  if (baro_bmp_cnt == 0) {
217  // Calculate average
219  // Limit offset
225  }
226  // Check if averaging needs to continue
229 #endif
230  } //baro offset init
231 
233 
234  if (baro_bmp_offset_init) {
236  // New value available
238 
239 #ifdef SENSOR_SYNC_SEND
240  DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &bmp_up, &bmp_ut, &bmp_p, &bmp_t);
241 #else
242  RunOnceEvery(10, DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &baro_bmp_temp, &bmp_ut, &bmp_p, &bmp_t));
243 #endif
244  } else {
245  baro_bmp_altitude = 0.0;
246  }
247  }
248  }
249 }
unsigned short uint16_t
Definition: types.h:16
int16_t bmp_mb
Definition: baro_bmp.c:83
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t baro_bmp_temp
Definition: baro_bmp.c:77
void baro_bmp_periodic(void)
Definition: baro_bmp.c:104
#define BARO_BMP_SIGMA2
Definition: baro_bmp.c:64
#define BARO_BMP_OFFSET_NBSAMPLES_INIT
Definition: baro_bmp.c:61
#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:77
#define BARO_BMP_READ_PRESS
Definition: baro_bmp.h:35
int16_t bmp_b1
Definition: baro_bmp.c:82
#define BARO_BMP_R
Definition: baro_bmp.c:63
#define BMP085_DAT_MSB
Definition: baro_bmp.h:26
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
uint16_t bmp_ac4
Definition: baro_bmp.c:81
uint16_t bmp_ac5
Definition: baro_bmp.c:81
#define BMP085_EEPROM_AC1
Definition: baro_bmp.h:6
bool_t i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r)
Definition: i2c.c:105
int32_t baro_bmp_altitude
Definition: baro_bmp.c:77
#define BARO_BMP_OFFSET_MIN
Definition: baro_bmp.c:60
bool_t baro_bmp_enabled
Definition: baro_bmp.c:68
uint8_t baro_bmp_status
Definition: baro_bmp.c:73
double tmp_float
Definition: baro_bmp.c:78
#define FALSE
Definition: imu_chimu.h:141
uint32_t baro_bmp_pressure
Definition: baro_bmp.c:75
bool_t baro_bmp_valid
Definition: baro_bmp.c:74
#define BMP085_START_TEMP
Definition: baro_bmp.h:20
Architecture independent timing functions.
int16_t bmp_b2
Definition: baro_bmp.c:82
Device independent GPS code (interface)
bool_t i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Definition: i2c.c:85
void baro_bmp_init(void)
Definition: baro_bmp.c:91
unsigned long uint32_t
Definition: types.h:18
int16_t bmp_ac1
Definition: baro_bmp.c:80
signed short int16_t
Definition: types.h:17
uint16_t baro_bmp_temperature
Definition: baro_bmp.c:76
uint16_t bmp_ac6
Definition: baro_bmp.c:81
float baro_bmp_sigma2
Definition: baro_bmp.c:70
int16_t bmp_mc
Definition: baro_bmp.c:83
float baro_bmp_r
Definition: baro_bmp.c:69
enum I2CTransactionStatus status
Definition: i2c.h:83
int16_t bmp_ac2
Definition: baro_bmp.c:80
#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:54
int32_t baro_bmp_offset_tmp
Definition: baro_bmp.c:88
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:82
uint16_t baro_bmp_cnt
Definition: baro_bmp.c:89
int32_t bmp_up
Definition: baro_bmp.c:84
int16_t bmp_md
Definition: baro_bmp.c:83
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
int16_t bmp_ac3
Definition: baro_bmp.c:80
#define BARO_BMP_OFFSET_NBSAMPLES_AVRG
Definition: baro_bmp.c:62
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:59
#define BARO_BMP_START_TEMP
Definition: baro_bmp.h:32
bool_t baro_bmp_offset_init
Definition: baro_bmp.c:87
struct GpsState gps
global GPS state
Definition: gps.c:33
int32_t baro_bmp
Definition: baro_bmp.c:77
int32_t bmp_ut
Definition: baro_bmp.c:84
struct i2c_transaction bmp_trans
Definition: baro_bmp.c:66
void baro_bmp_event(void)
Definition: baro_bmp.c:133
#define BMP085_SLAVE_ADDR
Definition: baro_bmp.c:57
Architecture independent I2C (Inter-Integrated Circuit Bus) API.