Paparazzi UAS  v6.0_unstable-53-gfe8bbd3-dirty
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"
48 #include "subsystems/abi.h"
50 #include <math.h>
51 
52 #ifndef USE_AIRSPEED_ETS
53 #if USE_AIRSPEED
54 #define USE_AIRSPEED_ETS TRUE
55 PRINT_CONFIG_MSG("USE_AIRSPEED_ETS automatically set to TRUE")
56 #endif
57 #endif
58 
59 #if !USE_AIRSPEED_ETS && !AIRSPEED_ETS_SYNC_SEND
60 #warning either set USE_AIRSPEED_ETS or AIRSPEED_ETS_SYNC_SEND to use airspeed_ets
61 #endif
62 
63 #define AIRSPEED_ETS_ADDR 0xEA
64 #ifndef AIRSPEED_ETS_SCALE
65 #define AIRSPEED_ETS_SCALE 1.8
66 #endif
67 #ifndef AIRSPEED_ETS_OFFSET
68 #define AIRSPEED_ETS_OFFSET 0
69 #endif
70 #define AIRSPEED_ETS_OFFSET_MAX 1750
71 #define AIRSPEED_ETS_OFFSET_MIN 1450
72 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT 40
73 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG 60
74 #define AIRSPEED_ETS_NBSAMPLES_AVRG 10
75 
76 #ifndef AIRSPEED_ETS_I2C_DEV
77 #define AIRSPEED_ETS_I2C_DEV i2c0
78 #endif
79 PRINT_CONFIG_VAR(AIRSPEED_ETS_I2C_DEV)
80 
81 
82 #ifndef AIRSPEED_ETS_START_DELAY
83 #define AIRSPEED_ETS_START_DELAY 0.2
84 #endif
85 PRINT_CONFIG_VAR(AIRSPEED_ETS_START_DELAY)
86 
87 #ifndef SITL
88 #if AIRSPEED_ETS_SDLOG
90 #include "subsystems/gps.h"
91 bool log_airspeed_ets_started;
92 #endif
93 #endif
94 
95 
96 // Global variables
103 
105 
106 // Local variables
107 volatile bool airspeed_ets_i2c_done;
113 
115 {
116  int n;
117  airspeed_ets_raw = 0;
118  airspeed_ets = 0.0;
119  airspeed_ets_offset = 0;
120  airspeed_ets_offset_tmp = 0;
121  airspeed_ets_i2c_done = true;
122  airspeed_ets_valid = false;
123  airspeed_ets_offset_init = false;
125 
126  airspeed_ets_buffer_idx = 0;
127  for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) {
128  airspeed_ets_buffer[n] = 0.0;
129  }
130 
132 
133  airspeed_ets_delay_done = false;
134  SysTimeTimerStart(airspeed_ets_delay_time);
135 
136 #ifndef SITL
137 #if AIRSPEED_ETS_SDLOG
138  log_airspeed_ets_started = false;
139 #endif
140 #endif
141 }
142 
144 {
145 #ifndef SITL
146  if (!airspeed_ets_delay_done) {
147  if (SysTimeTimer(airspeed_ets_delay_time) < USEC_OF_SEC(AIRSPEED_ETS_START_DELAY)) { return; }
148  else { airspeed_ets_delay_done = true; }
149  }
152  }
153 #elif !defined USE_NPS
154  extern float sim_air_speed;
155  stateSetAirspeed_f(sim_air_speed);
156 #endif //SITL
157 }
158 
160 {
161  int n;
162  float airspeed_tmp = 0.0;
163 
164  // Get raw airspeed from buffer
165  airspeed_ets_raw = ((uint16_t)(airspeed_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(airspeed_ets_i2c_trans.buf[0]);
166  // Check if this is valid airspeed
167  if (airspeed_ets_raw == 0) {
168  airspeed_ets_valid = false;
169  } else {
170  airspeed_ets_valid = true;
171  }
172 
173  // Continue only if a new airspeed value was received
174  if (airspeed_ets_valid) {
175 #if !AIRSPEED_ETS_3RD_PARTY_MODE
176  // Calculate offset average if not done already
177  if (!airspeed_ets_offset_init) {
179  // Check if averaging completed
180  if (airspeed_ets_cnt == 0) {
181  // Calculate average
182  airspeed_ets_offset = (uint16_t)(airspeed_ets_offset_tmp / AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG);
183  // Limit offset
184  if (airspeed_ets_offset < AIRSPEED_ETS_OFFSET_MIN) {
185  airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MIN;
186  }
187  if (airspeed_ets_offset > AIRSPEED_ETS_OFFSET_MAX) {
188  airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MAX;
189  }
190  airspeed_ets_offset_init = true;
191  }
192  // Check if averaging needs to continue
193  else if (airspeed_ets_cnt <= AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG) {
194  airspeed_ets_offset_tmp += airspeed_ets_raw;
195  }
196  }
197  // Convert raw to m/s
198 #ifdef AIRSPEED_ETS_REVERSE
199  if (airspeed_ets_offset_init && airspeed_ets_raw < airspeed_ets_offset) {
200  airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_offset - airspeed_ets_raw)) - AIRSPEED_ETS_OFFSET;
201  }
202 #else
203  if (airspeed_ets_offset_init && airspeed_ets_raw > airspeed_ets_offset) {
204  airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_raw - airspeed_ets_offset)) - AIRSPEED_ETS_OFFSET;
205  }
206 #endif
207  else {
208  airspeed_tmp = 0.0;
209  }
210 //use raw value for sensor set to third-party mode
211 #else
212  airspeed_tmp = airspeed_ets_raw;
213 #endif //AIRSPEED_ETS_3RD_PARTY_MODE
214 
215  // Airspeed should always be positive
216  if (airspeed_tmp < 0.0) {
217  airspeed_tmp = 0.0;
218  }
219  // Moving average
220  airspeed_ets_buffer[airspeed_ets_buffer_idx++] = airspeed_tmp;
221  if (airspeed_ets_buffer_idx >= AIRSPEED_ETS_NBSAMPLES_AVRG) {
222  airspeed_ets_buffer_idx = 0;
223  }
224  airspeed_ets = 0.0;
225  for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) {
226  airspeed_ets += airspeed_ets_buffer[n];
227  }
228  airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG;
229 
230  // Publish airspeed sensor
231  AbiSendMsgAIRSPEED(AIRSPEED_ETS_ID, airspeed_ets);
232 
233 #if USE_AIRSPEED_ETS
234  stateSetAirspeed_f(airspeed_ets);
235 #endif
236 #if AIRSPEED_ETS_SYNC_SEND
237  DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
238 #endif
239  } else {
240  airspeed_ets = 0.0;
241  }
242 
243 
244 #if AIRSPEED_ETS_SDLOG
245 #ifndef SITL
246  if (pprzLogFile != -1) {
247  if (!log_airspeed_ets_started) {
248  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");
249  log_airspeed_ets_started = true;
250  }
251  sdLogWriteLog(pprzLogFile, "airspeed_ets: %d %d %8.4f %d %d %d %d %d %d %d %d %d\n",
252  airspeed_ets_raw, airspeed_ets_offset, airspeed_ets,
253  gps.fix, gps.tow, gps.week,
256  }
257 #endif
258 #endif
259 
260 
261 
262  // Transaction has been read
264 }
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT
Definition: airspeed_ets.c:72
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:74
float sim_air_speed
Definition: sim_airspeed.c:10
float airspeed_ets
Definition: airspeed_ets.c:100
uint16_t week
GPS week.
Definition: gps.h:108
void airspeed_ets_init(void)
Definition: airspeed_ets.c:114
#define AIRSPEED_ETS_SCALE
Definition: airspeed_ets.c:65
#define AIRSPEED_ETS_OFFSET_MIN
Definition: airspeed_ets.c:71
Main include for ABI (AirBorneInterface).
int32_t z
Down.
#define AIRSPEED_ETS_ID
uint16_t airspeed_ets_cnt
Definition: airspeed_ets.c:110
bool airspeed_ets_delay_done
Definition: airspeed_ets.c:112
bool airspeed_ets_offset_init
Definition: airspeed_ets.c:108
void airspeed_ets_read_periodic(void)
Definition: airspeed_ets.c:143
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:73
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:77
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:63
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
int32_t lon
in degrees*1e7
FileDes pprzLogFile
Definition: sdlog_chibios.c:86
#define AIRSPEED_ETS_START_DELAY
delay in seconds until sensor is read after startup
Definition: airspeed_ets.c:83
#define AIRSPEED_ETS_OFFSET
Definition: airspeed_ets.c:68
int airspeed_ets_buffer_idx
Definition: airspeed_ets.c:101
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:107
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:334
struct i2c_transaction airspeed_ets_i2c_trans
Definition: airspeed_ets.c:104
uint16_t airspeed_ets_offset
Definition: airspeed_ets.c:98
void airspeed_ets_read_event(void)
Definition: airspeed_ets.c:159
float airspeed_ets_buffer[AIRSPEED_ETS_NBSAMPLES_AVRG]
Definition: airspeed_ets.c:102
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:70
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:111
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:97
bool airspeed_ets_valid
Definition: airspeed_ets.c:99
uint32_t airspeed_ets_offset_tmp
Definition: airspeed_ets.c:109
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
Architecture independent I2C (Inter-Integrated Circuit Bus) API.