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_amsys.c
Go to the documentation of this file.
1 /*
2  * Driver for a Amsys Barometric Sensor I2C
3  * AMS 5812-0150-A
4  * ----measuring only---
5  *
6  * Copyright (C) 2010 The Paparazzi Team
7  *
8  * This file is part of paparazzi.
9  *
10  * paparazzi is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2, or (at your option)
13  * any later version.
14  *
15  * paparazzi is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with paparazzi; see the file COPYING. If not, write to
22  * the Free Software Foundation, 59 Temple Place - Suite 330,
23  * Boston, MA 02111-1307, USA.
24  */
25 
26 #include "sensors/baro_amsys.h"
27 #include "mcu_periph/i2c.h"
28 #include "estimator.h"
29 #include <math.h>
30 #include "generated/flight_plan.h" // for ground alt
31 
32 #ifdef SITL
33 #include "subsystems/gps.h"
34 #endif
35 
36 //Messages
37 #include "mcu_periph/uart.h"
38 #include "messages.h"
40 //#include "gps.h"
41 #ifndef DOWNLINK_DEVICE
42 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
43 #endif
44 
45 #ifdef SITL
46 #include "gps.h"
47 #endif
48 
49 #define BARO_AMSYS_ADDR 0xF2
50 #define BARO_AMSYS_REG 0x07
51 #define BARO_AMSYS_SCALE 0.32
52 #define BARO_AMSYS_MAX_PRESSURE 103400 // Pascal
53 #define BARO_AMSYS_OFFSET_MAX 29491
54 #define BARO_AMSYS_OFFSET_MIN 3277
55 #define BARO_AMSYS_OFFSET_NBSAMPLES_INIT 40
56 #define BARO_AMSYS_OFFSET_NBSAMPLES_AVRG 60
57 
58 #ifdef MEASURE_AMSYS_TEMPERATURE
59 #define TEMPERATURE_AMSYS_OFFSET_MAX 29491
60 #define TEMPERATURE_AMSYS_OFFSET_MIN 3277
61 #define TEMPERATURE_AMSYS_MAX 110
62 #define TEMPERATURE_AMSYS_MIN -25
63 #endif
64 
65 //#define BARO_AMSYS_R 0.5
66 //#define BARO_AMSYS_SIGMA2 0.1
67 //#define BARO_ALT_CORRECTION 0.0
68 #ifndef BARO_AMSYS_I2C_DEV
69 #define BARO_AMSYS_I2C_DEV i2c0
70 #endif
71 
72 // Global variables
82 float ref_alt_init; //Altitude by initialising
84 float baro_old;
85 
86 //float baro_amsys_r;
87 //float baro_amsys_sigma2;
88 
89 
91 
92 // Local variables
96 
97 void baro_amsys_init( void ) {
98  baro_filter=BARO_FILTER;
99  pBaroRaw = 0;
100  tBaroRaw = 0;
101  baro_amsys_altitude = 0.0;
102  baro_amsys_p=0.0;
103  baro_amsys_offset = 0;
107 // baro_amsys_enabled = TRUE;
109 // baro_amsys_r = BARO_AMSYS_R;
110 // baro_amsys_sigma2 = BARO_AMSYS_SIGMA2;
111 // baro_head=0;
112  ref_alt_init = 0;
114 }
115 
117  // Initiate next read
118 #ifndef SITL
120 #ifndef MEASURE_AMSYS_TEMPERATURE
122 #else
124 #endif
125  }
126 #else // SITL
127  pBaroRaw = 0;
128  baro_amsys_altitude = gps.hmsl / 1000.0;
131 #endif
132 }
133 
134 void baro_amsys_read_event( void ) {
135  pBaroRaw = 0;
136  // Get raw altimeter from buffer
138 #ifdef MEASURE_AMSYS_TEMPERATURE
140  baro_amsys_temp = (float)(tBaroRaw-TEMPERATURE_AMSYS_OFFSET_MIN)*TEMPERATURE_AMSYS_MAX/(float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)+(float)TEMPERATURE_AMSYS_MIN;
141 #endif
142  // Check if this is valid altimeter
143  if (pBaroRaw == 0)
145  else
147 
148 
149  // Continue only if a new altimeter value was received
150  //if (baro_amsys_valid && GpsFixValid()) {
151  if (baro_amsys_valid) {
152  //Cut RAW Min and Max
157 
158  //Convert to pressure
160  if (!baro_amsys_offset_init) {
161  --baro_amsys_cnt;
162  // Check if averaging completed
163  if (baro_amsys_cnt == 0) {
164  // Calculate average
166  ref_alt_init = GROUND_ALT;
168 
169  // hight over Sea level at init point
170  //baro_amsys_offset_altitude = 288.15 / 0.0065 * (1 - pow((baro_amsys_p)/1013.25 , 1/5.255));
171  }
172  // Check if averaging needs to continue
175 
176  baro_amsys_altitude = 0.0;
177 
178  }
179  else {
180  // Lowpassfiltering and convert pressure to altitude
183 
184 
185  //New value available
186  //EstimatorSetAlt(baro_amsys_abs_altitude);
187  }
189  } /*else {
190  baro_amsys_abs_altitude = 0.0;
191  }*/
192 
193 
194  // Transaction has been read
196 #ifdef SENSOR_SYNC_SEND
198 #else
200 #endif
201 
202 }
unsigned short uint16_t
Definition: types.h:16
float baro_amsys_temp
Definition: baro_amsys.c:78
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
struct i2c_transaction baro_amsys_i2c_trans
Definition: baro_amsys.c:90
uint16_t tBaroRaw
Definition: baro_amsys.c:74
#define I2CReceive(_p, _t, _s_addr, _len)
Definition: i2c.h:140
double baro_amsys_offset_tmp
Definition: baro_amsys.c:94
float baro_amsys_p
Definition: baro_amsys.c:79
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
#define EstimatorSetAlt(z)
Definition: estimator.h:119
float baro_filter
Definition: baro_amsys.c:83
#define FALSE
Definition: imu_chimu.h:141
float baro_amsys_abs_altitude
Definition: baro_amsys.c:81
#define BARO_AMSYS_OFFSET_NBSAMPLES_AVRG
Definition: baro_amsys.c:56
#define BARO_AMSYS_MAX_PRESSURE
Definition: baro_amsys.c:52
#define BARO_AMSYS_ADDR
Definition: baro_amsys.c:49
Device independent GPS code (interface)
#define BARO_AMSYS_I2C_DEV
Definition: baro_amsys.c:69
#define BARO_AMSYS_OFFSET_NBSAMPLES_INIT
Definition: baro_amsys.c:55
bool_t baro_amsys_valid
Definition: baro_amsys.c:76
void baro_amsys_init(void)
Definition: baro_amsys.c:97
enum I2CTransactionStatus status
Definition: i2c.h:47
#define BARO_AMSYS_OFFSET_MAX
Definition: baro_amsys.c:53
float baro_amsys_altitude
Definition: baro_amsys.c:77
float baro_old
Definition: baro_amsys.c:84
#define TRUE
Definition: imu_chimu.h:144
float baro_amsys_offset_altitude
Definition: baro_amsys.c:80
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:46
float baro_amsys_offset
Definition: baro_amsys.c:75
State estimation, fusioning sensors.
void baro_amsys_read_event(void)
Definition: baro_amsys.c:134
bool_t baro_amsys_offset_init
Definition: baro_amsys.c:93
#define BARO_AMSYS_OFFSET_MIN
Definition: baro_amsys.c:54
uint16_t baro_amsys_cnt
Definition: baro_amsys.c:95
void baro_amsys_read_periodic(void)
Definition: baro_amsys.c:116
float ref_alt_init
Definition: baro_amsys.c:82
uint16_t pBaroRaw
Definition: baro_amsys.c:73
struct GpsState gps
global GPS state
Definition: gps.c:31