-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
baro_ets.c
173 lines (156 loc) · 4.94 KB
/
baro_ets.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
/*
* Copyright (C) 2009 Vassilis Varveropoulos
* Copyright (C) 2010 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file baro_ets.c
*
* Driver for the EagleTree Systems Altitude Sensor.
* Has only been tested with V3 of the sensor hardware.
*
* Notes:
* Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together.
* Sensor should be in the proprietary mode (default) and not in 3rd party mode.
* Pitch gains may need to be updated.
*
*
* Sensor module wire assignments:
* Red wire: 5V
* White wire: Ground
* Yellow wire: SDA
* Brown wire: SCL
*/
#include "sensors/baro_ets.h"
#include "mcu_periph/i2c.h"
#include "state.h"
#include <math.h>
#include "subsystems/nav.h"
#ifdef SITL
#include "subsystems/gps.h"
#endif
#ifdef BARO_ETS_TELEMETRY
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
#endif //BARO_ETS_TELEMETRY
#define BARO_ETS_ADDR 0xE8
#define BARO_ETS_REG 0x07
#define BARO_ETS_SCALE 0.32
#define BARO_ETS_OFFSET_MAX 30000
#define BARO_ETS_OFFSET_MIN 10
#define BARO_ETS_OFFSET_NBSAMPLES_INIT 20
#define BARO_ETS_OFFSET_NBSAMPLES_AVRG 40
#define BARO_ETS_R 0.5
#define BARO_ETS_SIGMA2 0.1
#ifndef BARO_ETS_I2C_DEV
#define BARO_ETS_I2C_DEV i2c0
#endif
// Global variables
uint16_t baro_ets_adc;
uint16_t baro_ets_offset;
bool_t baro_ets_valid;
float baro_ets_altitude;
bool_t baro_ets_enabled;
float baro_ets_r;
float baro_ets_sigma2;
struct i2c_transaction baro_ets_i2c_trans;
// Local variables
bool_t baro_ets_offset_init;
uint32_t baro_ets_offset_tmp;
uint16_t baro_ets_cnt;
void baro_ets_init( void ) {
baro_ets_adc = 0;
baro_ets_altitude = 0.0;
baro_ets_offset = 0;
baro_ets_offset_tmp = 0;
baro_ets_valid = TRUE;
baro_ets_offset_init = FALSE;
baro_ets_enabled = TRUE;
baro_ets_cnt = BARO_ETS_OFFSET_NBSAMPLES_INIT + BARO_ETS_OFFSET_NBSAMPLES_AVRG;
baro_ets_r = BARO_ETS_R;
baro_ets_sigma2 = BARO_ETS_SIGMA2;
baro_ets_i2c_trans.status = I2CTransDone;
}
void baro_ets_read_periodic( void ) {
// Initiate next read
#ifndef SITL
if (baro_ets_i2c_trans.status == I2CTransDone)
I2CReceive(BARO_ETS_I2C_DEV, baro_ets_i2c_trans, BARO_ETS_ADDR, 2);
#else // SITL
/* fake an offset so sim works for under hmsl as well */
if (!baro_ets_offset_init) {
baro_ets_offset = 200;
baro_ets_offset_init = TRUE;
}
baro_ets_altitude = gps.hmsl / 1000.0;
baro_ets_adc = baro_ets_offset - baro_ets_altitude / BARO_ETS_SCALE;
baro_ets_valid = TRUE;
#ifdef BARO_ETS_TELEMETRY
DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude);
#endif
#endif
}
void baro_ets_read_event( void ) {
// Get raw altimeter from buffer
baro_ets_adc = ((uint16_t)(baro_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(baro_ets_i2c_trans.buf[0]);
// Check if this is valid altimeter
if (baro_ets_adc == 0)
baro_ets_valid = FALSE;
else
baro_ets_valid = TRUE;
// Continue only if a new altimeter value was received
if (baro_ets_valid) {
// Calculate offset average if not done already
if (!baro_ets_offset_init) {
--baro_ets_cnt;
// Check if averaging completed
if (baro_ets_cnt == 0) {
// Calculate average
baro_ets_offset = (uint16_t)(baro_ets_offset_tmp / BARO_ETS_OFFSET_NBSAMPLES_AVRG);
// Limit offset
if (baro_ets_offset < BARO_ETS_OFFSET_MIN)
baro_ets_offset = BARO_ETS_OFFSET_MIN;
if (baro_ets_offset > BARO_ETS_OFFSET_MAX)
baro_ets_offset = BARO_ETS_OFFSET_MAX;
baro_ets_offset_init = TRUE;
}
// Check if averaging needs to continue
else if (baro_ets_cnt <= BARO_ETS_OFFSET_NBSAMPLES_AVRG)
baro_ets_offset_tmp += baro_ets_adc;
}
// Convert raw to m/s
if (baro_ets_offset_init) {
baro_ets_altitude = ground_alt + BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc);
// New value available
#ifdef BARO_ETS_TELEMETRY
DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude);
#endif
} else {
baro_ets_altitude = 0.0;
}
} else {
baro_ets_altitude = 0.0;
}
// Transaction has been read
baro_ets_i2c_trans.status = I2CTransDone;
}