Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : backend driver for airspeed from a I2C SDP3X sensor
18 :
19 : with thanks to https://github.com/PX4/Firmware/blob/master/src/drivers/sdp3x_airspeed
20 : */
21 : #include "AP_Airspeed_SDP3X.h"
22 :
23 : #if AP_AIRSPEED_SDP3X_ENABLED
24 :
25 : #include <GCS_MAVLink/GCS.h>
26 : #include <AP_Baro/AP_Baro.h>
27 :
28 : #include <stdio.h>
29 :
30 : #define SDP3X_SCALE_TEMPERATURE 200.0f
31 :
32 : #define SDP3XD0_I2C_ADDR 0x21
33 : #define SDP3XD1_I2C_ADDR 0x22
34 : #define SDP3XD2_I2C_ADDR 0x23
35 :
36 : #define SDP3X_CONT_MEAS_AVG_MODE 0x3615
37 : #define SDP3X_CONT_MEAS_STOP 0x3FF9
38 :
39 : #define SDP3X_SCALE_PRESSURE_SDP31 60
40 : #define SDP3X_SCALE_PRESSURE_SDP32 240
41 : #define SDP3X_SCALE_PRESSURE_SDP33 20
42 :
43 : extern const AP_HAL::HAL &hal;
44 :
45 : /*
46 : send a 16 bit command code
47 : */
48 0 : bool AP_Airspeed_SDP3X::_send_command(uint16_t cmd)
49 : {
50 0 : uint8_t b[2] {uint8_t(cmd >> 8), uint8_t(cmd & 0xFF)};
51 0 : return _dev->transfer(b, 2, nullptr, 0);
52 : }
53 :
54 : // probe and initialise the sensor
55 0 : __INITFUNC__ bool AP_Airspeed_SDP3X::init()
56 : {
57 0 : const uint8_t addresses[3] = { SDP3XD0_I2C_ADDR,
58 : SDP3XD1_I2C_ADDR,
59 : SDP3XD2_I2C_ADDR
60 : };
61 0 : bool found = false;
62 0 : bool ret = false;
63 :
64 0 : for (uint8_t i=0; i<ARRAY_SIZE(addresses) && !found; i++) {
65 0 : _dev = hal.i2c_mgr->get_device_ptr(get_bus(), addresses[i]);
66 0 : if (!_dev) {
67 0 : continue;
68 : }
69 0 : _dev->get_semaphore()->take_blocking();
70 :
71 : // lots of retries during probe
72 0 : _dev->set_retries(10);
73 :
74 : // stop continuous average mode
75 0 : if (!_send_command(SDP3X_CONT_MEAS_STOP)) {
76 0 : _dev->get_semaphore()->give();
77 0 : continue;
78 : }
79 :
80 : // these delays are needed for reliable operation
81 0 : _dev->get_semaphore()->give();
82 0 : hal.scheduler->delay_microseconds(20000);
83 0 : _dev->get_semaphore()->take_blocking();
84 :
85 : // start continuous average mode
86 0 : if (!_send_command(SDP3X_CONT_MEAS_AVG_MODE)) {
87 0 : _dev->get_semaphore()->give();
88 0 : continue;
89 : }
90 :
91 : // these delays are needed for reliable operation
92 0 : _dev->get_semaphore()->give();
93 0 : hal.scheduler->delay_microseconds(20000);
94 0 : _dev->get_semaphore()->take_blocking();
95 :
96 : // step 3 - get scale
97 : uint8_t val[9];
98 0 : ret = _dev->transfer(nullptr, 0, &val[0], sizeof(val));
99 0 : if (!ret) {
100 0 : _dev->get_semaphore()->give();
101 0 : continue;
102 : }
103 :
104 : // Check the CRC
105 0 : if (!_crc(&val[0], 2, val[2]) || !_crc(&val[3], 2, val[5]) || !_crc(&val[6], 2, val[8])) {
106 0 : _dev->get_semaphore()->give();
107 0 : continue;
108 : }
109 :
110 0 : _scale = (((uint16_t)val[6]) << 8) | val[7];
111 :
112 0 : _dev->get_semaphore()->give();
113 :
114 0 : found = true;
115 :
116 : #if HAL_GCS_ENABLED
117 0 : char c = 'X';
118 0 : switch (_scale) {
119 0 : case SDP3X_SCALE_PRESSURE_SDP31:
120 0 : c = '1';
121 0 : break;
122 0 : case SDP3X_SCALE_PRESSURE_SDP32:
123 0 : c = '2';
124 0 : break;
125 0 : case SDP3X_SCALE_PRESSURE_SDP33:
126 0 : c = '3';
127 0 : break;
128 : }
129 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SDP3%c[%u]: Found bus %u addr 0x%02x scale=%u",
130 : get_instance(),
131 : c, get_bus(), addresses[i], _scale);
132 : #endif
133 : }
134 :
135 0 : if (!found) {
136 0 : return false;
137 : }
138 :
139 : // this sensor uses zero offset
140 0 : set_use_zero_offset();
141 :
142 0 : _dev->set_device_type(uint8_t(DevType::SDP3X));
143 0 : set_bus_id(_dev->get_bus_id());
144 :
145 : // drop to 2 retries for runtime
146 0 : _dev->set_retries(2);
147 :
148 0 : _dev->register_periodic_callback(20000,
149 : FUNCTOR_BIND_MEMBER(&AP_Airspeed_SDP3X::_timer, void));
150 0 : return true;
151 : }
152 :
153 : // read the values from the sensor. Called at 50Hz
154 0 : void AP_Airspeed_SDP3X::_timer()
155 : {
156 : // read 6 bytes from the sensor
157 : uint8_t val[6];
158 0 : int ret = _dev->transfer(nullptr, 0, &val[0], sizeof(val));
159 0 : uint32_t now = AP_HAL::millis();
160 0 : if (!ret) {
161 0 : if (now - _last_sample_time_ms > 200) {
162 : // try and re-connect
163 0 : _send_command(SDP3X_CONT_MEAS_AVG_MODE);
164 : }
165 0 : return;
166 : }
167 : // Check the CRC
168 0 : if (!_crc(&val[0], 2, val[2]) || !_crc(&val[3], 2, val[5])) {
169 0 : return;
170 : }
171 :
172 0 : int16_t P = (((int16_t)val[0]) << 8) | val[1];
173 0 : int16_t temp = (((int16_t)val[3]) << 8) | val[4];
174 :
175 0 : float diff_press_pa = float(P) / float(_scale);
176 0 : float temperature = float(temp) / SDP3X_SCALE_TEMPERATURE;
177 :
178 0 : WITH_SEMAPHORE(sem);
179 :
180 0 : _press_sum += diff_press_pa;
181 0 : _temp_sum += temperature;
182 0 : _press_count++;
183 0 : _temp_count++;
184 0 : _last_sample_time_ms = now;
185 0 : }
186 :
187 : /*
188 : correct pressure for barometric height
189 : With thanks to:
190 : https://github.com/PX4/Firmware/blob/master/Tools/models/sdp3x_pitot_model.py
191 : */
192 0 : float AP_Airspeed_SDP3X::_correct_pressure(float press)
193 : {
194 0 : float sign = 1.0f;
195 :
196 : // fix for tube order
197 0 : AP_Airspeed::pitot_tube_order tube_order = get_tube_order();
198 0 : switch (tube_order) {
199 0 : case AP_Airspeed::PITOT_TUBE_ORDER_NEGATIVE:
200 0 : press = -press;
201 0 : sign = -1.0f;
202 0 : break;
203 0 : case AP_Airspeed::PITOT_TUBE_ORDER_POSITIVE:
204 0 : break;
205 0 : case AP_Airspeed::PITOT_TUBE_ORDER_AUTO:
206 : default:
207 0 : if (press < 0.0f) {
208 0 : sign = -1.0f;
209 0 : press = -press;
210 : }
211 0 : break;
212 : }
213 :
214 0 : if (press <= 0.0f) {
215 0 : return 0.0f;
216 : }
217 :
218 0 : AP_Baro *baro = AP_Baro::get_singleton();
219 :
220 : float baro_pressure;
221 0 : if (baro == nullptr || baro->num_instances() == 0) {
222 : // with no baro assume sea level
223 0 : baro_pressure = SSL_AIR_PRESSURE;
224 : } else {
225 0 : baro_pressure = baro->get_pressure();
226 : }
227 :
228 : float temperature;
229 0 : if (!get_temperature(temperature)) {
230 : // assume 25C if no temperature
231 0 : temperature = 25;
232 : }
233 :
234 0 : float rho_air = baro_pressure / (ISA_GAS_CONSTANT * C_TO_KELVIN(temperature));
235 0 : if (!is_positive(rho_air)) {
236 : // bad pressure
237 0 : return press;
238 : }
239 :
240 : /*
241 : the constants in the code below come from a calibrated test of
242 : the drotek pitot tube by Sensiron. They are specific to the droktek pitot tube
243 :
244 : At 25m/s, the rough proportions of each pressure correction are:
245 :
246 : - dp_pitot: 5%
247 : - press_correction: 14%
248 : - press: 81%
249 :
250 : dp_tube has been removed from the Sensiron model as it is
251 : insignificant (less than 0.02% over the supported speed ranges)
252 : */
253 :
254 : // flow through sensor
255 0 : float flow_SDP3X = (300.805f - 300.878f / (0.00344205f * (float)powf(press, 0.68698f) + 1.0f)) * 1.29f / rho_air;
256 0 : if (flow_SDP3X < 0.0f) {
257 0 : flow_SDP3X = 0.0f;
258 : }
259 :
260 : // differential pressure through pitot tube
261 0 : float dp_pitot = 28557670.0f * (1.0f - 1.0f / (1.0f + (float)powf((flow_SDP3X / 5027611.0f), 1.227924f)));
262 :
263 : // uncorrected pressure
264 0 : float press_uncorrected = (press + dp_pitot) / SSL_AIR_DENSITY;
265 :
266 : // correction for speed at pitot-tube tip due to flow through sensor
267 0 : float dv = 0.0331582f * flow_SDP3X;
268 :
269 : // airspeed ratio
270 0 : float ratio = get_airspeed_ratio();
271 0 : if (!is_positive(ratio)) {
272 : // cope with AP_Periph where ratio is 0
273 0 : ratio = 2.0;
274 : }
275 :
276 : // calculate equivalent pressure correction. This formula comes
277 : // from turning the dv correction above into an equivalent
278 : // pressure correction. We need to do this so the airspeed ratio
279 : // calibrator can work, as it operates on pressure values
280 0 : float press_correction = sq(sqrtf(press_uncorrected*ratio)+dv)/ratio - press_uncorrected;
281 :
282 0 : return (press_uncorrected + press_correction) * sign;
283 : }
284 :
285 : // return the current differential_pressure in Pascal
286 0 : bool AP_Airspeed_SDP3X::get_differential_pressure(float &pressure)
287 : {
288 0 : WITH_SEMAPHORE(sem);
289 :
290 0 : if (AP_HAL::millis() - _last_sample_time_ms > 100) {
291 0 : return false;
292 : }
293 :
294 0 : if (_press_count > 0) {
295 0 : _press = _press_sum / _press_count;
296 0 : _press_count = 0;
297 0 : _press_sum = 0;
298 : }
299 :
300 0 : pressure = _correct_pressure(_press);
301 0 : return true;
302 0 : }
303 :
304 : // return the current temperature in degrees C, if available
305 0 : bool AP_Airspeed_SDP3X::get_temperature(float &temperature)
306 : {
307 0 : WITH_SEMAPHORE(sem);
308 :
309 0 : if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
310 0 : return false;
311 : }
312 :
313 0 : if (_temp_count > 0) {
314 0 : _temp = _temp_sum / _temp_count;
315 0 : _temp_count = 0;
316 0 : _temp_sum = 0;
317 : }
318 :
319 0 : temperature = _temp;
320 0 : return true;
321 0 : }
322 :
323 : /*
324 : check CRC for a set of bytes
325 : */
326 0 : bool AP_Airspeed_SDP3X::_crc(const uint8_t data[], uint8_t size, uint8_t checksum)
327 : {
328 0 : uint8_t crc_value = 0xff;
329 :
330 : // calculate 8-bit checksum with polynomial 0x31 (x^8 + x^5 + x^4 + 1)
331 0 : for (uint8_t i = 0; i < size; i++) {
332 0 : crc_value ^= data[i];
333 0 : for (uint8_t bit = 8; bit > 0; --bit) {
334 0 : if (crc_value & 0x80) {
335 0 : crc_value = (crc_value << 1) ^ 0x31;
336 : } else {
337 0 : crc_value = (crc_value << 1);
338 : }
339 : }
340 : }
341 : // verify checksum
342 0 : return (crc_value == checksum);
343 : }
344 :
345 : #endif // AP_AIRSPEED_SDP3X_ENABLED
|
