Paparazzi UAS  v6.2_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
122 #endif
123 
124 #elif !defined USE_NPS
125  extern float 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
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 
airspeed_amsys_raw
uint16_t airspeed_amsys_raw
Definition: airspeed_amsys.c:71
stateSetAirspeed_f
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1309
i2c_transaction::buf
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
airspeed_amsys_init
void airspeed_amsys_init(void)
Definition: airspeed_amsys.c:92
airspeed_scale
float airspeed_scale
Definition: airspeed_amsys.c:78
sim_air_speed
float sim_air_speed
Definition: sim_airspeed.c:10
airspeed_amsys_i2c_trans
struct i2c_transaction airspeed_amsys_i2c_trans
Definition: airspeed_amsys.c:80
airspeed_amsys_valid
bool airspeed_amsys_valid
Definition: airspeed_amsys.c:73
airspeed_filter
float airspeed_filter
Definition: airspeed_amsys.c:79
airspeed_amsys_offset
float airspeed_amsys_offset
Definition: airspeed_amsys.c:74
AIRSPEED_AMSYS_ADDR
#define AIRSPEED_AMSYS_ADDR
Definition: airspeed_amsys.c:36
AIRSPEED_AMSYS_MAXPRESURE
#define AIRSPEED_AMSYS_MAXPRESURE
Definition: airspeed_amsys.c:46
AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT
#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT
Definition: airspeed_amsys.c:42
AIRSPEED_AMSYS_OFFSET_MIN
#define AIRSPEED_AMSYS_OFFSET_MIN
Definition: airspeed_amsys.c:41
airspeed_amsys
float airspeed_amsys
Definition: airspeed_amsys.c:77
AIRSPEED_AMSYS_OFFSET_MAX
#define AIRSPEED_AMSYS_OFFSET_MAX
Definition: airspeed_amsys.c:40
airspeed_amsys_offset_init
bool airspeed_amsys_offset_init
Definition: airspeed_amsys.c:86
AIRSPEED_AMSYS_SCALE
#define AIRSPEED_AMSYS_SCALE
Definition: airspeed_amsys.c:38
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
i2c_transaction::status
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
airspeed_amsys_cnt
uint16_t airspeed_amsys_cnt
Definition: airspeed_amsys.c:88
airspeed_amsys_read_event
void airspeed_amsys_read_event(void)
Definition: airspeed_amsys.c:143
i2c_transaction
I2C transaction structure.
Definition: i2c.h:93
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
airspeed_amsys.h
airspeed_amsys_offset_tmp
double airspeed_amsys_offset_tmp
Definition: airspeed_amsys.c:87
airspeed_amsys_read_periodic
void airspeed_amsys_read_periodic(void)
Definition: airspeed_amsys.c:109
AIRSPEED_AMSYS_FILTER
#define AIRSPEED_AMSYS_FILTER
Definition: airspeed_amsys.c:49
airspeed_amsys_p
float airspeed_amsys_p
Definition: airspeed_amsys.c:76
I2CTransDone
@ I2CTransDone
transaction set to done by user level
Definition: i2c.h:59
AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG
#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG
Definition: airspeed_amsys.c:43
state.h
airspeed_amsys_i2c_done
volatile bool airspeed_amsys_i2c_done
Definition: airspeed_amsys.c:83
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
airspeed_old
float airspeed_old
Definition: airspeed_amsys.c:85
i2c.h
airspeed_amsys_tmp
float airspeed_amsys_tmp
Definition: airspeed_amsys.c:75
tempAS_amsys_raw
uint16_t tempAS_amsys_raw
Definition: airspeed_amsys.c:72
AIRSPEED_AMSYS_I2C_DEV
#define AIRSPEED_AMSYS_I2C_DEV
Definition: airspeed_amsys.c:52
i2c_receive
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
airspeed_temperature
float airspeed_temperature
Definition: airspeed_amsys.c:84
airspeed_amsys_downlink
void airspeed_amsys_downlink(void)
Definition: airspeed_amsys.c:135