Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 "messages.h"
47 #include <math.h>
48 
49 #if !USE_AIRSPEED
50 #ifndef SENSOR_SYNC_SEND
51 #warning either set USE_AIRSPEED or SENSOR_SYNC_SEND to use ets_airspeed
52 #endif
53 #endif
54 
55 #define AIRSPEED_ETS_ADDR 0xEA
56 #ifndef AIRSPEED_ETS_SCALE
57 #define AIRSPEED_ETS_SCALE 1.8
58 #endif
59 #ifndef AIRSPEED_ETS_OFFSET
60 #define AIRSPEED_ETS_OFFSET 0
61 #endif
62 #define AIRSPEED_ETS_OFFSET_MAX 1750
63 #define AIRSPEED_ETS_OFFSET_MIN 1450
64 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT 40
65 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG 60
66 #define AIRSPEED_ETS_NBSAMPLES_AVRG 10
67 
68 #ifndef AIRSPEED_ETS_I2C_DEV
69 #define AIRSPEED_ETS_I2C_DEV i2c0
70 #endif
71 
72 #ifndef DOWNLINK_DEVICE
73 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
74 #endif
75 
76 // Global variables
83 
85 
86 // Local variables
87 volatile bool_t airspeed_ets_i2c_done;
91 
92 void airspeed_ets_init( void ) {
93  int n;
94  airspeed_ets_raw = 0;
95  airspeed_ets = 0.0;
102 
104  for (n=0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n)
105  airspeed_ets_buffer[n] = 0.0;
106 
108 }
109 
111 #ifndef SITL
114 #else // SITL
115  extern float sim_air_speed;
116  stateSetAirspeed_f(&sim_air_speed);
117 #endif //SITL
118 }
119 
121  int n;
122  float airspeed_tmp = 0.0;
123 
124  // Get raw airspeed from buffer
126  // Check if this is valid airspeed
127  if (airspeed_ets_raw == 0)
129  else
131 
132  // Continue only if a new airspeed value was received
133  if (airspeed_ets_valid) {
134  // Calculate offset average if not done already
137  // Check if averaging completed
138  if (airspeed_ets_cnt == 0) {
139  // Calculate average
141  // Limit offset
147  }
148  // Check if averaging needs to continue
151  }
152  // Convert raw to m/s
153 #ifdef AIRSPEED_ETS_REVERSE
155  airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_offset-airspeed_ets_raw) ) - AIRSPEED_ETS_OFFSET;
156 #else
158  airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_raw-airspeed_ets_offset) ) - AIRSPEED_ETS_OFFSET;
159 #endif
160  else
161  airspeed_tmp = 0.0;
162  // Airspeed should always be positive
163  if (airspeed_tmp < 0.0)
164  airspeed_tmp = 0.0;
165  // Moving average
169  airspeed_ets = 0.0;
170  for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n)
172  airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG;
173 #if USE_AIRSPEED
175 #endif
176 #ifdef SENSOR_SYNC_SEND
178 #endif
179  } else {
180  airspeed_ets = 0.0;
181  }
182 
183  // Transaction has been read
185 }
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT
Definition: airspeed_ets.c:64
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:66
float airspeed_tmp
float airspeed_ets
Definition: airspeed_ets.c:80
void airspeed_ets_init(void)
Definition: airspeed_ets.c:92
#define AIRSPEED_ETS_SCALE
Definition: airspeed_ets.c:57
#define AIRSPEED_ETS_OFFSET_MIN
Definition: airspeed_ets.c:63
#define FALSE
Definition: imu_chimu.h:141
uint16_t airspeed_ets_cnt
Definition: airspeed_ets.c:90
void airspeed_ets_read_periodic(void)
Definition: airspeed_ets.c:110
float sim_air_speed
Definition: sim_ir.c:14
#define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG
Definition: airspeed_ets.c:65
Driver for the EagleTree Systems Airspeed Sensor.
#define AIRSPEED_ETS_I2C_DEV
Definition: airspeed_ets.c:69
volatile bool_t airspeed_ets_i2c_done
Definition: airspeed_ets.c:87
#define AIRSPEED_ETS_ADDR
Definition: airspeed_ets.c:55
unsigned long uint32_t
Definition: types.h:18
static void stateSetAirspeed_f(float *airspeed)
Set airspeed (float).
Definition: state.h:1152
enum I2CTransactionStatus status
Definition: i2c.h:83
#define AIRSPEED_ETS_OFFSET
Definition: airspeed_ets.c:60
int airspeed_ets_buffer_idx
Definition: airspeed_ets.c:81
#define TRUE
Definition: imu_chimu.h:144
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:82
API to get/set the generic vehicle states.
struct i2c_transaction airspeed_ets_i2c_trans
Definition: airspeed_ets.c:84
uint16_t airspeed_ets_offset
Definition: airspeed_ets.c:78
bool_t airspeed_ets_valid
Definition: airspeed_ets.c:79
void airspeed_ets_read_event(void)
Definition: airspeed_ets.c:120
float airspeed_ets_buffer[AIRSPEED_ETS_NBSAMPLES_AVRG]
Definition: airspeed_ets.c:82
#define AIRSPEED_ETS_OFFSET_MAX
Definition: airspeed_ets.c:62
bool_t airspeed_ets_offset_init
Definition: airspeed_ets.c:88
uint16_t airspeed_ets_raw
Definition: airspeed_ets.c:77
uint32_t airspeed_ets_offset_tmp
Definition: airspeed_ets.c:89
bool_t i2c_receive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint16_t len)
Definition: i2c.c:95
Architecture independent I2C (Inter-Integrated Circuit Bus) API.