Paparazzi UAS  v5.15_devel-112-g521f3cf
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...
#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).
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:60
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")
signed short int16_t
Definition: types.h:17
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:308
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
static bool sdp3x_crc(const uint8_t data[], unsigned size, uint8_t checksum)
void sdp3x_init(void)
unsigned char uint8_t
Definition: types.h:14
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:318
#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
bool autoset_offset
Set offset value from current filtered value.
Architecture independent I2C (Inter-Integrated Circuit Bus) API.