Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 
41 #include "sensors/airspeed_ets.h"
42 #include "state.h"
43 #include "mcu_periph/i2c.h"
44 #include "mcu_periph/uart.h"
45 #include "mcu_periph/sys_time.h"
46 #include "messages.h"
48 #include <math.h>
49 
50 #if !USE_AIRSPEED
51 #ifndef AIRSPEED_ETS_SYNC_SEND
52 #warning either set USE_AIRSPEED or AIRSPEED_ETS_SYNC_SEND to use ets_airspeed
53 #endif
54 #endif
55 
56 #define AIRSPEED_ETS_ADDR 0xEA
57 #ifndef AIRSPEED_ETS_SCALE
58 #define AIRSPEED_ETS_SCALE 1.8
59 #endif
60 #ifndef AIRSPEED_ETS_OFFSET
61 #define AIRSPEED_ETS_OFFSET 0
62 #endif
63 #define AIRSPEED_ETS_OFFSET_MAX 1750
64 #define AIRSPEED_ETS_OFFSET_MIN 1450
65 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT 40
66 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG 60
67 #define AIRSPEED_ETS_NBSAMPLES_AVRG 10
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 
81 // Global variables
88 
90 
91 // Local variables
92 volatile bool_t airspeed_ets_i2c_done;
98 
99 void airspeed_ets_init( void ) {
100  int n;
101  airspeed_ets_raw = 0;
102  airspeed_ets = 0.0;
103  airspeed_ets_offset = 0;
104  airspeed_ets_offset_tmp = 0;
105  airspeed_ets_i2c_done = TRUE;
106  airspeed_ets_valid = FALSE;
107  airspeed_ets_offset_init = FALSE;
109 
110  airspeed_ets_buffer_idx = 0;
111  for (n=0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n)
112  airspeed_ets_buffer[n] = 0.0;
113 
114  airspeed_ets_i2c_trans.status = I2CTransDone;
115 
116  airspeed_ets_delay_done = FALSE;
117  SysTimeTimerStart(airspeed_ets_delay_time);
118 }
119 
121 #ifndef SITL
125  }
128 #elif !defined USE_NPS
129  extern float sim_air_speed;
130  stateSetAirspeed_f(&sim_air_speed);
131 #endif //SITL
132 }
133 
135  int n;
136  float airspeed_tmp = 0.0;
137 
138  // Get raw airspeed from buffer
140  // Check if this is valid airspeed
141  if (airspeed_ets_raw == 0)
143  else
145 
146  // Continue only if a new airspeed value was received
147  if (airspeed_ets_valid) {
148  // Calculate offset average if not done already
151  // Check if averaging completed
152  if (airspeed_ets_cnt == 0) {
153  // Calculate average
155  // Limit offset
161  }
162  // Check if averaging needs to continue
165  }
166  // Convert raw to m/s
167 #ifdef AIRSPEED_ETS_REVERSE
169  airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_offset-airspeed_ets_raw) ) - AIRSPEED_ETS_OFFSET;
170 #else
172  airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_raw-airspeed_ets_offset) ) - AIRSPEED_ETS_OFFSET;
173 #endif
174  else
175  airspeed_tmp = 0.0;
176  // Airspeed should always be positive
177  if (airspeed_tmp < 0.0)
178  airspeed_tmp = 0.0;
179  // Moving average
183  airspeed_ets = 0.0;
184  for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n)
186  airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG;
187 #if USE_AIRSPEED
189 #endif
190 #ifdef AIRSPEED_ETS_SYNC_SEND
192 #endif
193  } else {
194  airspeed_ets = 0.0;
195  }
196 
197  // Transaction has been read
199 }
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT
Definition: airspeed_ets.c:65
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define AIRSPEED_ETS_NBSAMPLES_AVRG
Definition: airspeed_ets.c:67
I2C transaction structure.
Definition: i2c.h:93
float airspeed_ets
Definition: airspeed_ets.c:85
void airspeed_ets_init(void)
Definition: airspeed_ets.c:99
#define AIRSPEED_ETS_SCALE
Definition: airspeed_ets.c:58
#define AIRSPEED_ETS_OFFSET_MIN
Definition: airspeed_ets.c:64
#define FALSE
Definition: imu_chimu.h:141
uint16_t airspeed_ets_cnt
Definition: airspeed_ets.c:95
void airspeed_ets_read_periodic(void)
Definition: airspeed_ets.c:120
float sim_air_speed
Definition: sim_ir.c:14
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG
Definition: airspeed_ets.c:66
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:70
volatile bool_t airspeed_ets_i2c_done
Definition: airspeed_ets.c:92
#define AIRSPEED_ETS_ADDR
Definition: airspeed_ets.c:56
unsigned long uint32_t
Definition: types.h:18
static void stateSetAirspeed_f(float *airspeed)
Set airspeed (float).
Definition: state.h:1152
#define AIRSPEED_ETS_START_DELAY
delay in seconds until sensor is read after startup
Definition: airspeed_ets.c:76
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
#define AIRSPEED_ETS_OFFSET
Definition: airspeed_ets.c:61
int airspeed_ets_buffer_idx
Definition: airspeed_ets.c:86
#define USEC_OF_SEC(sec)
Definition: sys_time.h:190
bool_t airspeed_ets_delay_done
Definition: airspeed_ets.c:97
#define SysTimeTimer(_t)
Definition: sys_time.h:199
#define TRUE
Definition: imu_chimu.h:144
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
API to get/set the generic vehicle states.
struct i2c_transaction airspeed_ets_i2c_trans
Definition: airspeed_ets.c:89
uint16_t airspeed_ets_offset
Definition: airspeed_ets.c:83
bool_t airspeed_ets_valid
Definition: airspeed_ets.c:84
void airspeed_ets_read_event(void)
Definition: airspeed_ets.c:134
float airspeed_ets_buffer[AIRSPEED_ETS_NBSAMPLES_AVRG]
Definition: airspeed_ets.c:87
#define SysTimeTimerStart(_t)
Definition: sys_time.h:198
#define AIRSPEED_ETS_OFFSET_MAX
Definition: airspeed_ets.c:63
uint32_t airspeed_ets_delay_time
Definition: airspeed_ets.c:96
bool_t airspeed_ets_offset_init
Definition: airspeed_ets.c:93
uint16_t airspeed_ets_raw
Definition: airspeed_ets.c:82
uint32_t airspeed_ets_offset_tmp
Definition: airspeed_ets.c:94
bool_t 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:248
Architecture independent I2C (Inter-Integrated Circuit Bus) API.