Paparazzi UAS  v5.15_devel-81-gd13dafb
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"
28 #include "subsystems/abi.h"
29 
30 #include "mcu_periph/uart.h"
31 #include "pprzlink/messages.h"
33 
34 #if PERIODIC_TELEMETRY
36 #endif
37 
40 #define SDP3X_SCALE_TEMPERATURE 200.0f
41 #define SDP3X_RESET_ADDR 0x00
42 #define SDP3X_RESET_CMD 0x06
43 
44 #define SDP3X_CONT_MEAS_AVG_MODE 0x3615
45 #define SDP3X_CONT_NONE_MODE 0x361E
46 
47 #define SDP3X_SCALE_PRESSURE_SDP31 60
48 #define SDP3X_SCALE_PRESSURE_SDP32 240
49 #define SDP3X_SCALE_PRESSURE_SDP33 20
50 
53 #ifndef SDP3X_I2C_ADDR
54 #define SDP3X_I2C_ADDR 0x42
55 #endif
56 
59 #ifndef SDP3X_MODE
60 #define SDP3X_MODE SDP3X_CONT_MEAS_AVG_MODE
61 #endif
62 
65 #ifndef SDP3X_PRESSURE_SCALE
66 #define SDP3X_PRESSURE_SCALE SDP3X_SCALE_PRESSURE_SDP31
67 #endif
68 
69 /* Default offset
70  */
71 #ifndef SDP3X_PRESSURE_OFFSET
72 #define SDP3X_PRESSURE_OFFSET 0.f
73 #endif
74 
75 PRINT_CONFIG_VAR(SDP3X_PRESSURE_SCALE)
76 PRINT_CONFIG_VAR(SDP3X_PRESSURE_OFFSET)
77 
81 #ifndef SDP3X_SYNC_SEND
82 #define SDP3X_SYNC_SEND FALSE
83 #endif
84 
90 #ifndef SDP3X_AIRSPEED_SCALE
91 #define SDP3X_AIRSPEED_SCALE 1.6327
92 #endif
93 
96 
97 static bool sdp3x_crc(const uint8_t data[], unsigned size, uint8_t checksum)
98 {
99  uint8_t crc_value = 0xff;
100 
101  // calculate 8-bit checksum with polynomial 0x31 (x^8 + x^5 + x^4 + 1)
102  for (unsigned i = 0; i < size; i++) {
103  crc_value ^= (data[i]);
104 
105  for (int bit = 8; bit > 0; --bit) {
106  if (crc_value & 0x80) {
107  crc_value = (crc_value << 1) ^ 0x31;
108 
109  } else {
110  crc_value = (crc_value << 1);
111  }
112  }
113  }
114 
115  // verify checksum
116  return (crc_value == checksum);
117 }
118 
119 static void sdp3x_downlink(struct transport_tx *trans, struct link_device *dev)
120 {
121  int16_t temp = (int16_t)(sdp3x.temperature * 10.f);
122  pprz_msg_send_AIRSPEED_MS45XX(trans,dev,AC_ID,
123  &sdp3x.pressure,
124  &temp,
125  &sdp3x.airspeed);
126 }
127 
128 void sdp3x_init(void)
129 {
130  sdp3x.pressure = 0.;
131  sdp3x.temperature = 0;
132  sdp3x.airspeed = 0.;
137  sdp3x.initialized = false;
138 
140  // setup low pass filter with time constant and 100Hz sampling freq
141 
142 #if PERIODIC_TELEMETRY
143  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_MS45XX, sdp3x_downlink); // FIXME
144 #endif
145 }
146 
147 void sdp3x_periodic(void)
148 {
150  return; // not ready
151  }
152 
153  if (sdp3x.initialized) {
154  // Initiate next read
155  i2c_receive(&SDP3X_I2C_DEV, &sdp3x_trans, SDP3X_I2C_ADDR, 6);
156  }
157  else {
158  // Init sensor in continuous mode
159  sdp3x_trans.buf[0] = SDP3X_MODE >> 8;
160  sdp3x_trans.buf[1] = SDP3X_MODE & 0xFF;
161  i2c_transmit(&SDP3X_I2C_DEV, &sdp3x_trans, SDP3X_I2C_ADDR, 2);
162  sdp3x.initialized = true;
163  }
164 }
165 
166 void sdp3x_event(void)
167 {
168  /* Check if transaction is succesfull */
170 
171  if (sdp3x.initialized) {
172  // make local copy of buffer
173  uint8_t buf[6];
174  for (uint8_t i = 0; i < 6; i++) {
175  buf[i] = sdp3x_trans.buf[i];
176  }
177 
178  // Check the CRC
179  if (!sdp3x_crc(&buf[0], 2, buf[2]) || !sdp3x_crc(&buf[3], 2, buf[5])) {
180  // error
182  return;
183  }
184 
185  int16_t p_raw = ((int16_t)(buf[0]) << 8) | (int16_t)(buf[1]);
187 
188  int16_t t_raw = ((int16_t)(buf[3]) << 8) | (int16_t)(buf[4]);
189  sdp3x.temperature = (float)t_raw / SDP3X_SCALE_TEMPERATURE;
190 
191  // Send (differential) pressure via ABI
192  AbiSendMsgBARO_DIFF(SDP3X_SENDER_ID, sdp3x.pressure);
193  // Send temperature as float in deg Celcius via ABI
194  AbiSendMsgTEMPERATURE(SDP3X_SENDER_ID, sdp3x.temperature);
195  // Compute airspeed
197 
198  if (sdp3x.sync_send) {
199  sdp3x_downlink(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
200  }
201  }
202 
203  // Set to done
205  } else if (sdp3x_trans.status == I2CTransFailed) {
206  // Just retry if failed
208  }
209 }
#define SDP3X_PRESSURE_SCALE
Default scale for SDP31.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
bool initialized
init flag
#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
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. ...
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
float pressure_offset
Offset in Pascal.
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 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.
#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
Architecture independent I2C (Inter-Integrated Circuit Bus) API.