PX4 Firmware
PX4 Autopilot Software http://px4.io
SDP3X.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file SDP3X.hpp
36  *
37  * Driver for Sensirion SDP3X Differential Pressure Sensor
38  *
39  */
40 
41 #include "SDP3X.hpp"
42 
43 int
45 {
46  return !init_sdp3x();
47 }
48 
50 {
51  uint8_t cmd[2];
52  cmd[0] = static_cast<uint8_t>(command >> 8);
53  cmd[1] = static_cast<uint8_t>(command & 0xff);
54  return transfer(&cmd[0], 2, nullptr, 0);
55 }
56 
57 bool
59 {
60  // step 1 - reset on broadcast
61  uint16_t prev_addr = get_device_address();
62  set_device_address(SDP3X_RESET_ADDR);
63  uint8_t reset_cmd = SDP3X_RESET_CMD;
64  int ret = transfer(&reset_cmd, 1, nullptr, 0);
65  set_device_address(prev_addr);
66 
67  if (ret != PX4_OK) {
69  return false;
70  }
71 
72  // wait until sensor is ready
73  px4_usleep(20000);
74 
75  // step 2 - configure
77 
78  if (ret != PX4_OK) {
80  PX4_ERR("config failed");
81  return false;
82  }
83 
84  px4_usleep(10000);
85 
86  // step 3 - get scale
87  uint8_t val[9];
88  ret = transfer(nullptr, 0, &val[0], sizeof(val));
89 
90  if (ret != PX4_OK) {
92  PX4_ERR("get scale failed");
93  return false;
94  }
95 
96  // Check the CRC
97  if (!crc(&val[0], 2, val[2]) || !crc(&val[3], 2, val[5]) || !crc(&val[6], 2, val[8])) {
99  return false;
100  }
101 
102  _scale = (((uint16_t)val[6]) << 8) | val[7];
103 
104  switch (_scale) {
106  _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP31;
107  break;
108 
110  _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP32;
111  break;
112 
114  _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP33;
115  break;
116  }
117 
118  return true;
119 }
120 
121 int
123 {
125 
126  // read 9 bytes from the sensor
127  uint8_t val[6];
128  int ret = transfer(nullptr, 0, &val[0], sizeof(val));
129 
130  if (ret != PX4_OK) {
132  return ret;
133  }
134 
135  // Check the CRC
136  if (!crc(&val[0], 2, val[2]) || !crc(&val[3], 2, val[5])) {
138  return EAGAIN;
139 
140  } else {
141  ret = 0;
142  }
143 
144  int16_t P = (((int16_t)val[0]) << 8) | val[1];
145  int16_t temp = (((int16_t)val[3]) << 8) | val[4];
146 
147  float diff_press_pa_raw = static_cast<float>(P) / static_cast<float>(_scale);
148  float temperature_c = temp / static_cast<float>(SDP3X_SCALE_TEMPERATURE);
149 
151 
152  report.timestamp = hrt_absolute_time();
154  report.temperature = temperature_c;
156  report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
157  report.device_id = _device_id.devid;
158 
159  if (_airspeed_pub != nullptr && !(_pub_blocked)) {
160  orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
161  }
162 
163  ret = OK;
164 
166 
167  return ret;
168 }
169 
170 void
172 {
173  int ret = PX4_ERROR;
174 
175  // measurement phase
176  ret = collect();
177 
178  if (PX4_OK != ret) {
179  _sensor_ok = false;
180  DEVICE_DEBUG("measure error");
181  }
182 
183  // schedule a fresh cycle call when the measurement is done
184  ScheduleDelayed(CONVERSION_INTERVAL);
185 }
186 
187 bool SDP3X::crc(const uint8_t data[], unsigned size, uint8_t checksum)
188 {
189  uint8_t crc_value = 0xff;
190 
191  // calculate 8-bit checksum with polynomial 0x31 (x^8 + x^5 + x^4 + 1)
192  for (unsigned i = 0; i < size; i++) {
193  crc_value ^= (data[i]);
194 
195  for (int bit = 8; bit > 0; --bit) {
196  if (crc_value & 0x80) {
197  crc_value = (crc_value << 1) ^ 0x31;
198 
199  } else {
200  crc_value = (crc_value << 1);
201  }
202  }
203  }
204 
205  // verify checksum
206  return (crc_value == checksum);
207 }
orb_advert_t _airspeed_pub
Definition: airspeed.h:81
#define SDP3X_RESET_ADDR
Definition: SDP3X.hpp:55
#define SDP3X_SCALE_PRESSURE_SDP32
Definition: SDP3X.hpp:60
bool init_sdp3x()
Definition: SDP3X.cpp:58
#define CONVERSION_INTERVAL
#define DRV_DIFF_PRESS_DEVTYPE_SDP33
Definition: drv_sensor.h:101
#define SDP3X_CONT_MEAS_AVG_MODE
Definition: SDP3X.hpp:57
#define DRV_DIFF_PRESS_DEVTYPE_SDP31
Definition: drv_sensor.h:99
#define SDP3X_SCALE_TEMPERATURE
Definition: SDP3X.hpp:54
int write_command(uint16_t command)
Write a command in Sensirion specific logic.
Definition: SDP3X.cpp:49
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void perf_count(perf_counter_t handle)
Count a performance event.
bool crc(const uint8_t data[], unsigned size, uint8_t checksum)
Calculate the CRC8 for the sensor payload data.
Definition: SDP3X.cpp:187
uint8_t * data
Definition: dataman.cpp:149
perf_counter_t _sample_perf
Definition: airspeed.h:88
uint16_t _scale
Definition: SDP3X.hpp:103
int probe() override
Definition: SDP3X.cpp:44
Driver for Sensirion SDP3X Differential Pressure Sensor.
void perf_end(perf_counter_t handle)
End a performance event.
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
int collect() override
Definition: SDP3X.cpp:122
float apply(float sample)
Add a new raw value to the filter.
#define SDP3X_RESET_CMD
Definition: SDP3X.hpp:56
float _diff_pres_offset
Definition: airspeed.h:79
#define SDP3X_SCALE_PRESSURE_SDP31
Definition: SDP3X.hpp:59
uint64_t perf_event_count(perf_counter_t handle)
Return current event_count.
math::LowPassFilter2p _filter
Definition: SDP3X.hpp:89
#define OK
Definition: uavcan_main.cpp:71
P[0][0]
Definition: quatCovMat.c:44
bool _sensor_ok
Definition: airspeed.h:76
void perf_begin(perf_counter_t handle)
Begin a performance event.
#define SDP3X_SCALE_PRESSURE_SDP33
Definition: SDP3X.hpp:61
#define DEVICE_DEBUG(FMT,...)
Definition: Device.hpp:52
perf_counter_t _comms_errors
Definition: airspeed.h:89
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
Definition: SDP3X.cpp:171
#define DRV_DIFF_PRESS_DEVTYPE_SDP32
Definition: drv_sensor.h:100