Paparazzi UAS  v5.15_devel-109-gee85905
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
airspeed_ets.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009 Vassilis Varveropoulos
3  * Modified by Mark Griffin on 8 September 2010 to work with new i2c transaction routines.
4  * Converted by Gautier Hattenberger to modules (10/2010)
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
42 #include "sensors/airspeed_ets.h"
43 #include "state.h"
44 #include "mcu_periph/i2c.h"
45 #include "mcu_periph/uart.h"
46 #include "mcu_periph/sys_time.h"
47 #include "pprzlink/messages.h"
49 #include <math.h>
50 
51 #ifndef USE_AIRSPEED_ETS
52 #if USE_AIRSPEED
53 #define USE_AIRSPEED_ETS TRUE
54 PRINT_CONFIG_MSG("USE_AIRSPEED_ETS automatically set to TRUE")
55 #endif
56 #endif
57 
58 #if !USE_AIRSPEED_ETS && !AIRSPEED_ETS_SYNC_SEND
59 #warning either set USE_AIRSPEED_ETS or AIRSPEED_ETS_SYNC_SEND to use airspeed_ets
60 #endif
61 
62 #define AIRSPEED_ETS_ADDR 0xEA
63 #ifndef AIRSPEED_ETS_SCALE
64 #define AIRSPEED_ETS_SCALE 1.8
65 #endif
66 #ifndef AIRSPEED_ETS_OFFSET
67 #define AIRSPEED_ETS_OFFSET 0
68 #endif
69 #define AIRSPEED_ETS_OFFSET_MAX 1750
70 #define AIRSPEED_ETS_OFFSET_MIN 1450
71 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT 40
72 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG 60
73 #define AIRSPEED_ETS_NBSAMPLES_AVRG 10
74 
75 #ifndef AIRSPEED_ETS_I2C_DEV
76 #define AIRSPEED_ETS_I2C_DEV i2c0
77 #endif
78 PRINT_CONFIG_VAR(AIRSPEED_ETS_I2C_DEV)
79 
80 
81 #ifndef AIRSPEED_ETS_START_DELAY
82 #define AIRSPEED_ETS_START_DELAY 0.2
83 #endif
84 PRINT_CONFIG_VAR(AIRSPEED_ETS_START_DELAY)
85 
86 #ifndef SITL
87 #if AIRSPEED_ETS_SDLOG
89 #include "subsystems/gps.h"
90 bool log_airspeed_ets_started;
91 #endif
92 #endif
93 
94 
95 // Global variables
102 
104 
105 // Local variables
106 volatile bool airspeed_ets_i2c_done;
112 
114 {
115  int n;
116  airspeed_ets_raw = 0;
117  airspeed_ets = 0.0;
118  airspeed_ets_offset = 0;
119  airspeed_ets_offset_tmp = 0;
120  airspeed_ets_i2c_done = true;
121  airspeed_ets_valid = false;
122  airspeed_ets_offset_init = false;
124 
125  airspeed_ets_buffer_idx = 0;
126  for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) {
127  airspeed_ets_buffer[n] = 0.0;
128  }
129 
131 
132  airspeed_ets_delay_done = false;
133  SysTimeTimerStart(airspeed_ets_delay_time);
134 
135 #ifndef SITL
136 #if AIRSPEED_ETS_SDLOG
137  log_airspeed_ets_started = false;
138 #endif
139 #endif
140 }
141 
143 {
144 #ifndef SITL
145  if (!airspeed_ets_delay_done) {
146  if (SysTimeTimer(airspeed_ets_delay_time) < USEC_OF_SEC(AIRSPEED_ETS_START_DELAY)) { return; }
147  else { airspeed_ets_delay_done = true; }
148  }
151  }
152 #elif !defined USE_NPS
153  extern float sim_air_speed;
154  stateSetAirspeed_f(sim_air_speed);
155 #endif //SITL
156 }
157 
159 {
160  int n;
161  float airspeed_tmp = 0.0;
162 
163  // Get raw airspeed from buffer
164  airspeed_ets_raw = ((uint16_t)(airspeed_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(airspeed_ets_i2c_trans.buf[0]);
165  // Check if this is valid airspeed
166  if (airspeed_ets_raw == 0) {
167  airspeed_ets_valid = false;
168  } else {
169  airspeed_ets_valid = true;
170  }
171 
172  // Continue only if a new airspeed value was received
173  if (airspeed_ets_valid) {
174 #if !AIRSPEED_ETS_3RD_PARTY_MODE
175  // Calculate offset average if not done already
176  if (!airspeed_ets_offset_init) {
178  // Check if averaging completed
179  if (airspeed_ets_cnt == 0) {
180  // Calculate average
181  airspeed_ets_offset = (uint16_t)(airspeed_ets_offset_tmp / AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG);
182  // Limit offset
183  if (airspeed_ets_offset < AIRSPEED_ETS_OFFSET_MIN) {
184  airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MIN;
185  }
186  if (airspeed_ets_offset > AIRSPEED_ETS_OFFSET_MAX) {
187  airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MAX;
188  }
189  airspeed_ets_offset_init = true;
190  }
191  // Check if averaging needs to continue
192  else if (airspeed_ets_cnt <= AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG) {
193  airspeed_ets_offset_tmp += airspeed_ets_raw;
194  }
195  }
196  // Convert raw to m/s
197 #ifdef AIRSPEED_ETS_REVERSE
198  if (airspeed_ets_offset_init && airspeed_ets_raw < airspeed_ets_offset) {
199  airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_offset - airspeed_ets_raw)) - AIRSPEED_ETS_OFFSET;
200  }
201 #else
202  if (airspeed_ets_offset_init && airspeed_ets_raw > airspeed_ets_offset) {
203  airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_raw - airspeed_ets_offset)) - AIRSPEED_ETS_OFFSET;
204  }
205 #endif
206  else {
207  airspeed_tmp = 0.0;
208  }
209 //use raw value for sensor set to third-party mode
210 #else
211  airspeed_tmp = airspeed_ets_raw;
212 #endif //AIRSPEED_ETS_3RD_PARTY_MODE
213 
214  // Airspeed should always be positive
215  if (airspeed_tmp < 0.0) {
216  airspeed_tmp = 0.0;
217  }
218  // Moving average
219  airspeed_ets_buffer[airspeed_ets_buffer_idx++] = airspeed_tmp;
220  if (airspeed_ets_buffer_idx >= AIRSPEED_ETS_NBSAMPLES_AVRG) {
221  airspeed_ets_buffer_idx = 0;
222  }
223  airspeed_ets = 0.0;
224  for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) {
225  airspeed_ets += airspeed_ets_buffer[n];
226  }
227  airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG;
228 #if USE_AIRSPEED_ETS
229  stateSetAirspeed_f(airspeed_ets);
230 #endif
231 #if AIRSPEED_ETS_SYNC_SEND
232  DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
233 #endif
234  } else {
235  airspeed_ets = 0.0;
236  }
237 
238 
239 #if AIRSPEED_ETS_SDLOG
240 #ifndef SITL
241  if (pprzLogFile != -1) {
242  if (!log_airspeed_ets_started) {
243  sdLogWriteLog(pprzLogFile, "AIRSPEED_ETS: raw offset airspeed(m/s) GPS_fix TOW(ms) Week Lat(1e7deg) Lon(1e7deg) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n");
244  log_airspeed_ets_started = true;
245  }
246  sdLogWriteLog(pprzLogFile, "airspeed_ets: %d %d %8.4f %d %d %d %d %d %d %d %d %d\n",
247  airspeed_ets_raw, airspeed_ets_offset, airspeed_ets,
248  gps.fix, gps.tow, gps.week,
251  }
252 #endif
253 #endif
254 
255 
256 
257  // Transaction has been read
259 }
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT
Definition: airspeed_ets.c:71
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
#define AIRSPEED_ETS_NBSAMPLES_AVRG
Definition: airspeed_ets.c:73
float airspeed_ets
Definition: airspeed_ets.c:99
uint16_t week
GPS week.
Definition: gps.h:108
void airspeed_ets_init(void)
Definition: airspeed_ets.c:113
#define AIRSPEED_ETS_SCALE
Definition: airspeed_ets.c:64
#define AIRSPEED_ETS_OFFSET_MIN
Definition: airspeed_ets.c:70
int32_t z
Down.
uint16_t airspeed_ets_cnt
Definition: airspeed_ets.c:109
bool airspeed_ets_delay_done
Definition: airspeed_ets.c:111
bool airspeed_ets_offset_init
Definition: airspeed_ets.c:107
void airspeed_ets_read_periodic(void)
Definition: airspeed_ets.c:142
float sim_air_speed
Definition: sim_ir.c:14
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG
Definition: airspeed_ets.c:72
transaction set to done by user level
Definition: i2c.h:59
Driver for the EagleTree Systems Airspeed Sensor.
Architecture independent timing functions.
#define AIRSPEED_ETS_I2C_DEV
Definition: airspeed_ets.c:76
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
Device independent GPS code (interface)
#define AIRSPEED_ETS_ADDR
Definition: airspeed_ets.c:62
unsigned long uint32_t
Definition: types.h:18
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
int32_t lon
in degrees*1e7
FileDes pprzLogFile
Definition: sdlog_chibios.c:85
#define AIRSPEED_ETS_START_DELAY
delay in seconds until sensor is read after startup
Definition: airspeed_ets.c:82
#define AIRSPEED_ETS_OFFSET
Definition: airspeed_ets.c:67
int airspeed_ets_buffer_idx
Definition: airspeed_ets.c:100
I2C transaction structure.
Definition: i2c.h:93
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
#define USEC_OF_SEC(sec)
Definition: sys_time.h:210
#define SysTimeTimer(_t)
Definition: sys_time.h:219
volatile bool airspeed_ets_i2c_done
Definition: airspeed_ets.c:106
API to get/set the generic vehicle states.
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1309
bool i2c_receive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint16_t len)
Submit a read only transaction.
Definition: i2c.c:318
struct i2c_transaction airspeed_ets_i2c_trans
Definition: airspeed_ets.c:103
uint16_t airspeed_ets_offset
Definition: airspeed_ets.c:97
void airspeed_ets_read_event(void)
Definition: airspeed_ets.c:158
float airspeed_ets_buffer[AIRSPEED_ETS_NBSAMPLES_AVRG]
Definition: airspeed_ets.c:101
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
#define SysTimeTimerStart(_t)
Definition: sys_time.h:218
#define AIRSPEED_ETS_OFFSET_MAX
Definition: airspeed_ets.c:69
int32_t lat
in degrees*1e7
uint8_t fix
status of fix
Definition: gps.h:107
uint32_t airspeed_ets_delay_time
Definition: airspeed_ets.c:110
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
struct GpsState gps
global GPS state
Definition: gps.c:69
uint16_t airspeed_ets_raw
Definition: airspeed_ets.c:96
bool airspeed_ets_valid
Definition: airspeed_ets.c:98
uint32_t airspeed_ets_offset_tmp
Definition: airspeed_ets.c:108
Architecture independent I2C (Inter-Integrated Circuit Bus) API.