Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 {
95  airspeed_amsys = 0.0;
96  airspeed_amsys_p = 0.0;
100  airspeed_amsys_valid = true;
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(MODULE_AIRSPEED_AMSYS_ID, airspeed_amsys);
122 #endif
123 
124 #elif !defined USE_NPS
125  extern float sim_air_speed;
126  stateSetAirspeed_f(MODULE_AIRSPEED_AMSYS_ID, 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,
141 }
142 
144 {
145 
146  // Get raw airspeed from buffer
147  airspeed_amsys_raw = 0;
149 #ifdef MEASURE_AMSYS_TEMPERATURE
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
171  }
172  // raw not over offest max
175  }
176 
177  // calculate raw to pressure
178  const float p_off_scale = (float)(AIRSPEED_AMSYS_MAXPRESURE) /
181 
184  // Check if averaging completed
185  if (airspeed_amsys_cnt == 0) {
186  // Calculate average
189  }
190  // Check if averaging needs to continue
193  }
194 
195  airspeed_amsys = 0.;
196 
197  } else {
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
208 
209  //New value available
210 #if USE_AIRSPEED
211  stateSetAirspeed_f(MODULE_AIRSPEED_AMSYS_ID, 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 
void airspeed_amsys_init(void)
double airspeed_amsys_offset_tmp
float airspeed_old
#define AIRSPEED_AMSYS_OFFSET_MAX
struct i2c_transaction airspeed_amsys_i2c_trans
uint16_t tempAS_amsys_raw
#define AIRSPEED_AMSYS_SCALE
float airspeed_scale
float airspeed_amsys_offset
float airspeed_filter
#define AIRSPEED_AMSYS_OFFSET_MIN
void airspeed_amsys_read_event(void)
float airspeed_temperature
float airspeed_amsys_tmp
volatile bool airspeed_amsys_i2c_done
#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG
float airspeed_amsys
#define AIRSPEED_AMSYS_FILTER
uint16_t airspeed_amsys_raw
void airspeed_amsys_downlink(void)
bool airspeed_amsys_offset_init
void airspeed_amsys_read_periodic(void)
float airspeed_amsys_p
uint16_t airspeed_amsys_cnt
bool airspeed_amsys_valid
#define AIRSPEED_AMSYS_I2C_DEV
#define AIRSPEED_AMSYS_ADDR
#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT
#define AIRSPEED_AMSYS_MAXPRESURE
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
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:334
@ I2CTransDone
transaction set to done by user level
Definition: i2c.h:59
I2C transaction structure.
Definition: i2c.h:93
static void stateSetAirspeed_f(uint16_t id, float airspeed)
Set airspeed (float).
Definition: state.h:1486
Architecture independent I2C (Inter-Integrated Circuit Bus) API.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
API to get/set the generic vehicle states.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88