Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
airspeed_ets.c
Go to the documentation of this file.
1 /*
2  * Driver for the EagleTree Systems Airspeed Sensor
3  * Has only been tested with V3 of the sensor hardware
4  *
5  * Notes:
6  * Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together.
7  * Sensor should be in the proprietary mode (default) and not in 3rd party mode.
8  *
9  * Sensor module wire assignments:
10  * Red wire: 5V
11  * White wire: Ground
12  * Yellow wire: SDA
13  * Brown wire: SCL
14  *
15  * Copyright (C) 2009 Vassilis Varveropoulos
16  * Modified by Mark Griffin on 8 September 2010 to work with new i2c transaction routines.
17  * Converted by Gautier Hattenberger to modules (10/2010)
18  *
19  * This file is part of paparazzi.
20  *
21  * paparazzi is free software; you can redistribute it and/or modify
22  * it under the terms of the GNU General Public License as published by
23  * the Free Software Foundation; either version 2, or (at your option)
24  * any later version.
25  *
26  * paparazzi is distributed in the hope that it will be useful,
27  * but WITHOUT ANY WARRANTY; without even the implied warranty of
28  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
29  * GNU General Public License for more details.
30  *
31  * You should have received a copy of the GNU General Public License
32  * along with paparazzi; see the file COPYING. If not, write to
33  * the Free Software Foundation, 59 Temple Place - Suite 330,
34  * Boston, MA 02111-1307, USA.
35  *
36  */
37 #include "sensors/airspeed_ets.h"
38 #include "estimator.h"
39 #include "mcu_periph/i2c.h"
40 #include "mcu_periph/uart.h"
41 #include "messages.h"
43 #include <math.h>
44 
45 #if !USE_AIRSPEED
46 #ifndef SENSOR_SYNC_SEND
47 #warning either set USE_AIRSPEED or SENSOR_SYNC_SEND to use ets_airspeed
48 #endif
49 #endif
50 
51 #define AIRSPEED_ETS_ADDR 0xEA
52 #ifndef AIRSPEED_ETS_SCALE
53 #define AIRSPEED_ETS_SCALE 1.8
54 #endif
55 #ifndef AIRSPEED_ETS_OFFSET
56 #define AIRSPEED_ETS_OFFSET 0
57 #endif
58 #define AIRSPEED_ETS_OFFSET_MAX 1750
59 #define AIRSPEED_ETS_OFFSET_MIN 1450
60 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT 40
61 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG 60
62 #define AIRSPEED_ETS_NBSAMPLES_AVRG 10
63 
64 #ifndef AIRSPEED_ETS_I2C_DEV
65 #define AIRSPEED_ETS_I2C_DEV i2c0
66 #endif
67 
68 #ifndef DOWNLINK_DEVICE
69 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
70 #endif
71 
72 // Global variables
79 
81 
82 // Local variables
83 volatile bool_t airspeed_ets_i2c_done;
87 
88 void airspeed_ets_init( void ) {
89  int n;
90  airspeed_ets_raw = 0;
91  airspeed_ets = 0.0;
98 
100  for (n=0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n)
101  airspeed_ets_buffer[n] = 0.0;
102 
104 }
105 
107 #ifndef SITL
110 #else // SITL
111  extern float sim_air_speed;
112  EstimatorSetAirspeed(sim_air_speed);
113 #endif //SITL
114 }
115 
117  int n;
118  float airspeed_tmp = 0.0;
119 
120  // Get raw airspeed from buffer
122  // Check if this is valid airspeed
123  if (airspeed_ets_raw == 0)
125  else
127 
128  // Continue only if a new airspeed value was received
129  if (airspeed_ets_valid) {
130  // Calculate offset average if not done already
133  // Check if averaging completed
134  if (airspeed_ets_cnt == 0) {
135  // Calculate average
137  // Limit offset
143  }
144  // Check if averaging needs to continue
147  }
148  // Convert raw to m/s
149 #ifdef AIRSPEED_ETS_REVERSE
151  airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_offset-airspeed_ets_raw) ) - AIRSPEED_ETS_OFFSET;
152 #else
154  airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_raw-airspeed_ets_offset) ) - AIRSPEED_ETS_OFFSET;
155 #endif
156  else
157  airspeed_tmp = 0.0;
158  // Airspeed should always be positive
159  if (airspeed_tmp < 0.0)
160  airspeed_tmp = 0.0;
161  // Moving average
165  airspeed_ets = 0.0;
166  for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n)
168  airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG;
169 #if USE_AIRSPEED
171 #endif
172 #ifdef SENSOR_SYNC_SEND
174 #endif
175  } else {
176  airspeed_ets = 0.0;
177  }
178 
179  // Transaction has been read
181 }
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT
Definition: airspeed_ets.c:60
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define I2CReceive(_p, _t, _s_addr, _len)
Definition: i2c.h:140
#define AIRSPEED_ETS_NBSAMPLES_AVRG
Definition: airspeed_ets.c:62
float airspeed_tmp
float airspeed_ets
Definition: airspeed_ets.c:76
void airspeed_ets_init(void)
Definition: airspeed_ets.c:88
#define AIRSPEED_ETS_SCALE
Definition: airspeed_ets.c:53
#define AIRSPEED_ETS_OFFSET_MIN
Definition: airspeed_ets.c:59
#define EstimatorSetAirspeed(airspeed)
Definition: estimator.h:128
#define FALSE
Definition: imu_chimu.h:141
uint16_t airspeed_ets_cnt
Definition: airspeed_ets.c:86
void airspeed_ets_read_periodic(void)
Definition: airspeed_ets.c:106
float sim_air_speed
Definition: sim_ir.c:14
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG
Definition: airspeed_ets.c:61
#define AIRSPEED_ETS_I2C_DEV
Definition: airspeed_ets.c:65
volatile bool_t airspeed_ets_i2c_done
Definition: airspeed_ets.c:83
#define AIRSPEED_ETS_ADDR
Definition: airspeed_ets.c:51
unsigned long uint32_t
Definition: types.h:18
enum I2CTransactionStatus status
Definition: i2c.h:47
#define AIRSPEED_ETS_OFFSET
Definition: airspeed_ets.c:56
int airspeed_ets_buffer_idx
Definition: airspeed_ets.c:77
#define TRUE
Definition: imu_chimu.h:144
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:46
struct i2c_transaction airspeed_ets_i2c_trans
Definition: airspeed_ets.c:80
uint16_t airspeed_ets_offset
Definition: airspeed_ets.c:74
State estimation, fusioning sensors.
bool_t airspeed_ets_valid
Definition: airspeed_ets.c:75
void airspeed_ets_read_event(void)
Definition: airspeed_ets.c:116
float airspeed_ets_buffer[AIRSPEED_ETS_NBSAMPLES_AVRG]
Definition: airspeed_ets.c:78
#define AIRSPEED_ETS_OFFSET_MAX
Definition: airspeed_ets.c:58
bool_t airspeed_ets_offset_init
Definition: airspeed_ets.c:84
uint16_t airspeed_ets_raw
Definition: airspeed_ets.c:73
uint32_t airspeed_ets_offset_tmp
Definition: airspeed_ets.c:85