Paparazzi UAS  v6.0_unstable-39-g758f8ef-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
airspeed_sdp3x.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
25 #include "std.h"
26 #include "mcu_periph/i2c.h"
29 #include "subsystems/abi.h"
30 
31 #include "mcu_periph/uart.h"
32 #include "pprzlink/messages.h"
34 
35 #if PERIODIC_TELEMETRY
37 #endif
38 
39 #ifndef USE_AIRSPEED_SDP3X
40 #if USE_AIRSPEED
41 #define USE_AIRSPEED_SDP3X TRUE
42 PRINT_CONFIG_MSG("USE_AIRSPEED_SDP3X set to TRUE since this is set USE_AIRSPEED")
43 #endif
44 #endif
45 
48 #ifndef USE_AIRSPEED_LOWPASS_FILTER
49 #define USE_AIRSPEED_LOWPASS_FILTER TRUE
50 #endif
51 
54 #define SDP3X_SCALE_TEMPERATURE 200.0f
55 #define SDP3X_RESET_ADDR 0x00
56 #define SDP3X_RESET_CMD 0x06
57 
58 #define SDP3X_CONT_MEAS_AVG_MODE 0x3615
59 #define SDP3X_CONT_NONE_MODE 0x361E
60 
61 #define SDP3X_SCALE_PRESSURE_SDP31 60
62 #define SDP3X_SCALE_PRESSURE_SDP32 240
63 #define SDP3X_SCALE_PRESSURE_SDP33 20
64 
67 #ifndef SDP3X_I2C_ADDR
68 #define SDP3X_I2C_ADDR 0x42
69 #endif
70 
73 #ifndef SDP3X_MODE
74 #define SDP3X_MODE SDP3X_CONT_MEAS_AVG_MODE
75 #endif
76 
79 #ifndef SDP3X_PRESSURE_SCALE
80 #define SDP3X_PRESSURE_SCALE SDP3X_SCALE_PRESSURE_SDP31
81 #endif
82 
83 /* Default offset
84  */
85 #ifndef SDP3X_PRESSURE_OFFSET
86 #define SDP3X_PRESSURE_OFFSET 0.f
87 #endif
88 
89 PRINT_CONFIG_VAR(SDP3X_PRESSURE_SCALE)
90 PRINT_CONFIG_VAR(SDP3X_PRESSURE_OFFSET)
91 
95 #ifndef SDP3X_SYNC_SEND
96 #define SDP3X_SYNC_SEND FALSE
97 #endif
98 
104 #ifndef SDP3X_AIRSPEED_SCALE
105 #define SDP3X_AIRSPEED_SCALE 1.6327
106 #endif
107 
111 #ifndef SDP3X_LOWPASS_TAU
112 #define SDP3X_LOWPASS_TAU 0.15
113 #endif
114 
117 
118 #ifdef USE_AIRSPEED_LOWPASS_FILTER
120 #endif
121 
122 static bool sdp3x_crc(const uint8_t data[], unsigned size, uint8_t checksum)
123 {
124  uint8_t crc_value = 0xff;
125 
126  // calculate 8-bit checksum with polynomial 0x31 (x^8 + x^5 + x^4 + 1)
127  for (unsigned i = 0; i < size; i++) {
128  crc_value ^= (data[i]);
129 
130  for (int bit = 8; bit > 0; --bit) {
131  if (crc_value & 0x80) {
132  crc_value = (crc_value << 1) ^ 0x31;
133 
134  } else {
135  crc_value = (crc_value << 1);
136  }
137  }
138  }
139 
140  // verify checksum
141  return (crc_value == checksum);
142 }
143 
144 static void sdp3x_downlink(struct transport_tx *trans, struct link_device *dev)
145 {
146  int16_t temp = (int16_t)(sdp3x.temperature * 10.f);
147  pprz_msg_send_AIRSPEED_MS45XX(trans,dev,AC_ID,
148  &sdp3x.pressure,
149  &temp,
150  &sdp3x.airspeed);
151 }
152 
153 void sdp3x_init(void)
154 {
155  sdp3x.pressure = 0.f;
156  sdp3x.temperature = 0.f;
157  sdp3x.airspeed = 0.f;
161  sdp3x.autoset_offset = false;
163  sdp3x.initialized = false;
164 
166  // setup low pass filter with time constant and 100Hz sampling freq
167 #ifdef USE_AIRSPEED_LOWPASS_FILTER
169  SDP3X_PERIODIC_PERIOD, 0);
170 #endif
171 
172 #if PERIODIC_TELEMETRY
173  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_MS45XX, sdp3x_downlink); // FIXME
174 #endif
175 }
176 
177 void sdp3x_periodic(void)
178 {
180  return; // not ready
181  }
182 
183  if (sdp3x.initialized) {
184  // Initiate next read
185  i2c_receive(&SDP3X_I2C_DEV, &sdp3x_trans, SDP3X_I2C_ADDR, 6);
186  }
187  else {
188  // Init sensor in continuous mode
189  sdp3x_trans.buf[0] = SDP3X_MODE >> 8;
190  sdp3x_trans.buf[1] = SDP3X_MODE & 0xFF;
191  i2c_transmit(&SDP3X_I2C_DEV, &sdp3x_trans, SDP3X_I2C_ADDR, 2);
192  sdp3x.initialized = true;
193  }
194 }
195 
196 #define AUTOSET_NB_MAX 20
197 
198 void sdp3x_event(void)
199 {
200  /* Check if transaction is succesfull */
202 
203  if (sdp3x.initialized) {
204  static int autoset_nb = 0;
205  static float autoset_offset = 0.f;
206  uint8_t buf[6];
207  for (uint8_t i = 0; i < 6; i++) {
208  buf[i] = sdp3x_trans.buf[i];
209  }
210 
211  // Check the CRC
212  if (!sdp3x_crc(&buf[0], 2, buf[2]) || !sdp3x_crc(&buf[3], 2, buf[5])) {
213  // error
215  return;
216  }
217 
218  int16_t p_raw = ((int16_t)(buf[0]) << 8) | (int16_t)(buf[1]);
219 
220  float p_out = ((float)p_raw / sdp3x.pressure_scale) - sdp3x.pressure_offset;
221 
222 #ifdef USE_AIRSPEED_LOWPASS_FILTER
223  sdp3x.pressure = update_butterworth_2_low_pass(&sdp3x_filter, p_out);
224 #else
225  sdp3x.pressure = p_out;
226 #endif
227 
228  if (sdp3x.autoset_offset) {
229  if (autoset_nb < AUTOSET_NB_MAX) {
230  autoset_offset += p_raw * sdp3x.pressure_scale;
231  autoset_nb++;
232  } else {
233  sdp3x.pressure_offset = autoset_offset / (float)autoset_nb;
234  autoset_offset = 0.f;
235  autoset_nb = 0;
236  sdp3x.autoset_offset = false;
237  }
238  }
239 
240  int16_t t_raw = ((int16_t)(buf[3]) << 8) | (int16_t)(buf[4]);
241  sdp3x.temperature = (float)t_raw / SDP3X_SCALE_TEMPERATURE;
242 
243  // Send (differential) pressure via ABI
244  AbiSendMsgBARO_DIFF(SDP3X_SENDER_ID, sdp3x.pressure);
245  // Send temperature as float in deg Celcius via ABI
246  AbiSendMsgTEMPERATURE(SDP3X_SENDER_ID, sdp3x.temperature);
247  // Compute airspeed
249 
250 #if USE_AIRSPEED_SDP3X
251  AbiSendMsgAIRSPEED(AIRSPEED_SDP3X_ID, sdp3x.airspeed);
252 #endif
253  if (sdp3x.sync_send) {
254  sdp3x_downlink(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
255  }
256  }
257 
258  // Set to done
260  } else if (sdp3x_trans.status == I2CTransFailed) {
261  // Just retry if failed
263  }
264 }
#define SDP3X_LOWPASS_TAU
Time constant for second order Butterworth low pass filter Default of 0.15 should give cut-off freq o...
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
#define SDP3X_PRESSURE_SCALE
Default scale for SDP31.
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
bool initialized
init flag
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
#define SDP3X_AIRSPEED_SCALE
Quadratic scale factor for indicated airspeed.
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
static Butterworth2LowPass sdp3x_filter
transaction successfully finished by I2C driver
Definition: i2c.h:57
Periodic telemetry system header (includes downlink utility and generated code).
bool sync_send
Flag to enable sending every new measurement via telemetry for debugging purpose. ...
Simple first order low pass filter with bilinear transform.
Main include for ABI (AirBorneInterface).
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
Airspeed driver for the SDP3X pressure sensor via I2C.
float temperature
Temperature in deg Celcius.
float airspeed
Airspeed in m/s estimated from (differential) pressure.
void sdp3x_event(void)
static void sdp3x_downlink(struct transport_tx *trans, struct link_device *dev)
static uint8_t checksum
Definition: airspeed_uADC.c:61
transaction set to done by user level
Definition: i2c.h:59
#define AUTOSET_NB_MAX
float pressure_offset
Offset in Pascal.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
float pressure
(differential) pressure in Pascal
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
struct AirspeedSdp3x sdp3x
#define SDP3X_MODE
Default operation mode.
void sdp3x_periodic(void)
transaction failed
Definition: i2c.h:58
#define Max(x, y)
Definition: main_fbw.c:53
I2C transaction structure.
Definition: i2c.h:93
#define SDP3X_SYNC_SEND
Send a AIRSPEED_MS45XX message with every new measurement.
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
#define SDP3X_PRESSURE_OFFSET
bool i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Submit a write only transaction.
Definition: i2c.c:324
static bool sdp3x_crc(const uint8_t data[], unsigned size, uint8_t checksum)
void sdp3x_init(void)
float airspeed_scale
Quadratic scale factor to convert (differential) pressure to airspeed.
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
#define AIRSPEED_SDP3X_ID
#define SDP3X_SENDER_ID
static struct i2c_transaction sdp3x_trans
float pressure_scale
Scaling factor from raw measurement to Pascal.
#define SDP3X_SCALE_TEMPERATURE
Commands and scales.
Second order low pass filter structure.
#define SDP3X_I2C_ADDR
Sensor I2C slave address (existing defaults 0x42, 0x44 and 0x46)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
bool autoset_offset
Set offset value from current filtered value.
Architecture independent I2C (Inter-Integrated Circuit Bus) API.