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_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 #define BARO_ETS_SCALE 0.32
52 #define BARO_ETS_OFFSET_MAX 30000
53 #define BARO_ETS_OFFSET_MIN 10
54 #define BARO_ETS_OFFSET_NBSAMPLES_INIT 20
55 #define BARO_ETS_OFFSET_NBSAMPLES_AVRG 40
56 #define BARO_ETS_R 0.5
57 #define BARO_ETS_SIGMA2 0.1
58 
59 #ifndef BARO_ETS_I2C_DEV
60 #define BARO_ETS_I2C_DEV i2c0
61 #endif
62 
63 // Global variables
69 float baro_ets_r;
71 
73 
74 // Local variables
78 
79 void baro_ets_init( void ) {
80  baro_ets_adc = 0;
81  baro_ets_altitude = 0.0;
82  baro_ets_offset = 0;
90 
92 }
93 
94 void baro_ets_read_periodic( void ) {
95  // Initiate next read
96 #ifndef SITL
99 #else // SITL
100  baro_ets_adc = 0;
101  baro_ets_altitude = gps.hmsl / 1000.0;
104 #endif
105 }
106 
107 #ifdef BARO_ETS_TELEMETRY
108 
109 #ifndef DOWNLINK_DEVICE
110 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
111 #endif
112 
113 #include "mcu_periph/uart.h"
114 #include "messages.h"
116 
117 #endif
118 
119 void baro_ets_read_event( void ) {
120  // Get raw altimeter from buffer
122  // Check if this is valid altimeter
123  if (baro_ets_adc == 0)
125  else
127 
128  // Continue only if a new altimeter value was received
129  if (baro_ets_valid) {
130  // Calculate offset average if not done already
131  if (!baro_ets_offset_init) {
132  --baro_ets_cnt;
133  // Check if averaging completed
134  if (baro_ets_cnt == 0) {
135  // Calculate average
137  // Limit offset
143  }
144  // Check if averaging needs to continue
147  }
148  // Convert raw to m/s
149  if (baro_ets_offset_init) {
151  // New value available
153 #ifdef BARO_ETS_TELEMETRY
155 #endif
156  } else {
157  baro_ets_altitude = 0.0;
158  }
159  } else {
160  baro_ets_altitude = 0.0;
161  }
162 
163  // Transaction has been read
165 }
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:72
#define BARO_ETS_SCALE
Definition: baro_ets.c:51
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
#define EstimatorSetAlt(z)
Definition: estimator.h:119
float baro_ets_r
Definition: baro_ets.c:69
#define FALSE
Definition: imu_chimu.h:141
#define BARO_ETS_ADDR
Definition: baro_ets.c:49
#define BARO_ETS_SIGMA2
Definition: baro_ets.c:57
bool_t baro_ets_valid
Definition: baro_ets.c:66
#define BARO_ETS_R
Definition: baro_ets.c:56
Device independent GPS code (interface)
unsigned long uint32_t
Definition: types.h:18
float baro_ets_altitude
Definition: baro_ets.c:67
uint16_t baro_ets_cnt
Definition: baro_ets.c:77
void baro_ets_init(void)
Definition: baro_ets.c:79
enum I2CTransactionStatus status
Definition: i2c.h:47
#define BARO_ETS_OFFSET_MAX
Definition: baro_ets.c:52
#define TRUE
Definition: imu_chimu.h:144
bool_t baro_ets_enabled
Definition: baro_ets.c:68
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:46
bool_t baro_ets_offset_init
Definition: baro_ets.c:75
float baro_ets_sigma2
Definition: baro_ets.c:70
#define BARO_ETS_OFFSET_MIN
Definition: baro_ets.c:53
void baro_ets_read_periodic(void)
Definition: baro_ets.c:94
uint16_t baro_ets_offset
Definition: baro_ets.c:65
#define BARO_ETS_OFFSET_NBSAMPLES_INIT
Definition: baro_ets.c:54
#define BARO_ETS_I2C_DEV
Definition: baro_ets.c:60
State estimation, fusioning sensors.
#define BARO_ETS_OFFSET_NBSAMPLES_AVRG
Definition: baro_ets.c:55
void baro_ets_read_event(void)
Definition: baro_ets.c:119
struct GpsState gps
global GPS state
Definition: gps.c:31
uint16_t baro_ets_adc
Definition: baro_ets.c:64
uint32_t baro_ets_offset_tmp
Definition: baro_ets.c:76