Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
41
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 "modules/core/abi.h"
50#include <math.h>
51
52#if PERIODIC_TELEMETRY
54#endif
55
56#define AIRSPEED_ETS_ADDR 0xEA
57
58#ifndef AIRSPEED_ETS_SCALE
59#define AIRSPEED_ETS_SCALE 1.8
60#endif
61#ifndef AIRSPEED_ETS_OFFSET
62#define AIRSPEED_ETS_OFFSET 0
63#endif
64#define AIRSPEED_ETS_OFFSET_MAX 1750
65#define AIRSPEED_ETS_OFFSET_MIN 1450
66#define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT 40
67#define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG 60
68
69#ifndef AIRSPEED_ETS_I2C_DEV
70#define AIRSPEED_ETS_I2C_DEV i2c0
71#endif
73
74
75#ifndef AIRSPEED_ETS_START_DELAY
76#define AIRSPEED_ETS_START_DELAY 0.2
77#endif
79
80#ifndef SITL
81#if AIRSPEED_ETS_SDLOG
83#include "modules/gps/gps.h"
84bool log_airspeed_ets_started;
85#endif
86#endif
87
88
89
90// Global variables
95
97
98// Local variables
105
107{
109 airspeed_ets = 0.0;
113 airspeed_ets_valid = false;
116
118
121
122#ifndef SITL
123#if AIRSPEED_ETS_SDLOG
124 log_airspeed_ets_started = false;
125#endif
126#endif
127}
128
130{
131#ifndef SITL
134 else { airspeed_ets_delay_done = true; }
135 }
136 if (airspeed_ets_i2c_trans.status == I2CTransDone) {
138 }
139#elif !defined USE_NPS
140 extern float sim_air_speed;
141 AbiSendMsgAIRSPEED_RAW(AIRSPEED_ETS_ID, get_sys_time_usec(), sim_air_speed);
142#endif //SITL
143}
144
146{
147 float airspeed_tmp = 0.0;
148
149 // Get raw airspeed from buffer
151 // Check if this is valid airspeed
152 if (airspeed_ets_raw == 0) {
153 airspeed_ets_valid = false;
154 } else {
155 airspeed_ets_valid = true;
156 }
157
158 // Continue only if a new airspeed value was received
159 if (airspeed_ets_valid) {
160#if !AIRSPEED_ETS_3RD_PARTY_MODE
161
162 // Calculate offset average if not done already
165 // Check if averaging completed
166 if (airspeed_ets_cnt == 0) {
167 // Calculate average
169 // Limit offset
172 }
175 }
177 }
178 // Check if averaging needs to continue
181 }
182 }
183
184 // Convert raw to m/s
185#ifdef AIRSPEED_ETS_REVERSE
187 airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_offset - airspeed_ets_raw)) - (float)AIRSPEED_ETS_OFFSET;
188 }
189#else
191 airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_raw - airspeed_ets_offset)) - (float)AIRSPEED_ETS_OFFSET;
192 }
193#endif
194 else {
195 airspeed_tmp = 0.0;
196 }
197//use raw value for sensor set to third-party mode
198#else
199 airspeed_tmp = airspeed_ets_raw;
200#endif //AIRSPEED_ETS_3RD_PARTY_MODE
201
202 // Airspeed should always be positive
203 if (airspeed_tmp < 0.0) {
204 airspeed_tmp = 0.0;
205 }
206 // air_data filters it
207 airspeed_ets = airspeed_tmp;
208
209 // Publish airspeed sensor, air_data filters and calibrates it and sets the airspeed
210 AbiSendMsgAIRSPEED_RAW(AIRSPEED_ETS_ID, get_sys_time_usec(), airspeed_ets);
211 } else {
212 airspeed_ets = 0.0;
213 }
214
215
216#if AIRSPEED_ETS_SDLOG
217#ifndef SITL
218 if (pprzLogFile != -1) {
219 if (!log_airspeed_ets_started) {
220 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");
221 log_airspeed_ets_started = true;
222 }
223 sdLogWriteLog(pprzLogFile, "airspeed_ets: %d %d %8.4f %d %d %d %d %d %d %d %d %d\n",
225 gps.fix, gps.tow, gps.week,
226 gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl,
227 gps.gspeed, gps.course, -gps.ned_vel.z);
228 }
229#endif
230#endif
231
232
233
234 // Transaction has been read
236}
Main include for ABI (AirBorneInterface).
#define AIRSPEED_ETS_ID
#define AIRSPEED_ETS_OFFSET_MIN
bool airspeed_ets_delay_done
#define AIRSPEED_ETS_SCALE
bool airspeed_ets_offset_init
volatile bool airspeed_ets_i2c_done
uint32_t airspeed_ets_offset_tmp
uint16_t airspeed_ets_cnt
float airspeed_ets
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG
uint16_t airspeed_ets_raw
#define AIRSPEED_ETS_START_DELAY
delay in seconds until sensor is read after startup
#define AIRSPEED_ETS_OFFSET_MAX
void airspeed_ets_read_event(void)
uint32_t airspeed_ets_delay_time
bool airspeed_ets_valid
uint16_t airspeed_ets_offset
void airspeed_ets_init(void)
void airspeed_ets_read_periodic(void)
#define AIRSPEED_ETS_OFFSET
struct i2c_transaction airspeed_ets_i2c_trans
#define AIRSPEED_ETS_I2C_DEV
#define AIRSPEED_ETS_ADDR
Driver for the EagleTree Systems Airspeed Sensor.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
struct GpsState gps
global GPS state
Definition gps.c:74
Device independent GPS code (interface).
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:212
@ I2CTransDone
transaction set to done by user level
Definition i2c.h:60
I2C transaction structure.
Definition i2c.h:94
Architecture independent I2C (Inter-Integrated Circuit Bus) API.
static FILE * pprzLogFile
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
SdioError sdLogWriteLog(const FileDes fd, const char *fmt,...)
API to get/set the generic vehicle states.
Architecture independent timing functions.
#define SysTimeTimerStart(_t)
Definition sys_time.h:329
#define USEC_OF_SEC(sec)
Definition sys_time.h:322
#define SysTimeTimer(_t)
Definition sys_time.h:330
Periodic telemetry system header (includes downlink utility and generated code).
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.