Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/core/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 
92 
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
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
248  sdp3x.airspeed = sqrtf(Max(sdp3x.pressure * sdp3x.airspeed_scale, 0));
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 }
SDP3X_AIRSPEED_SCALE
#define SDP3X_AIRSPEED_SCALE
Quadratic scale factor for indicated airspeed.
Definition: airspeed_sdp3x.c:105
AirspeedSdp3x::pressure_offset
float pressure_offset
Offset in Pascal.
Definition: airspeed_sdp3x.h:36
AirspeedSdp3x::airspeed_scale
float airspeed_scale
Quadratic scale factor to convert (differential) pressure to airspeed.
Definition: airspeed_sdp3x.h:34
update_butterworth_2_low_pass
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
Definition: low_pass_filter.h:291
sdp3x_init
void sdp3x_init(void)
Definition: airspeed_sdp3x.c:153
i2c_transaction::buf
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
SDP3X_PRESSURE_SCALE
#define SDP3X_PRESSURE_SCALE
Default scale for SDP31.
Definition: airspeed_sdp3x.c:80
SDP3X_SCALE_TEMPERATURE
#define SDP3X_SCALE_TEMPERATURE
Commands and scales.
Definition: airspeed_sdp3x.c:54
AUTOSET_NB_MAX
#define AUTOSET_NB_MAX
Definition: airspeed_sdp3x.c:196
AirspeedSdp3x::sync_send
bool sync_send
Flag to enable sending every new measurement via telemetry for debugging purpose.
Definition: airspeed_sdp3x.h:38
AirspeedSdp3x::initialized
bool initialized
init flag
Definition: airspeed_sdp3x.h:39
SDP3X_MODE
#define SDP3X_MODE
Default operation mode.
Definition: airspeed_sdp3x.c:74
abi.h
AirspeedSdp3x::pressure_scale
float pressure_scale
Scaling factor from raw measurement to Pascal.
Definition: airspeed_sdp3x.h:35
sdp3x_downlink
static void sdp3x_downlink(struct transport_tx *trans, struct link_device *dev)
Definition: airspeed_sdp3x.c:144
sdp3x_event
void sdp3x_event(void)
Definition: airspeed_sdp3x.c:198
I2CTransFailed
@ I2CTransFailed
transaction failed
Definition: i2c.h:58
SecondOrderLowPass
Second order low pass filter structure.
Definition: low_pass_filter.h:136
SDP3X_SENDER_ID
#define SDP3X_SENDER_ID
Definition: abi_sender_ids.h:92
SDP3X_LOWPASS_TAU
#define SDP3X_LOWPASS_TAU
Time constant for second order Butterworth low pass filter Default of 0.15 should give cut-off freq o...
Definition: airspeed_sdp3x.c:112
AirspeedSdp3x::airspeed
float airspeed
Airspeed in m/s estimated from (differential) pressure.
Definition: airspeed_sdp3x.h:33
I2CTransSuccess
@ I2CTransSuccess
transaction successfully finished by I2C driver
Definition: i2c.h:57
telemetry.h
AirspeedSdp3x::pressure
float pressure
(differential) pressure in Pascal
Definition: airspeed_sdp3x.h:31
int16_t
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
std.h
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
sdp3x_filter
static Butterworth2LowPass sdp3x_filter
Definition: airspeed_sdp3x.c:119
AirspeedSdp3x::autoset_offset
bool autoset_offset
Set offset value from current filtered value.
Definition: airspeed_sdp3x.h:37
i2c_transmit
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
AirspeedSdp3x
Definition: airspeed_sdp3x.h:30
sdp3x
struct AirspeedSdp3x sdp3x
Definition: airspeed_sdp3x.c:115
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
i2c_transaction::status
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
i2c_transaction
I2C transaction structure.
Definition: i2c.h:93
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
sdp3x_crc
static bool sdp3x_crc(const uint8_t data[], unsigned size, uint8_t checksum)
Definition: airspeed_sdp3x.c:122
airspeed_sdp3x.h
AirspeedSdp3x::temperature
float temperature
Temperature in deg Celcius.
Definition: airspeed_sdp3x.h:32
SDP3X_I2C_ADDR
#define SDP3X_I2C_ADDR
Sensor I2C slave address (existing defaults 0x42, 0x44 and 0x46)
Definition: airspeed_sdp3x.c:68
sdp3x_trans
static struct i2c_transaction sdp3x_trans
Definition: airspeed_sdp3x.c:116
I2CTransDone
@ I2CTransDone
transaction set to done by user level
Definition: i2c.h:59
init_butterworth_2_low_pass
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
Definition: low_pass_filter.h:280
SDP3X_PRESSURE_OFFSET
#define SDP3X_PRESSURE_OFFSET
Definition: airspeed_sdp3x.c:86
sdp3x_periodic
void sdp3x_periodic(void)
Definition: airspeed_sdp3x.c:177
SDP3X_SYNC_SEND
#define SDP3X_SYNC_SEND
Send a AIRSPEED_MS45XX message with every new measurement.
Definition: airspeed_sdp3x.c:96
AIRSPEED_SDP3X_ID
#define AIRSPEED_SDP3X_ID
Definition: abi_sender_ids.h:107
checksum
static uint8_t checksum
Definition: airspeed_uADC.c:61
i2c.h
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
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
low_pass_filter.h
Simple first order low pass filter with bilinear transform.