Paparazzi UAS  v5.15_devel-109-gee85905
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
airspeed_ms45xx_i2c.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
3  * 2014 Felix Ruess <felix.ruess@gmail.com>
4  *
5  * This file is part of paparazzi
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
28 #include "std.h"
29 #include "mcu_periph/i2c.h"
32 #include "subsystems/abi.h"
33 
34 #include "mcu_periph/uart.h"
35 #include "pprzlink/messages.h"
37 
38 #if PERIODIC_TELEMETRY
40 #endif
41 
42 #ifndef USE_AIRSPEED_MS45XX
43 #if USE_AIRSPEED
44 #define USE_AIRSPEED_MS45XX TRUE
45 PRINT_CONFIG_MSG("USE_AIRSPEED_MS45XX set to TRUE since this is set USE_AIRSPEED")
46 #endif
47 #endif
48 
49 #if USE_AIRSPEED_MS45XX
50 #include "state.h"
51 #endif
52 
55 #ifndef MS45XX_I2C_DEV
56 #define MS45XX_I2C_DEV i2c2
57 #endif
58 
61 #ifndef MS45XX_I2C_ADDR
62 #define MS45XX_I2C_ADDR 0x50
63 #endif
64 
68 #ifndef MS45XX_PRESSURE_OUTPUT_TYPE_InH2O
69 #define MS45XX_PRESSURE_OUTPUT_TYPE_InH2O 0
70 #endif
71 
76 #ifndef MS45XX_PRESSURE_RANGE
77 #define MS45XX_PRESSURE_RANGE 1
78 #endif
79 
82 #ifndef MS45XX_PRESSURE_TYPE
83 #define MS45XX_PRESSURE_TYPE 0
84 #endif
85 
88 #ifndef USE_AIRSPEED_LOWPASS_FILTER
89 #define USE_AIRSPEED_LOWPASS_FILTER TRUE
90 #endif
91 
96 #ifndef MS45XX_OUTPUT_TYPE
97 #define MS45XX_OUTPUT_TYPE 0
98 #endif
99 
101 #define InH2O_TO_PA 249.08891
102 
104 #define PSI_TO_PA 6894.75729
105 
106 #if MS45XX_PRESSURE_OUTPUT_TYPE_InH2O
107 #define OutputPressureToPa InH2O_TO_PA
108 #else
109 #define OutputPressureToPa PSI_TO_PA
110 #endif
111 
112 #if MS45XX_OUTPUT_TYPE == 0
113 /* Offset and scaling for OUTPUT TYPE A:
114  * p_raw = (0.8*16383)/ (Pmax - Pmin) * (pressure - Pmin) + 0.1*16383
115  * For differential sensors Pmax = MS45XX_PRESSURE_RANGE = -Pmin.
116  *
117  * p_diff = (p_raw - 0.1*16383) * 2*RANGE/(0.8*16383) - RANGE
118  * p_diff = p_raw * 2*RANGE/(0.8*16383) - (RANGE + (0.1 * 16383) * 2*RANGE/(0.8*16383)
119  * p_diff = p_raw * 2*RANGE/(0.8*16383) - (1.25 * RANGE)
120  * p_diff = p_raw * scale - offset
121  * then convert to Pascal
122  */
123 #ifndef MS45XX_PRESSURE_SCALE
124 #define MS45XX_PRESSURE_SCALE (2 * MS45XX_PRESSURE_RANGE / (0.8 * 16383) * OutputPressureToPa)
125 #endif
126 #ifndef MS45XX_PRESSURE_OFFSET
127 #define MS45XX_PRESSURE_OFFSET (1.25 * MS45XX_PRESSURE_RANGE * OutputPressureToPa)
128 #endif
129 #else /* Can still be improved using another if statment with MS45XX_PRESSURE_TYPE etc. */
130 /* Offset and scaling for OUTPUT TYPE B:
131  * p_raw = (0.9*16383)/ (Pmax - Pmin) * (pressure - Pmin) + 0.05*16383
132  */
133 #ifndef MS45XX_PRESSURE_SCALE
134 #define MS45XX_PRESSURE_SCALE (MS45XX_PRESSURE_RANGE/(0.9*16383)*OutputPressureToPa)
135 #endif
136 #ifndef MS45XX_PRESSURE_OFFSET
137 #define MS45XX_PRESSURE_OFFSET (((MS45XX_PRESSURE_RANGE*0.05*16383)/(0.9*16383))*OutputPressureToPa)
138 #endif
139 #endif
140 
141 PRINT_CONFIG_VAR(MS45XX_OUTPUT_TYPE)
142 PRINT_CONFIG_VAR(MS45XX_PRESSURE_TYPE)
143 PRINT_CONFIG_VAR(MS45XX_PRESSURE_RANGE)
144 PRINT_CONFIG_VAR(MS45XX_PRESSURE_SCALE)
145 PRINT_CONFIG_VAR(MS45XX_PRESSURE_OFFSET)
146 
150 #ifndef MS45XX_SYNC_SEND
151 #define MS45XX_SYNC_SEND FALSE
152 #endif
153 
159 #ifndef MS45XX_AIRSPEED_SCALE
160 #define MS45XX_AIRSPEED_SCALE 1.6327
161 #endif
162 
166 #ifndef MS45XX_LOWPASS_TAU
167 #define MS45XX_LOWPASS_TAU 0.15
168 #endif
169 
172 #ifdef USE_AIRSPEED_LOWPASS_FILTER
174 #endif
175 
176 static void ms45xx_downlink(struct transport_tx *trans, struct link_device *dev)
177 {
178  pprz_msg_send_AIRSPEED_MS45XX(trans,dev,AC_ID,
179  &ms45xx.pressure,
181  &ms45xx.airspeed);
182 }
183 
184 void ms45xx_i2c_init(void)
185 {
186  ms45xx.pressure = 0.;
187  ms45xx.temperature = 0;
188  ms45xx.airspeed = 0.;
194 
196  // setup low pass filter with time constant and 100Hz sampling freq
197 #ifdef USE_AIRSPEED_LOWPASS_FILTER
199  MS45XX_I2C_PERIODIC_PERIOD, 0);
200 #endif
201 
202 #if PERIODIC_TELEMETRY
203  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_MS45XX, ms45xx_downlink);
204 #endif
205 }
206 
208 {
209  // Initiate next read
212  }
213 }
214 
215 #define AUTOSET_NB_MAX 20
216 
218 {
219  static int autoset_nb = 0;
220  static float autoset_offset = 0.f;
221 
222  /* Check if transaction is succesfull */
224 
225  /* 2 MSB of data are status bits, 0 = good data, 2 = already fetched, 3 = fault */
226  uint8_t status = (0xC0 & ms45xx_trans.buf[0]) >> 6;
227 
228  if (status == 0) {
229  /* 14bit raw pressure */
230  uint16_t p_raw = 0x3FFF & (((uint16_t)(ms45xx_trans.buf[0]) << 8) | (uint16_t)(ms45xx_trans.buf[1]));
231 
232  /* For type Diff
233  * Output is proportional to the difference between Port 1 and Port 2. Output
234  * swings positive when Port 1> Port 2. Output is 50% of total counts
235  * when Port 1=Port 2.
236  * For type Gauge
237  * p_out = p_raw * scale - offset
238  */
239 
240  float p_out = (p_raw * ms45xx.pressure_scale) - ms45xx.pressure_offset;
242  ms45xx.pressure = update_butterworth_2_low_pass(&ms45xx_filter, p_out);
243 #else
244  ms45xx.pressure = p_out;
245 #endif
246 
247  if (ms45xx.autoset_offset) {
248  if (autoset_nb < AUTOSET_NB_MAX) {
249  autoset_offset += p_raw * ms45xx.pressure_scale;
250  autoset_nb++;
251  } else {
252  ms45xx.pressure_offset = autoset_offset / (float)autoset_nb;
253  autoset_offset = 0.f;
254  autoset_nb = 0;
255  ms45xx.autoset_offset = false;
256  }
257  }
258 
259  /* 11bit raw temperature, 5 LSB bits not used */
260  uint16_t temp_raw = 0xFFE0 & (((uint16_t)(ms45xx_trans.buf[2]) << 8) |
261  (uint16_t)(ms45xx_trans.buf[3]));
262  temp_raw = temp_raw >> 5;
263  /* 0 = -50degC, 20147 = 150degC
264  * ms45xx_temperature in 0.1 deg Celcius
265  */
266  ms45xx.temperature = ((uint32_t)temp_raw * 2000) / 2047 - 500;
267 
268  // Send (differential) pressure via ABI
269  AbiSendMsgBARO_DIFF(MS45XX_SENDER_ID, ms45xx.pressure);
270  // Send temperature as float in deg Celcius via ABI
271  float temp = ms45xx.temperature / 10.0f;
272  AbiSendMsgTEMPERATURE(MS45XX_SENDER_ID, temp);
273  // Compute airspeed
275 
276 #if USE_AIRSPEED_MS45XX
278 #endif
279  if (ms45xx.sync_send) {
280  ms45xx_downlink(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
281  }
282  }
283 
284  // Set to done
286  } else if (ms45xx_trans.status == I2CTransFailed) {
287  // Just retry if failed
289  }
290 }
unsigned short uint16_t
Definition: types.h:16
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
#define MS45XX_I2C_ADDR
Sensor I2C slave address (existing defaults 0x50, 0x6C and 0x8C)
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
transaction successfully finished by I2C driver
Definition: i2c.h:57
int16_t temperature
Temperature in 0.1 deg Celcius.
Periodic telemetry system header (includes downlink utility and generated code).
Simple first order low pass filter with bilinear transform.
bool pressure_type
Pressure type Differential of Gauge.
void ms45xx_i2c_init(void)
Main include for ABI (AirBorneInterface).
struct AirspeedMs45xx ms45xx
bool autoset_offset
Set offset value from current filtered value.
float pressure
(differential) pressure in Pascal
#define MS45XX_PRESSURE_OFFSET
bool sync_send
Flag to enable sending every new measurement via telemetry for debugging purpose. ...
#define MS45XX_PRESSURE_TYPE
Pressure Type 0 = Differential, 1 = Gauge note there are theoretical more types than 2...
void ms45xx_i2c_event(void)
float airspeed
Airspeed in m/s estimated from (differential) pressure.
transaction set to done by user level
Definition: i2c.h:59
#define MS45XX_AIRSPEED_SCALE
Quadratic scale factor for indicated airspeed.
static Butterworth2LowPass ms45xx_filter
uint8_t status
float pressure_scale
Scaling factor from raw measurement to Pascal.
unsigned long uint32_t
Definition: types.h:18
float airspeed_scale
Quadratic scale factor to convert (differential) pressure to airspeed.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#define MS45XX_SYNC_SEND
Send a AIRSPEED_MS45XX message with every new measurement.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
transaction failed
Definition: i2c.h:58
float pressure_offset
Offset in Pascal.
#define Max(x, y)
Definition: main_fbw.c:53
I2C transaction structure.
Definition: i2c.h:93
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define MS45XX_SENDER_ID
unsigned char uint8_t
Definition: types.h:14
#define MS45XX_OUTPUT_TYPE
MS45xx output Type.
API to get/set the generic vehicle states.
static struct i2c_transaction ms45xx_trans
Airspeed driver for the MS45xx pressure sensor via I2C.
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:318
#define USE_AIRSPEED_LOWPASS_FILTER
Use low pass filter on pressure values.
#define MS45XX_PRESSURE_RANGE
MS45xx pressure range in PSI or InH2O The sensor is available in many ranges, the datasheet of your p...
Second order low pass filter structure.
#define MS45XX_I2C_DEV
Default I2C device.
void ms45xx_i2c_periodic(void)
#define AUTOSET_NB_MAX
#define MS45XX_LOWPASS_TAU
Time constant for second order Butterworth low pass filter Default of 0.15 should give cut-off freq o...
#define MS45XX_PRESSURE_SCALE
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static void ms45xx_downlink(struct transport_tx *trans, struct link_device *dev)
Architecture independent I2C (Inter-Integrated Circuit Bus) API.