Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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;
120  airspeed_ets_i2c_done = true;
121  airspeed_ets_valid = false;
122  airspeed_ets_offset_init = false;
124 
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;
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
147  else { airspeed_ets_delay_done = true; }
148  }
151  }
152 #elif !defined USE_NPS
153  extern float 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
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
178  // Check if averaging completed
179  if (airspeed_ets_cnt == 0) {
180  // Calculate average
182  // Limit offset
185  }
188  }
190  }
191  // Check if averaging needs to continue
194  }
195  }
196  // Convert raw to m/s
197 #ifdef AIRSPEED_ETS_REVERSE
199  airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_offset - airspeed_ets_raw)) - AIRSPEED_ETS_OFFSET;
200  }
201 #else
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
222  }
223  airspeed_ets = 0.0;
224  for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) {
226  }
228 #if USE_AIRSPEED_ETS
230 #endif
231 #if AIRSPEED_ETS_SYNC_SEND
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",
248  gps.fix, gps.tow, gps.week,
251  }
252 #endif
253 #endif
254 
255 
256 
257  // Transaction has been read
259 }
stateSetAirspeed_f
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1309
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
uint16_t
unsigned short uint16_t
Definition: types.h:16
i2c_transaction::buf
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
pprzLogFile
FileDes pprzLogFile
Definition: sdlog_chibios.c:86
AIRSPEED_ETS_NBSAMPLES_AVRG
#define AIRSPEED_ETS_NBSAMPLES_AVRG
Definition: airspeed_ets.c:73
airspeed_ets_raw
uint16_t airspeed_ets_raw
Definition: airspeed_ets.c:96
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
airspeed_ets_delay_done
bool airspeed_ets_delay_done
Definition: airspeed_ets.c:111
AIRSPEED_ETS_SCALE
#define AIRSPEED_ETS_SCALE
Definition: airspeed_ets.c:64
uint32_t
unsigned long uint32_t
Definition: types.h:18
airspeed_ets
float airspeed_ets
Definition: airspeed_ets.c:99
SysTimeTimer
#define SysTimeTimer(_t)
Definition: sys_time.h:219
airspeed_ets_cnt
uint16_t airspeed_ets_cnt
Definition: airspeed_ets.c:109
airspeed_ets_init
void airspeed_ets_init(void)
Definition: airspeed_ets.c:113
GpsState::ned_vel
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
AIRSPEED_ETS_OFFSET_MIN
#define AIRSPEED_ETS_OFFSET_MIN
Definition: airspeed_ets.c:70
sim_air_speed
float sim_air_speed
Definition: sim_ir.c:14
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
sdlog_chibios.h
USEC_OF_SEC
#define USEC_OF_SEC(sec)
Definition: sys_time.h:210
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
AIRSPEED_ETS_I2C_DEV
#define AIRSPEED_ETS_I2C_DEV
Definition: airspeed_ets.c:76
gps.h
Device independent GPS code (interface)
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:107
airspeed_ets_offset_init
bool airspeed_ets_offset_init
Definition: airspeed_ets.c:107
airspeed_ets_read_periodic
void airspeed_ets_read_periodic(void)
Definition: airspeed_ets.c:142
AIRSPEED_ETS_ADDR
#define AIRSPEED_ETS_ADDR
Definition: airspeed_ets.c:62
GpsState::week
uint16_t week
GPS week.
Definition: gps.h:108
AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG
Definition: airspeed_ets.c:72
sys_time.h
Architecture independent timing functions.
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
AIRSPEED_ETS_OFFSET
#define AIRSPEED_ETS_OFFSET
Definition: airspeed_ets.c:67
i2c_transaction::status
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
i2c_transaction
I2C transaction structure.
Definition: i2c.h:93
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
airspeed_ets_buffer_idx
int airspeed_ets_buffer_idx
Definition: airspeed_ets.c:100
AIRSPEED_ETS_START_DELAY
#define AIRSPEED_ETS_START_DELAY
delay in seconds until sensor is read after startup
Definition: airspeed_ets.c:82
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
airspeed_ets_i2c_done
volatile bool airspeed_ets_i2c_done
Definition: airspeed_ets.c:106
airspeed_ets_delay_time
uint32_t airspeed_ets_delay_time
Definition: airspeed_ets.c:110
airspeed_ets.h
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
I2CTransDone
@ I2CTransDone
transaction set to done by user level
Definition: i2c.h:59
AIRSPEED_ETS_OFFSET_MAX
#define AIRSPEED_ETS_OFFSET_MAX
Definition: airspeed_ets.c:69
airspeed_ets_i2c_trans
struct i2c_transaction airspeed_ets_i2c_trans
Definition: airspeed_ets.c:103
airspeed_ets_offset
uint16_t airspeed_ets_offset
Definition: airspeed_ets.c:97
AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT
Definition: airspeed_ets.c:71
airspeed_ets_buffer
float airspeed_ets_buffer[AIRSPEED_ETS_NBSAMPLES_AVRG]
Definition: airspeed_ets.c:101
state.h
i2c.h
SysTimeTimerStart
#define SysTimeTimerStart(_t)
Definition: sys_time.h:218
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
airspeed_ets_read_event
void airspeed_ets_read_event(void)
Definition: airspeed_ets.c:158
i2c_receive
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
airspeed_ets_valid
bool airspeed_ets_valid
Definition: airspeed_ets.c:98
airspeed_ets_offset_tmp
uint32_t airspeed_ets_offset_tmp
Definition: airspeed_ets.c:108