Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
airspeed_amsys.c
Go to the documentation of this file.
1 /*
2  * Driver for a Amsys Differential Presure Sensor I2C
3  * AMS 5812-0003-D
4  * AMS 5812-0001-D
5  *
6  * Copyright (C) 2010 The Paparazzi Team
7  *
8  * This file is part of paparazzi.
9  *
10  * paparazzi is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2, or (at your option)
13  * any later version.
14  *
15  * paparazzi is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with paparazzi; see the file COPYING. If not, write to
22  * the Free Software Foundation, 59 Temple Place - Suite 330,
23  * Boston, MA 02111-1307, USA.
24  */
25 
26 #include "sensors/airspeed_amsys.h"
27 #include "state.h"
28 #include "mcu_periph/i2c.h"
29 #include "mcu_periph/uart.h"
30 #include "pprzlink/messages.h"
32 #include <math.h>
33 //#include <stdlib.h>
34 
35 
36 #define AIRSPEED_AMSYS_ADDR 0xE8 // original F0
37 #ifndef AIRSPEED_AMSYS_SCALE
38 #define AIRSPEED_AMSYS_SCALE 1
39 #endif
40 #define AIRSPEED_AMSYS_OFFSET_MAX 29491
41 #define AIRSPEED_AMSYS_OFFSET_MIN 3277
42 #define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT 40
43 #define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG 60
44 #define AIRSPEED_AMSYS_NBSAMPLES_AVRG 10
45 #ifndef AIRSPEED_AMSYS_MAXPRESURE
46 #define AIRSPEED_AMSYS_MAXPRESURE 2068 //003-2068, 001-1034 //Pascal
47 #endif
48 #ifndef AIRSPEED_AMSYS_FILTER
49 #define AIRSPEED_AMSYS_FILTER 0
50 #endif
51 #ifndef AIRSPEED_AMSYS_I2C_DEV
52 #define AIRSPEED_AMSYS_I2C_DEV i2c0
53 #endif
54 #ifdef MEASURE_AMSYS_TEMPERATURE
55 #define TEMPERATURE_AMSYS_OFFSET_MAX 29491
56 #define TEMPERATURE_AMSYS_OFFSET_MIN 3277
57 #define TEMPERATURE_AMSYS_MAX 85
58 #define TEMPERATURE_AMSYS_MIN -25
59 #endif
60 
61 #ifndef USE_AIRSPEED_AMSYS
62 #if USE_AIRSPEED
63 #define USE_AIRSPEED_AMSYS TRUE
64 PRINT_CONFIG_MSG("USE_AIRSPEED_AMSYS automatically set to TRUE")
65 #endif
66 #endif
67 
68 
69 
70 // Global variables
76 float airspeed_amsys_p; //Pascal
77 float airspeed_amsys; //mps
81 
82 // Local variables
85 float airspeed_old = 0.0;
89 
90 void airspeed_amsys_downlink(void);
91 
93 {
94  airspeed_amsys_raw = 0;
95  airspeed_amsys = 0.0;
96  airspeed_amsys_p = 0.0;
97  airspeed_amsys_offset = 0;
98  airspeed_amsys_offset_tmp = 0;
99  airspeed_amsys_i2c_done = true;
100  airspeed_amsys_valid = true;
101  airspeed_amsys_offset_init = false;
102  airspeed_scale = AIRSPEED_AMSYS_SCALE;
103  airspeed_filter = AIRSPEED_AMSYS_FILTER;
105  airspeed_amsys_cnt = AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT +
107 }
108 
110 {
111 #ifndef SITL
113 #ifndef MEASURE_AMSYS_TEMPERATURE
115 #else
117 #endif
118  }
119 
120 #if USE_AIRSPEED_AMSYS
121  stateSetAirspeed_f(airspeed_amsys);
122 #endif
123 
124 #elif !defined USE_NPS
125  extern float sim_air_speed;
126  stateSetAirspeed_f(sim_air_speed);
127 #endif //SITL
128 
129 
130 #ifndef AIRSPEED_AMSYS_SYNC_SEND
131  RunOnceEvery(10, airspeed_amsys_downlink());
132 #endif
133 }
134 
136 {
137  DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice,
138  &airspeed_amsys_raw, &airspeed_amsys_p,
139  &airspeed_amsys_tmp, &airspeed_amsys,
140  &airspeed_temperature);
141 }
142 
144 {
145 
146  // Get raw airspeed from buffer
147  airspeed_amsys_raw = 0;
148  airspeed_amsys_raw = (airspeed_amsys_i2c_trans.buf[0] << 8) | airspeed_amsys_i2c_trans.buf[1];
149 #ifdef MEASURE_AMSYS_TEMPERATURE
150  tempAS_amsys_raw = (airspeed_amsys_i2c_trans.buf[2] << 8) | airspeed_amsys_i2c_trans.buf[3];
151  const float temp_off_scale = (float)(TEMPERATURE_AMSYS_MAX - TEMPERATURE_AMSYS_MIN) /
152  (TEMPERATURE_AMSYS_OFFSET_MAX - TEMPERATURE_AMSYS_OFFSET_MIN);
153  // Tmin=-25, Tmax=85
154  airspeed_temperature = temp_off_scale * (tempAS_amsys_raw - TEMPERATURE_AMSYS_OFFSET_MIN) +
155  TEMPERATURE_AMSYS_MIN;
156 #endif
157 
158  // Check if this is valid airspeed
159  if (airspeed_amsys_raw == 0) {
160  airspeed_amsys_valid = false;
161  } else {
162  airspeed_amsys_valid = true;
163  }
164 
165  // Continue only if a new airspeed value was received
166  if (airspeed_amsys_valid) {
167 
168  // raw not under offest min
169  if (airspeed_amsys_raw < AIRSPEED_AMSYS_OFFSET_MIN) {
170  airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MIN;
171  }
172  // raw not over offest max
173  if (airspeed_amsys_raw > AIRSPEED_AMSYS_OFFSET_MAX) {
174  airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MAX;
175  }
176 
177  // calculate raw to pressure
178  const float p_off_scale = (float)(AIRSPEED_AMSYS_MAXPRESURE) /
180  airspeed_amsys_p = p_off_scale * (airspeed_amsys_raw - AIRSPEED_AMSYS_OFFSET_MIN);
181 
182  if (!airspeed_amsys_offset_init) {
184  // Check if averaging completed
185  if (airspeed_amsys_cnt == 0) {
186  // Calculate average
187  airspeed_amsys_offset = airspeed_amsys_offset_tmp / AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG;
188  airspeed_amsys_offset_init = true;
189  }
190  // Check if averaging needs to continue
191  else if (airspeed_amsys_cnt <= AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG) {
192  airspeed_amsys_offset_tmp += airspeed_amsys_p;
193  }
194 
195  airspeed_amsys = 0.;
196 
197  } else {
198  airspeed_amsys_p = airspeed_amsys_p - airspeed_amsys_offset;
199  if (airspeed_amsys_p <= 0) {
200  airspeed_amsys_p = 0.000000001;
201  }
202  // convert pressure to airspeed
203  airspeed_amsys_tmp = sqrtf(2 * airspeed_amsys_p * airspeed_scale / 1.2041);
204  // Lowpassfiltering
205  airspeed_amsys = airspeed_filter * airspeed_old +
206  (1.0 - airspeed_filter) * airspeed_amsys_tmp;
207  airspeed_old = airspeed_amsys;
208 
209  //New value available
210 #if USE_AIRSPEED
211  stateSetAirspeed_f(airspeed_amsys);
212 #endif
213 #ifdef AIRSPEED_AMSYS_SYNC_SEND
215 #endif
216  }
217 
218  }
219  /*else {
220  airspeed_amsys = 0.0;
221  }*/
222 
223 
224  // Transaction has been read
226 }
227 
unsigned short uint16_t
Definition: types.h:16
float airspeed_amsys
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
float airspeed_temperature
#define AIRSPEED_AMSYS_ADDR
#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT
volatile bool airspeed_amsys_i2c_done
float airspeed_filter
float sim_air_speed
Definition: sim_ir.c:14
#define AIRSPEED_AMSYS_OFFSET_MIN
struct i2c_transaction airspeed_amsys_i2c_trans
transaction set to done by user level
Definition: i2c.h:59
#define AIRSPEED_AMSYS_FILTER
float airspeed_amsys_p
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
I2C transaction structure.
Definition: i2c.h:93
float airspeed_scale
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
bool airspeed_amsys_valid
#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG
API to get/set the generic vehicle states.
uint16_t airspeed_amsys_raw
float airspeed_amsys_offset
uint16_t airspeed_amsys_cnt
void airspeed_amsys_read_event(void)
void airspeed_amsys_init(void)
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1309
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:268
double airspeed_amsys_offset_tmp
float airspeed_old
#define AIRSPEED_AMSYS_I2C_DEV
float airspeed_amsys_tmp
#define AIRSPEED_AMSYS_OFFSET_MAX
bool airspeed_amsys_offset_init
void airspeed_amsys_downlink(void)
uint16_t tempAS_amsys_raw
void airspeed_amsys_read_periodic(void)
#define AIRSPEED_AMSYS_SCALE
#define AIRSPEED_AMSYS_MAXPRESURE
Architecture independent I2C (Inter-Integrated Circuit Bus) API.