Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
baro_ets.c
Go to the documentation of this file.
1 /*
2  * Driver for the EagleTree Systems Altitude Sensor
3  * Has only been tested with V3 of the sensor hardware
4  *
5  * Notes:
6  * Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together.
7  * Sensor should be in the proprietary mode (default) and not in 3rd party mode.
8  * Pitch gains may need to be updated.
9  *
10  *
11  * Sensor module wire assignments:
12  * Red wire: 5V
13  * White wire: Ground
14  * Yellow wire: SDA
15  * Brown wire: SCL
16  *
17  * Copyright (C) 2009 Vassilis Varveropoulos
18  * Copyright (C) 2010 The Paparazzi Team
19  *
20  * This file is part of paparazzi.
21  *
22  * paparazzi is free software; you can redistribute it and/or modify
23  * it under the terms of the GNU General Public License as published by
24  * the Free Software Foundation; either version 2, or (at your option)
25  * any later version.
26  *
27  * paparazzi is distributed in the hope that it will be useful,
28  * but WITHOUT ANY WARRANTY; without even the implied warranty of
29  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
30  * GNU General Public License for more details.
31  *
32  * You should have received a copy of the GNU General Public License
33  * along with paparazzi; see the file COPYING. If not, write to
34  * the Free Software Foundation, 59 Temple Place - Suite 330,
35  * Boston, MA 02111-1307, USA.
36  */
37 
38 #include "sensors/baro_ets.h"
39 #include "mcu_periph/i2c.h"
40 #include "estimator.h"
41 #include <math.h>
42 
43 #include "subsystems/nav.h"
44 
45 #ifdef SITL
46 #include "subsystems/gps.h"
47 #endif
48 
49 #define BARO_ETS_ADDR 0xE8
50 #define BARO_ETS_REG 0x07
51 #ifndef BARO_ETS_SCALE
52 #define BARO_ETS_SCALE 0.32
53 #endif
54 #define BARO_ETS_OFFSET_MAX 30000
55 #define BARO_ETS_OFFSET_MIN 10
56 #define BARO_ETS_OFFSET_NBSAMPLES_INIT 20
57 #define BARO_ETS_OFFSET_NBSAMPLES_AVRG 40
58 #define BARO_ETS_R 0.5
59 #define BARO_ETS_SIGMA2 0.1
60 
61 #ifndef BARO_ETS_I2C_DEV
62 #define BARO_ETS_I2C_DEV i2c0
63 #endif
64 
65 // Global variables
71 float baro_ets_r;
73 
75 
76 // Local variables
80 
81 void baro_ets_init( void ) {
82  baro_ets_adc = 0;
83  baro_ets_altitude = 0.0;
84  baro_ets_offset = 0;
92 
94 }
95 
96 void baro_ets_read_periodic( void ) {
97  // Initiate next read
98 #ifndef SITL
101 #else // SITL
102  baro_ets_adc = 0;
103  baro_ets_altitude = gps.hmsl / 1000.0;
106 #endif
107 }
108 
109 #ifdef BARO_ETS_TELEMETRY
110 
111 #ifndef DOWNLINK_DEVICE
112 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
113 #endif
114 
115 #include "mcu_periph/uart.h"
116 #include "messages.h"
118 
119 #endif
120 
121 void baro_ets_read_event( void ) {
122  // Get raw altimeter from buffer
124  // Check if this is valid altimeter
125  if (baro_ets_adc == 0)
127  else
129 
130  // Continue only if a new altimeter value was received
131  if (baro_ets_valid) {
132  // Calculate offset average if not done already
133  if (!baro_ets_offset_init) {
134  --baro_ets_cnt;
135  // Check if averaging completed
136  if (baro_ets_cnt == 0) {
137  // Calculate average
139  // Limit offset
145  }
146  // Check if averaging needs to continue
149  }
150  // Convert raw to m/s
151  if (baro_ets_offset_init) {
153  // New value available
155 #ifdef BARO_ETS_TELEMETRY
157 #endif
158  } else {
159  baro_ets_altitude = 0.0;
160  }
161  } else {
162  baro_ets_altitude = 0.0;
163  }
164 
165  // Transaction has been read
167 }
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define I2CReceive(_p, _t, _s_addr, _len)
Definition: i2c.h:140
struct i2c_transaction baro_ets_i2c_trans
Definition: baro_ets.c:74
#define BARO_ETS_SCALE
Definition: baro_ets.c:52
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
#define EstimatorSetAlt(z)
Definition: estimator.h:123
float baro_ets_r
Definition: baro_ets.c:71
#define FALSE
Definition: imu_chimu.h:141
#define BARO_ETS_ADDR
Definition: baro_ets.c:49
#define BARO_ETS_SIGMA2
Definition: baro_ets.c:59
bool_t baro_ets_valid
Definition: baro_ets.c:68
#define BARO_ETS_R
Definition: baro_ets.c:58
Device independent GPS code (interface)
unsigned long uint32_t
Definition: types.h:18
float baro_ets_altitude
Definition: baro_ets.c:69
uint16_t baro_ets_cnt
Definition: baro_ets.c:79
void baro_ets_init(void)
Definition: baro_ets.c:81
enum I2CTransactionStatus status
Definition: i2c.h:47
#define BARO_ETS_OFFSET_MAX
Definition: baro_ets.c:54
#define TRUE
Definition: imu_chimu.h:144
bool_t baro_ets_enabled
Definition: baro_ets.c:70
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:46
bool_t baro_ets_offset_init
Definition: baro_ets.c:77
float baro_ets_sigma2
Definition: baro_ets.c:72
#define BARO_ETS_OFFSET_MIN
Definition: baro_ets.c:55
void baro_ets_read_periodic(void)
Definition: baro_ets.c:96
uint16_t baro_ets_offset
Definition: baro_ets.c:67
#define BARO_ETS_OFFSET_NBSAMPLES_INIT
Definition: baro_ets.c:56
#define BARO_ETS_I2C_DEV
Definition: baro_ets.c:62
State estimation, fusioning sensors.
#define BARO_ETS_OFFSET_NBSAMPLES_AVRG
Definition: baro_ets.c:57
void baro_ets_read_event(void)
Definition: baro_ets.c:121
struct GpsState gps
global GPS state
Definition: gps.c:31
uint16_t baro_ets_adc
Definition: baro_ets.c:66
uint32_t baro_ets_offset_tmp
Definition: baro_ets.c:78