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 : * AP_Airspeed.cpp - airspeed (pitot) driver
17 : */
18 :
19 : #include "AP_Airspeed_config.h"
20 :
21 : #if AP_AIRSPEED_ENABLED
22 :
23 : #include "AP_Airspeed.h"
24 :
25 : #include <AP_Vehicle/AP_Vehicle_Type.h>
26 :
27 : // Dummy the AP_Airspeed class to allow building Airspeed only for plane, rover, sub, and copter & heli 2MB boards
28 : // This could be removed once the build system allows for APM_BUILD_TYPE in header files
29 : // Note that this is also defined in AP_Airspeed_Params.cpp
30 : #ifndef AP_AIRSPEED_DUMMY_METHODS_ENABLED
31 : #define AP_AIRSPEED_DUMMY_METHODS_ENABLED ((APM_BUILD_COPTER_OR_HELI && HAL_PROGRAM_SIZE_LIMIT_KB <= 1024) || \
32 : APM_BUILD_TYPE(APM_BUILD_AntennaTracker) || APM_BUILD_TYPE(APM_BUILD_Blimp))
33 : #endif
34 :
35 : #if !AP_AIRSPEED_DUMMY_METHODS_ENABLED
36 :
37 : #include <AP_Common/AP_Common.h>
38 : #include <AP_HAL/AP_HAL.h>
39 : #include <AP_HAL/I2CDevice.h>
40 : #include <AP_Math/AP_Math.h>
41 : #include <GCS_MAVLink/GCS.h>
42 : #include <SRV_Channel/SRV_Channel.h>
43 : #include <AP_Logger/AP_Logger.h>
44 : #include <utility>
45 : #include "AP_Airspeed_MS4525.h"
46 : #include "AP_Airspeed_MS5525.h"
47 : #include "AP_Airspeed_SDP3X.h"
48 : #include "AP_Airspeed_DLVR.h"
49 : #include "AP_Airspeed_analog.h"
50 : #include "AP_Airspeed_ASP5033.h"
51 : #include "AP_Airspeed_Backend.h"
52 : #include "AP_Airspeed_DroneCAN.h"
53 : #include "AP_Airspeed_NMEA.h"
54 : #include "AP_Airspeed_MSP.h"
55 : #include "AP_Airspeed_AUAV.h"
56 : #include "AP_Airspeed_External.h"
57 : #include "AP_Airspeed_Scripting.h"
58 : #include "AP_Airspeed_SITL.h"
59 : extern const AP_HAL::HAL &hal;
60 :
61 : #include <AP_Vehicle/AP_FixedWing.h>
62 :
63 : #ifdef HAL_AIRSPEED_TYPE_DEFAULT
64 : #define ARSPD_DEFAULT_TYPE HAL_AIRSPEED_TYPE_DEFAULT
65 : #ifndef ARSPD_DEFAULT_PIN
66 : #define ARSPD_DEFAULT_PIN 1
67 : #endif
68 : #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
69 : // The HAL_BOARD_SITL setting is required because of current probe process for MS4525 will
70 : // connect and find the SIM_DLVR sensors & fault as there is no way to tell them apart
71 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
72 : #define ARSPD_DEFAULT_TYPE TYPE_ANALOG
73 : #define ARSPD_DEFAULT_PIN 1
74 : #elif AP_AIRSPEED_MS4525_ENABLED
75 : #define ARSPD_DEFAULT_TYPE TYPE_I2C_MS4525
76 : #ifdef HAL_DEFAULT_AIRSPEED_PIN
77 : #define ARSPD_DEFAULT_PIN HAL_DEFAULT_AIRSPEED_PIN
78 : #else
79 : #define ARSPD_DEFAULT_PIN 15
80 : #endif
81 : #else
82 : #define ARSPD_DEFAULT_TYPE TYPE_NONE
83 : #define ARSPD_DEFAULT_PIN 15
84 : #endif //CONFIG_HAL_BOARD
85 : #else // All Other Vehicle Types
86 : #define ARSPD_DEFAULT_TYPE TYPE_NONE
87 : #define ARSPD_DEFAULT_PIN 15
88 : #endif
89 :
90 : #ifndef HAL_AIRSPEED_BUS_DEFAULT
91 : #define HAL_AIRSPEED_BUS_DEFAULT 1
92 : #endif
93 :
94 : #define OPTIONS_DEFAULT AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE | AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE | AP_Airspeed::OptionsMask::USE_EKF_CONSISTENCY
95 :
96 : #define ENABLE_PARAMETER !(APM_BUILD_TYPE(APM_BUILD_ArduPlane) || defined(HAL_BUILD_AP_PERIPH))
97 :
98 : // table of user settable parameters
99 : const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
100 :
101 : #if ENABLE_PARAMETER
102 : // @Param: _ENABLE
103 : // @DisplayName: Airspeed Enable
104 : // @Description: Enable airspeed sensor support
105 : // @Values: 0:Disable, 1:Enable
106 : // @User: Standard
107 : AP_GROUPINFO_FLAGS("_ENABLE", 30, AP_Airspeed, _enable, 0, AP_PARAM_FLAG_ENABLE),
108 : #endif
109 : // slots 0-9 (and 63) were previously used by params before being refactored into AP_Airspeed_Params
110 :
111 : // @Param: _TUBE_ORDER
112 : // @DisplayName: Control pitot tube order
113 : // @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the first (often the top) connector on the sensor needs to be the stagnation pressure (the pressure at the tip of the pitot tube). If set to 1 then the second (often the bottom) connector needs to be the stagnation pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft is receiving excessive pressure on the static port compared to the stagnation port such as during a stall, which would otherwise be seen as a positive airspeed.
114 : // @User: Advanced
115 : // @Values: 0:Normal,1:Swapped,2:Auto Detect
116 :
117 : // tube order param had to be shortened so is not preserved in per group descriptions
118 :
119 : #if AIRSPEED_MAX_SENSORS > 1
120 : // @Param: _PRIMARY
121 : // @DisplayName: Primary airspeed sensor
122 : // @Description: This selects which airspeed sensor will be the primary if multiple sensors are found
123 : // @Values: 0:FirstSensor, 1:2nd Sensor, 2:3rd Sensor, 3:4th Sensor, 4:5th Sensor, 5:6th Sensor
124 : // @User: Advanced
125 : AP_GROUPINFO("_PRIMARY", 10, AP_Airspeed, primary_sensor, 0),
126 : #endif
127 :
128 : // 11-20 were previously used by second sensor params before being refactored into AP_Airspeed_Params
129 :
130 : #ifndef HAL_BUILD_AP_PERIPH
131 : // @Param: _OPTIONS
132 : // @DisplayName: Airspeed options bitmask
133 : // @Description: Bitmask of options to use with airspeed. 0:Disable use based on airspeed/groundspeed mismatch (see ARSPD_WIND_MAX), 1:Automatically reenable use based on airspeed/groundspeed mismatch recovery (see ARSPD_WIND_MAX) 2:Disable voltage correction, 3:Check that the airspeed is statistically consistent with the navigation EKF vehicle and wind velocity estimates using EKF3 (requires AHRS_EKF_TYPE = 3), 4:Report cal offset to GCS
134 : // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
135 : // @Bitmask: 0:SpeedMismatchDisable, 1:AllowSpeedMismatchRecovery, 2:DisableVoltageCorrection, 3:UseEkf3Consistency, 4:ReportOffset
136 : // @User: Advanced
137 : AP_GROUPINFO("_OPTIONS", 21, AP_Airspeed, _options, OPTIONS_DEFAULT),
138 :
139 : // @Param: _WIND_MAX
140 : // @DisplayName: Maximum airspeed and ground speed difference
141 : // @Description: If the difference between airspeed and ground speed is greater than this value the sensor will be marked unhealthy. Using ARSPD_OPTIONS this health value can be used to disable the sensor.
142 : // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
143 : // @Units: m/s
144 : // @User: Advanced
145 : AP_GROUPINFO("_WIND_MAX", 22, AP_Airspeed, _wind_max, 0),
146 :
147 : // @Param: _WIND_WARN
148 : // @DisplayName: Airspeed and GPS speed difference that gives a warning
149 : // @Description: If the difference between airspeed and GPS speed is greater than this value the sensor will issue a warning. If 0 ARSPD_WIND_MAX is used.
150 : // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
151 : // @Units: m/s
152 : // @User: Advanced
153 : AP_GROUPINFO("_WIND_WARN", 23, AP_Airspeed, _wind_warn, 0),
154 :
155 : // @Param: _WIND_GATE
156 : // @DisplayName: Re-enable Consistency Check Gate Size
157 : // @Description: Number of standard deviations applied to the re-enable EKF consistency check that is used when ARSPD_OPTIONS bit position 3 is set. Larger values will make the re-enabling of the airspeed sensor faster, but increase the likelihood of re-enabling a degraded sensor. The value can be tuned by using the ARSP.TR log message by setting ARSPD_WIND_GATE to a value that is higher than the value for ARSP.TR observed with a healthy airspeed sensor. Occasional transients in ARSP.TR above the value set by ARSPD_WIND_GATE can be tolerated provided they are less than 5 seconds in duration and less than 10% duty cycle.
158 : // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle.
159 : // @Range: 0.0 10.0
160 : // @User: Advanced
161 : AP_GROUPINFO("_WIND_GATE", 26, AP_Airspeed, _wind_gate, 5.0f),
162 :
163 : // @Param{Plane}: _OFF_PCNT
164 : // @DisplayName: Maximum offset cal speed error
165 : // @Description: The maximum percentage speed change in airspeed reports that is allowed due to offset changes between calibrations before a warning is issued. This potential speed error is in percent of AIRSPEED_MIN. 0 disables. Helps warn of calibrations without pitot being covered.
166 : // @Range: 0.0 10.0
167 : // @Units: %
168 : // @User: Advanced
169 : AP_GROUPINFO_FRAME("_OFF_PCNT", 27, AP_Airspeed, max_speed_pcnt, 0, AP_PARAM_FRAME_PLANE),
170 :
171 : #endif
172 :
173 : // @Group: _
174 : // @Path: AP_Airspeed_Params.cpp
175 : AP_SUBGROUPINFO(param[0], "_", 28, AP_Airspeed, AP_Airspeed_Params),
176 :
177 : #if AIRSPEED_MAX_SENSORS > 1
178 : // @Group: 2_
179 : // @Path: AP_Airspeed_Params.cpp
180 : AP_SUBGROUPINFO(param[1], "2_", 29, AP_Airspeed, AP_Airspeed_Params),
181 : #endif
182 :
183 : // index 30 is used by enable at the top of the table
184 :
185 : #if AIRSPEED_MAX_SENSORS > 2
186 : // @Group: 3_
187 : // @Path: AP_Airspeed_Params.cpp
188 : AP_SUBGROUPINFO(param[2], "3_", 31, AP_Airspeed, AP_Airspeed_Params),
189 : #endif
190 :
191 : #if AIRSPEED_MAX_SENSORS > 3
192 : // @Group: 4_
193 : // @Path: AP_Airspeed_Params.cpp
194 : AP_SUBGROUPINFO(param[3], "4_", 32, AP_Airspeed, AP_Airspeed_Params),
195 : #endif
196 :
197 : #if AIRSPEED_MAX_SENSORS > 4
198 : // @Group: 5_
199 : // @Path: AP_Airspeed_Params.cpp
200 : AP_SUBGROUPINFO(param[4], "5_", 33, AP_Airspeed, AP_Airspeed_Params),
201 : #endif
202 :
203 : #if AIRSPEED_MAX_SENSORS > 5
204 : // @Group: 6_
205 : // @Path: AP_Airspeed_Params.cpp
206 : AP_SUBGROUPINFO(param[5], "6_", 34, AP_Airspeed, AP_Airspeed_Params),
207 : #endif
208 :
209 : AP_GROUPEND
210 : };
211 :
212 : /*
213 : this scaling factor converts from the old system where we used a
214 : 0 to 4095 raw ADC value for 0-5V to the new system which gets the
215 : voltage in volts directly from the ADC driver
216 : */
217 : #define SCALING_OLD_CALIBRATION 819 // 4095/5
218 :
219 10325 : AP_Airspeed::AP_Airspeed()
220 : {
221 2065 : AP_Param::setup_object_defaults(this, var_info);
222 :
223 : // Setup defaults that only apply to first sensor
224 2065 : param[0].type.set_default(ARSPD_DEFAULT_TYPE);
225 : #ifndef HAL_BUILD_AP_PERIPH
226 2065 : param[0].bus.set_default(HAL_AIRSPEED_BUS_DEFAULT);
227 2065 : param[0].pin.set_default(ARSPD_DEFAULT_PIN);
228 : #endif
229 :
230 2065 : if (_singleton != nullptr) {
231 0 : AP_HAL::panic("AP_Airspeed must be singleton");
232 : }
233 2065 : _singleton = this;
234 2065 : }
235 :
236 692 : void AP_Airspeed::set_fixedwing_parameters(const AP_FixedWing *_fixed_wing_parameters)
237 : {
238 692 : fixed_wing_parameters = _fixed_wing_parameters;
239 692 : }
240 :
241 : // macro for use by HAL_INS_PROBE_LIST
242 : #define GET_I2C_DEVICE(bus, address) hal.i2c_mgr->get_device_ptr(bus, address)
243 :
244 0 : bool AP_Airspeed::add_backend(AP_Airspeed_Backend *backend)
245 : {
246 0 : if (!backend) {
247 0 : return false;
248 : }
249 0 : if (num_sensors >= AIRSPEED_MAX_SENSORS) {
250 0 : AP_HAL::panic("Too many airspeed drivers");
251 : }
252 0 : const uint8_t i = num_sensors;
253 0 : sensor[num_sensors++] = backend;
254 0 : if (!sensor[i]->init()) {
255 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Airspeed %u init failed", i+1);
256 0 : delete sensor[i];
257 0 : sensor[i] = nullptr;
258 : }
259 0 : return true;
260 : }
261 :
262 : /*
263 : macro to add a backend with check for too many sensors
264 : We don't try to start more than the maximum allowed
265 : */
266 : #define ADD_BACKEND(backend) \
267 : do { add_backend(backend); \
268 : if (num_sensors == AIRSPEED_MAX_SENSORS) { return; } \
269 : } while (0)
270 :
271 :
272 : // convert params to per instance param table
273 : // PARAMETER_CONVERSION - Added: Dec-2022
274 2054 : void AP_Airspeed::convert_per_instance()
275 : {
276 : AP_Param::ConversionInfo info;
277 : #ifndef HAL_BUILD_AP_PERIPH
278 : // Vehicle conversion
279 2054 : if (!AP_Param::find_key_by_pointer(this, info.old_key)) {
280 0 : return;
281 : }
282 :
283 : static const struct convert_table {
284 : uint32_t element[2];
285 : ap_var_type type;
286 : const char* name;
287 : } conversion_table[] = {
288 : { {4042, 714}, AP_PARAM_INT8, "TYPE" }, // ARSPD_TYPE, ARSPD2_TYPE
289 : { {74, 778}, AP_PARAM_INT8, "USE" }, // ARSPD_USE, ARSPD2_USE
290 : { {138, 842}, AP_PARAM_FLOAT, "OFFSET" }, // ARSPD_OFFSET, ARSPD2_OFFSET
291 : { {202, 906}, AP_PARAM_FLOAT, "RATIO" }, // ARSPD_RATIO, ARSPD2_RATIO
292 : { {266, 970}, AP_PARAM_INT8, "PIN" }, // ARSPD_PIN, ARSPD2_PIN
293 : #if AP_AIRSPEED_AUTOCAL_ENABLE
294 : { {330, 1034}, AP_PARAM_INT8, "AUTOCAL" }, // ARSPD_AUTOCAL, ARSPD2_AUTOCAL
295 : #endif
296 : { {394, 1098}, AP_PARAM_INT8, "TUBE_ORDR" }, // ARSPD_TUBE_ORDER, ARSPD2_TUBE_ORDR
297 : { {458, 1162}, AP_PARAM_INT8, "SKIP_CAL" }, // ARSPD_SKIP_CAL, ARSPD2_SKIP_CAL
298 : { {522, 1226}, AP_PARAM_FLOAT, "PSI_RANGE" }, // ARSPD_PSI_RANGE, ARSPD2_PSI_RANGE
299 : { {586, 1290}, AP_PARAM_INT8, "BUS" }, // ARSPD_BUS, ARSPD2_BUS
300 : { {1546, 1610}, AP_PARAM_INT32, "DEVID" }, // ARSPD_DEVID, ARSPD2_DEVID
301 : };
302 :
303 : #else
304 : // Periph conversion
305 : if (!AP_Param::find_top_level_key_by_pointer(this, info.old_key)) {
306 : return;
307 : }
308 : const struct convert_table {
309 : uint32_t element[2];
310 : ap_var_type type;
311 : const char* name;
312 : } conversion_table[] = {
313 : { {0, 11}, AP_PARAM_INT8, "TYPE" }, // ARSPD_TYPE, ARSPD2_TYPE
314 : #if AP_AIRSPEED_AUTOCAL_ENABLE
315 : { {5, 16}, AP_PARAM_INT8, "AUTOCAL" }, // ARSPD_AUTOCAL, ARSPD2_AUTOCAL
316 : #endif
317 : { {8, 19}, AP_PARAM_FLOAT, "PSI_RANGE" }, // ARSPD_PSI_RANGE, ARSPD2_PSI_RANGE
318 : { {24, 25}, AP_PARAM_INT32, "DEVID" }, // ARSPD_DEVID, ARSPD2_DEVID
319 : };
320 : #endif
321 :
322 2054 : char param_name[17] {};
323 2054 : info.new_name = param_name;
324 :
325 24648 : for (const auto & elem : conversion_table) {
326 22594 : info.type = elem.type;
327 67782 : for (uint8_t i=0; i < MIN(AIRSPEED_MAX_SENSORS,2); i++) {
328 45188 : info.old_group_element = elem.element[i];
329 45188 : if (i == 0) {
330 22594 : hal.util->snprintf(param_name, sizeof(param_name), "ARSPD_%s", elem.name);
331 : } else {
332 22594 : hal.util->snprintf(param_name, sizeof(param_name), "ARSPD%i_%s", i+1, elem.name);
333 : }
334 45188 : AP_Param::convert_old_parameter(&info, 1.0, 0);
335 : }
336 : }
337 : }
338 :
339 2054 : void AP_Airspeed::init()
340 : {
341 :
342 2054 : convert_per_instance();
343 :
344 : // Set primary from parameter to avoid primary changed message at boot
345 2054 : primary = primary_sensor.get();
346 :
347 : #if ENABLE_PARAMETER
348 : // if either type is set then enable if not manually set
349 1363 : if (!_enable.configured() && ((param[0].type.get() != TYPE_NONE) || (param[1].type.get() != TYPE_NONE))) {
350 0 : _enable.set_and_save(1);
351 : }
352 :
353 : // Check if enabled
354 1363 : if (!lib_enabled()) {
355 1354 : return;
356 : }
357 : #endif
358 :
359 700 : if (enabled(0)) {
360 699 : allocate();
361 : }
362 691 : }
363 :
364 699 : void AP_Airspeed::allocate()
365 : {
366 699 : if (sensor[0] != nullptr) {
367 : // already initialised, periph may call allocate several times to allow CAN detection
368 0 : return;
369 : }
370 :
371 : #ifdef HAL_AIRSPEED_PROBE_LIST
372 : // load sensors via a list from hwdef.dat
373 : HAL_AIRSPEED_PROBE_LIST;
374 : #else
375 : // look for sensors based on type parameters
376 2097 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
377 : #if AP_AIRSPEED_AUTOCAL_ENABLE
378 1398 : state[i].calibration.init(param[i].ratio);
379 1398 : state[i].last_saved_ratio = param[i].ratio;
380 : #endif
381 :
382 : // Set the enable automatically to false and set the probability that the airspeed is healhy to start with
383 1398 : state[i].failures.health_probability = 1.0f;
384 :
385 1398 : switch ((enum airspeed_type)param[i].type.get()) {
386 688 : case TYPE_NONE:
387 : // nothing to do
388 688 : break;
389 : #if AP_AIRSPEED_MS4525_ENABLED
390 0 : case TYPE_I2C_MS4525:
391 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_MS4525(*this, i);
392 0 : break;
393 : #endif // AP_AIRSPEED_MS4525_ENABLED
394 : #if AP_AIRSPEED_SITL_ENABLED
395 6 : case TYPE_SITL:
396 6 : sensor[i] = NEW_NOTHROW AP_Airspeed_SITL(*this, i);
397 6 : break;
398 : #endif // AP_AIRSPEED_SITL_ENABLED
399 : #if AP_AIRSPEED_ANALOG_ENABLED
400 690 : case TYPE_ANALOG:
401 690 : sensor[i] = NEW_NOTHROW AP_Airspeed_Analog(*this, i);
402 690 : break;
403 : #endif // AP_AIRSPEED_ANALOG_ENABLED
404 : #if AP_AIRSPEED_MS5525_ENABLED
405 3 : case TYPE_I2C_MS5525:
406 3 : sensor[i] = NEW_NOTHROW AP_Airspeed_MS5525(*this, i, AP_Airspeed_MS5525::MS5525_ADDR_AUTO);
407 3 : break;
408 : #endif // AP_AIRSPEED_MS5525_ENABLED
409 : #if AP_AIRSPEED_MS5525_ENABLED
410 0 : case TYPE_I2C_MS5525_ADDRESS_1:
411 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_MS5525(*this, i, AP_Airspeed_MS5525::MS5525_ADDR_1);
412 0 : break;
413 : #endif // AP_AIRSPEED_MS5525_ENABLED
414 : #if AP_AIRSPEED_MS5525_ENABLED
415 0 : case TYPE_I2C_MS5525_ADDRESS_2:
416 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_MS5525(*this, i, AP_Airspeed_MS5525::MS5525_ADDR_2);
417 0 : break;
418 : #endif // AP_AIRSPEED_MS5525_ENABLED
419 : #if AP_AIRSPEED_SDP3X_ENABLED
420 0 : case TYPE_I2C_SDP3X:
421 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_SDP3X(*this, i);
422 0 : break;
423 : #endif // AP_AIRSPEED_SDP3X_ENABLED
424 : #if AP_AIRSPEED_DLVR_ENABLED
425 10 : case TYPE_I2C_DLVR_5IN:
426 10 : sensor[i] = NEW_NOTHROW AP_Airspeed_DLVR(*this, i, 5);
427 10 : break;
428 0 : case TYPE_I2C_DLVR_10IN:
429 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_DLVR(*this, i, 10);
430 0 : break;
431 0 : case TYPE_I2C_DLVR_20IN:
432 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_DLVR(*this, i, 20);
433 0 : break;
434 0 : case TYPE_I2C_DLVR_30IN:
435 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_DLVR(*this, i, 30);
436 0 : break;
437 0 : case TYPE_I2C_DLVR_60IN:
438 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_DLVR(*this, i, 60);
439 0 : break;
440 : #endif // AP_AIRSPEED_DLVR_ENABLED
441 : #if AP_AIRSPEED_ASP5033_ENABLED
442 0 : case TYPE_I2C_ASP5033:
443 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_ASP5033(*this, i);
444 0 : break;
445 : #endif // AP_AIRSPEED_ASP5033_ENABLED
446 : #if AP_AIRSPEED_DRONECAN_ENABLED
447 0 : case TYPE_UAVCAN:
448 0 : sensor[i] = AP_Airspeed_DroneCAN::probe(*this, i, uint32_t(param[i].bus_id.get()));
449 0 : break;
450 : #endif // AP_AIRSPEED_DRONECAN_ENABLED
451 : #if AP_AIRSPEED_NMEA_ENABLED
452 0 : case TYPE_NMEA_WATER:
453 : #if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
454 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_NMEA(*this, i);
455 : #endif
456 0 : break;
457 : #endif // AP_AIRSPEED_NMEA_ENABLED
458 : #if AP_AIRSPEED_MSP_ENABLED
459 0 : case TYPE_MSP:
460 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_MSP(*this, i, 0);
461 0 : break;
462 : #endif // AP_AIRSPEED_MSP_ENABLED
463 : #if AP_AIRSPEED_EXTERNAL_ENABLED
464 0 : case TYPE_EXTERNAL:
465 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_External(*this, i);
466 0 : break;
467 : #endif // AP_AIRSPEED_EXTERNAL_ENABLED
468 : #if AP_AIRSPEED_AUAV_ENABLED
469 0 : case TYPE_AUAV_5IN:
470 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_AUAV(*this, i, 5);
471 0 : break;
472 0 : case TYPE_AUAV_10IN:
473 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_AUAV(*this, i, 10);
474 0 : break;
475 0 : case TYPE_AUAV_30IN:
476 0 : sensor[i] = NEW_NOTHROW AP_Airspeed_AUAV(*this, i, 30);
477 0 : break;
478 : #endif // AP_AIRSPEED_AUAV_ENABLED
479 : #if AP_AIRSPEED_SCRIPTING_ENABLED
480 1 : case TYPE_SCRIPTING:
481 1 : sensor[i] = NEW_NOTHROW AP_Airspeed_Scripting(*this, i);
482 1 : break;
483 : #endif // AP_AIRSPEED_SCRIPTING_ENABLED
484 : }
485 1398 : if (sensor[i] && !sensor[i]->init()) {
486 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Airspeed %u init failed", i + 1);
487 0 : delete sensor[i];
488 0 : sensor[i] = nullptr;
489 : }
490 1398 : if (sensor[i] != nullptr) {
491 710 : num_sensors = i+1;
492 : }
493 : }
494 :
495 : #if AP_AIRSPEED_DRONECAN_ENABLED
496 : // we need a 2nd pass for DroneCAN sensors so we can match order by DEVID
497 : // the 2nd pass accepts any devid
498 2097 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
499 1398 : if (sensor[i] == nullptr && (enum airspeed_type)param[i].type.get() == TYPE_UAVCAN) {
500 0 : sensor[i] = AP_Airspeed_DroneCAN::probe(*this, i, 0);
501 0 : if (sensor[i] != nullptr) {
502 0 : num_sensors = i+1;
503 : }
504 : }
505 : }
506 : #endif // AP_AIRSPEED_DRONECAN_ENABLED
507 : #endif // HAL_AIRSPEED_PROBE_LIST
508 :
509 : // set DEVID to zero for any sensors not found. This allows backends to order
510 : // based on previous value of DEVID. This allows for swapping out sensors
511 2097 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
512 1398 : if (sensor[i] == nullptr) {
513 : // note we use set() not set_and_save() to allow a sensor to be temporarily
514 : // removed for one boot without losing its slot
515 688 : param[i].bus_id.set(0);
516 : }
517 : }
518 : }
519 :
520 : // get a temperature reading if possible
521 1705242 : bool AP_Airspeed::get_temperature(uint8_t i, float &temperature) const
522 : {
523 1705242 : if (!enabled(i)) {
524 0 : return false;
525 : }
526 1705242 : if (sensor[i]) {
527 1704883 : return sensor[i]->get_temperature(temperature);
528 : }
529 359 : return false;
530 : }
531 :
532 : // calibrate the zero offset for the airspeed. This must be called at
533 : // least once before the get_airspeed() interface can be used
534 705 : void AP_Airspeed::calibrate(bool in_startup)
535 : {
536 : #ifndef HAL_BUILD_AP_PERIPH
537 705 : if (!lib_enabled()) {
538 0 : return;
539 : }
540 705 : if (hal.util->was_watchdog_reset()) {
541 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Airspeed: skipping cal");
542 1 : return;
543 : }
544 2112 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
545 1408 : if (!enabled(i)) {
546 692 : continue;
547 : }
548 716 : if (state[i].cal.state == CalibrationState::NOT_REQUIRED_ZERO_OFFSET) {
549 0 : continue;
550 : }
551 716 : if (in_startup) {
552 709 : switch ((AP_Airspeed_Params::SkipCalType)param[i].skip_cal.get()) {
553 709 : case AP_Airspeed_Params::SkipCalType::None:
554 709 : break;
555 :
556 0 : case AP_Airspeed_Params::SkipCalType::NoCalRequired:
557 : // Skip startup calibration, use saved value.
558 0 : state[i].cal.state = CalibrationState::SUCCESS;
559 0 : continue;
560 :
561 0 : case AP_Airspeed_Params::SkipCalType::SkipBootCal:
562 : // Skip startup calibration, calibration state remains NOT_STARTED.
563 0 : continue;
564 : }
565 : }
566 716 : if (sensor[i] == nullptr) {
567 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Airspeed %u not initialized, cannot cal", i+1);
568 0 : continue;
569 : }
570 716 : state[i].cal.start_ms = AP_HAL::millis();
571 716 : state[i].cal.count = 0;
572 716 : state[i].cal.sum = 0;
573 716 : state[i].cal.read_count = 0;
574 716 : state[i].cal.state = CalibrationState::IN_PROGRESS;
575 716 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Airspeed %u calibration started", i+1);
576 : }
577 : #endif // HAL_BUILD_AP_PERIPH
578 : }
579 :
580 : /*
581 : update async airspeed zero offset calibration
582 : */
583 11868 : void AP_Airspeed::update_calibration(uint8_t i, float raw_pressure)
584 : {
585 : #ifndef HAL_BUILD_AP_PERIPH
586 11868 : if (!enabled(i) || state[i].cal.start_ms == 0) {
587 0 : return;
588 : }
589 :
590 : // consider calibration complete when we have at least 15 samples
591 : // over at least 1 second
592 16639 : if (AP_HAL::millis() - state[i].cal.start_ms >= 1000 &&
593 4771 : state[i].cal.read_count > 15) {
594 581 : if (state[i].cal.count == 0) {
595 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Airspeed %u unhealthy", i + 1);
596 0 : state[i].cal.state = CalibrationState::FAILED;
597 : } else {
598 581 : float calibrated_offset = state[i].cal.sum / state[i].cal.count;
599 : // check if new offset differs too greatly from last calibration, indicating pitot uncovered in wind
600 581 : if (fixed_wing_parameters != nullptr) {
601 570 : float airspeed_min = fixed_wing_parameters->airspeed_min.get();
602 : // use percentage of AIRSPEED_MIN as criteria for max allowed change in offset
603 570 : float max_change = 0.5*(sq((1 + (max_speed_pcnt * 0.01))*airspeed_min) - sq(airspeed_min));
604 570 : if (max_speed_pcnt > 0 && (abs(calibrated_offset-param[i].offset) > max_change) && (abs(param[i].offset) > 0)) {
605 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Arspd %d offset change large;cover and recal", i +1);
606 : }
607 : }
608 581 : param[i].offset.set_and_save(calibrated_offset);
609 581 : state[i].cal.state = CalibrationState::SUCCESS;
610 581 : if (_options & AP_Airspeed::OptionsMask::REPORT_OFFSET ){
611 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u calibrated, offset = %4.0f", i + 1, calibrated_offset);
612 : } else {
613 581 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u calibrated", i + 1);
614 : }
615 : }
616 581 : state[i].cal.start_ms = 0;
617 581 : return;
618 : }
619 : // we discard the first 5 samples
620 11287 : if (state[i].healthy && state[i].cal.read_count > 5) {
621 7013 : state[i].cal.sum += raw_pressure;
622 7013 : state[i].cal.count++;
623 : }
624 11287 : state[i].cal.read_count++;
625 : #endif // HAL_BUILD_AP_PERIPH
626 : }
627 :
628 : // get aggregate calibration state for the Airspeed library:
629 13 : AP_Airspeed::CalibrationState AP_Airspeed::get_calibration_state() const
630 : {
631 23 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
632 18 : if (!enabled(i)) {
633 4 : continue;
634 : }
635 :
636 14 : switch (state[i].cal.state) {
637 6 : case CalibrationState::SUCCESS:
638 : case CalibrationState::NOT_REQUIRED_ZERO_OFFSET:
639 6 : continue;
640 0 : case CalibrationState::NOT_STARTED:
641 0 : return CalibrationState::NOT_STARTED;
642 8 : case CalibrationState::IN_PROGRESS:
643 8 : return CalibrationState::IN_PROGRESS;
644 0 : case CalibrationState::FAILED:
645 0 : return CalibrationState::FAILED;
646 : }
647 : }
648 5 : return CalibrationState::SUCCESS;
649 : }
650 :
651 : // read one airspeed sensor
652 1118660 : void AP_Airspeed::read(uint8_t i)
653 : {
654 1118660 : if (!enabled(i) || !sensor[i]) {
655 539811 : return;
656 : }
657 578904 : state[i].last_update_ms = AP_HAL::millis();
658 :
659 : // try and get a direct reading of airspeed
660 578904 : if (sensor[i]->has_airspeed()) {
661 55 : state[i].healthy = sensor[i]->get_airspeed(state[i].airspeed);
662 55 : state[i].raw_airspeed = state[i].airspeed; // for logging
663 55 : return;
664 : }
665 :
666 : #ifndef HAL_BUILD_AP_PERIPH
667 : /*
668 : remember the old healthy state
669 : */
670 578849 : bool prev_healthy = state[i].healthy;
671 : #endif
672 :
673 578849 : float raw_pressure = 0;
674 578849 : state[i].healthy = sensor[i]->get_differential_pressure(raw_pressure);
675 :
676 578849 : float airspeed_pressure = raw_pressure - get_offset(i);
677 :
678 : // remember raw pressure for logging
679 578849 : state[i].corrected_pressure = airspeed_pressure;
680 :
681 : #ifndef HAL_BUILD_AP_PERIPH
682 578849 : if (state[i].cal.start_ms != 0) {
683 11868 : update_calibration(i, raw_pressure);
684 : }
685 :
686 : // filter before clamping positive
687 578849 : if (!prev_healthy) {
688 : // if the previous state was not healthy then we should not
689 : // use an IIR filter, otherwise a bad reading will last for
690 : // some time after the sensor becomes healthy again
691 713 : state[i].filtered_pressure = airspeed_pressure;
692 : } else {
693 578136 : state[i].filtered_pressure = 0.7f * state[i].filtered_pressure + 0.3f * airspeed_pressure;
694 : }
695 :
696 : /*
697 : we support different pitot tube setups so user can choose if
698 : they want to be able to detect pressure on the static port
699 : */
700 578849 : switch ((enum pitot_tube_order)param[i].tube_order.get()) {
701 0 : case PITOT_TUBE_ORDER_NEGATIVE:
702 0 : state[i].last_pressure = -airspeed_pressure;
703 0 : state[i].raw_airspeed = sqrtf(MAX(-airspeed_pressure, 0) * param[i].ratio);
704 0 : state[i].airspeed = sqrtf(MAX(-state[i].filtered_pressure, 0) * param[i].ratio);
705 0 : break;
706 0 : case PITOT_TUBE_ORDER_POSITIVE:
707 0 : state[i].last_pressure = airspeed_pressure;
708 0 : state[i].raw_airspeed = sqrtf(MAX(airspeed_pressure, 0) * param[i].ratio);
709 0 : state[i].airspeed = sqrtf(MAX(state[i].filtered_pressure, 0) * param[i].ratio);
710 0 : break;
711 578849 : case PITOT_TUBE_ORDER_AUTO:
712 : default:
713 578849 : state[i].last_pressure = fabsf(airspeed_pressure);
714 578849 : state[i].raw_airspeed = sqrtf(fabsf(airspeed_pressure) * param[i].ratio);
715 578849 : state[i].airspeed = sqrtf(fabsf(state[i].filtered_pressure) * param[i].ratio);
716 578849 : break;
717 : }
718 : #endif // HAL_BUILD_AP_PERIPH
719 : }
720 :
721 : // Select primary sensor based on user parameters and health
722 559330 : uint8_t AP_Airspeed::select_primary()
723 : {
724 : // user selected primary from parameter, track changes
725 559330 : const uint8_t user_primary = primary_sensor.get();
726 559330 : const bool user_primary_changed = user_primary != last_user_primary;
727 559330 : last_user_primary = user_primary;
728 :
729 : // If user selected instance is both healthy and set to use then it is a valid primary
730 559330 : const bool user_healthy = healthy(user_primary);
731 559330 : const bool user_healthy_and_use = user_healthy && use(user_primary);
732 :
733 559330 : if ((user_primary_changed || !hal.util->get_soft_armed()) && user_healthy_and_use) {
734 : /*
735 : If user selected primary is healthy and set to use then:
736 : Always change when the user changes the parameter.
737 : Always change if disarmed, if armed its better to stick with the current sensor to avoid needless switching.
738 :
739 : The EKF3 innovation check only applies to the active sensor so a bad sensor will appear good after some time because
740 : the EKF is no longer using the sensor so cannot provide feedback.
741 :
742 : We can't just stick with the current primary when disarmed due to variation in detection time.
743 : */
744 128191 : return user_primary;
745 : }
746 :
747 : // If the currently selected primary is valid there is no need to change
748 431139 : const bool primary_healthy = healthy(primary);
749 431139 : if (primary_healthy && use(primary)) {
750 399541 : return primary;
751 : }
752 :
753 31598 : if (user_healthy_and_use) {
754 : // The current primary is not valid, try the user set primary first
755 3 : return user_primary;
756 : }
757 :
758 : // Select the first sensor which is both healthy and set to use
759 94781 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
760 63188 : if ((i == primary) || (i == user_primary)) {
761 : // No need to re-check current/user primary
762 32876 : continue;
763 : }
764 30312 : if (healthy(i) && use(i)) {
765 2 : return i;
766 : }
767 : }
768 :
769 : // No sensor is both healthy and set to use
770 :
771 : // Continue with current primary if healthy
772 31593 : if (primary_healthy) {
773 28218 : return primary;
774 : }
775 :
776 : // Use user selected instance if healthy
777 3375 : if (user_healthy) {
778 0 : return user_primary;
779 : }
780 :
781 : // Select the first sensor which is healthy
782 10122 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
783 6750 : if ((i == primary) || (i == user_primary)) {
784 : // No need to re-check current/user primary
785 3375 : continue;
786 : }
787 3375 : if (healthy(i)) {
788 3 : return i;
789 : }
790 : }
791 :
792 : // No healthy sensor, don't change primary
793 3372 : return primary;
794 : }
795 :
796 : // read all airspeed sensors
797 1486808 : void AP_Airspeed::update()
798 : {
799 1486808 : if (!lib_enabled()) {
800 927478 : return;
801 : }
802 :
803 1677990 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
804 1118660 : read(i);
805 : }
806 :
807 : #if HAL_GCS_ENABLED
808 : // debugging until we get MAVLink support for 2nd airspeed sensor
809 559330 : if (enabled(1)) {
810 19805 : gcs().send_named_float("AS2", get_airspeed(1));
811 : }
812 : #endif
813 :
814 : // Check for failures possibly marking sensors and unhealthy
815 559330 : check_sensor_failures();
816 :
817 : // Record old primary sensor for reporting
818 559330 : const uint8_t old_primary = primary;
819 :
820 : // Select primary sensor based on user parameters and health
821 559330 : primary = select_primary();
822 :
823 559330 : if (primary != old_primary) {
824 10 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Airspeed primary changed: %i", primary+1);
825 : }
826 :
827 : #if HAL_LOGGING_ENABLED
828 559330 : if (primary != old_primary) {
829 10 : AP::logger().Write_Event(LogEvent::AIRSPEED_PRIMARY_CHANGED);
830 : }
831 559330 : if (_log_bit != (uint32_t)-1 && AP::logger().should_log(_log_bit)) {
832 536417 : Log_Airspeed();
833 : }
834 : #endif
835 : }
836 :
837 : #if AP_AIRSPEED_MSP_ENABLED
838 : /*
839 : handle MSP airspeed data
840 : */
841 0 : void AP_Airspeed::handle_msp(const MSP::msp_airspeed_data_message_t &pkt)
842 : {
843 0 : if (!lib_enabled()) {
844 0 : return;
845 : }
846 :
847 0 : if (pkt.instance > 1) {
848 0 : return; //supporting 2 airspeed sensors at most
849 : }
850 :
851 0 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
852 0 : if (sensor[i]) {
853 0 : sensor[i]->handle_msp(pkt);
854 : }
855 : }
856 : }
857 : #endif
858 :
859 : #if AP_AIRSPEED_EXTERNAL_ENABLED
860 : /*
861 : handle airspeed airspeed data
862 : */
863 35334 : void AP_Airspeed::handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt)
864 : {
865 35334 : if (!lib_enabled()) {
866 0 : return;
867 : }
868 :
869 106002 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
870 70668 : if (param[i].type == TYPE_EXTERNAL && sensor[i]) {
871 0 : sensor[i]->handle_external(pkt);
872 : }
873 : }
874 : }
875 : #endif
876 :
877 : #if HAL_LOGGING_ENABLED
878 : // @LoggerMessage: HYGR
879 : // @Description: Hygrometer data
880 : // @Field: TimeUS: Time since system startup
881 : // @Field: Id: sensor ID
882 : // @Field: Humidity: percentage humidity
883 : // @Field: Temp: temperature in degrees C
884 :
885 536417 : void AP_Airspeed::Log_Airspeed()
886 : {
887 536417 : const uint64_t now = AP_HAL::micros64();
888 1609251 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
889 1072834 : if (!enabled(i) || sensor[i] == nullptr) {
890 516843 : continue;
891 : }
892 : float temperature;
893 555991 : if (!get_temperature(i, temperature)) {
894 529159 : temperature = 0;
895 : }
896 555991 : const struct log_ARSP pkt{
897 : LOG_PACKET_HEADER_INIT(LOG_ARSP_MSG),
898 : time_us : now,
899 : instance : i,
900 555991 : airspeed : get_raw_airspeed(i),
901 555991 : diffpressure : get_differential_pressure(i),
902 555991 : temperature : (int16_t)(temperature * 100.0f),
903 555991 : rawpressure : get_corrected_pressure(i),
904 555991 : offset : get_offset(i),
905 555991 : use : use(i),
906 555991 : healthy : healthy(i),
907 555991 : health_prob : get_health_probability(i),
908 555991 : test_ratio : get_test_ratio(i),
909 555991 : primary : get_primary()
910 555991 : };
911 555991 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
912 :
913 : #if AP_AIRSPEED_HYGROMETER_ENABLE
914 : struct {
915 : uint32_t sample_ms;
916 : float temperature;
917 : float humidity;
918 : } hygrometer;
919 555991 : if (sensor[i]->get_hygrometer(hygrometer.sample_ms, hygrometer.temperature, hygrometer.humidity) &&
920 0 : hygrometer.sample_ms != state[i].last_hygrometer_log_ms) {
921 0 : AP::logger().WriteStreaming("HYGR",
922 : "TimeUS,Id,Humidity,Temp",
923 : "s#%O",
924 : "F---",
925 : "QBff",
926 : AP_HAL::micros64(),
927 : i,
928 0 : hygrometer.humidity,
929 0 : hygrometer.temperature);
930 0 : state[i].last_hygrometer_log_ms = hygrometer.sample_ms;
931 : }
932 : #endif
933 : }
934 536417 : }
935 : #endif
936 :
937 161018536 : bool AP_Airspeed::use(uint8_t i) const
938 : {
939 : #ifndef HAL_BUILD_AP_PERIPH
940 161018536 : if (!lib_enabled()) {
941 99432820 : return false;
942 : }
943 61585715 : if (_force_disable_use) {
944 89365 : return false;
945 : }
946 61496350 : if (!enabled(i) || !param[i].use) {
947 11127167 : return false;
948 : }
949 50369182 : if (param[i].use == 2 && !is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle))) {
950 : // special case for gliders with airspeed sensors behind the
951 : // propeller. Allow airspeed to be disabled when throttle is
952 : // running
953 0 : return false;
954 : }
955 50369181 : return true;
956 : #else
957 : return false;
958 : #endif // HAL_BUILD_AP_PERIPH
959 : }
960 :
961 : /*
962 : return true if all enabled sensors are healthy
963 : */
964 564382 : bool AP_Airspeed::all_healthy(void) const
965 : {
966 1684917 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
967 1124719 : if (enabled(i) && !healthy(i)) {
968 4184 : return false;
969 : }
970 : }
971 560198 : return true;
972 : }
973 :
974 397244425 : bool AP_Airspeed::lib_enabled() const {
975 : #if ENABLE_PARAMETER
976 169419458 : return _enable > 0;
977 : #endif
978 227824967 : return true;
979 : }
980 :
981 : // return true if airspeed is enabled
982 234701682 : bool AP_Airspeed::enabled(uint8_t i) const {
983 234701682 : if (!lib_enabled()) {
984 62189340 : return false;
985 : }
986 172512343 : if (i < AIRSPEED_MAX_SENSORS) {
987 171951482 : return param[i].type.get() != TYPE_NONE;
988 : }
989 560861 : return false;
990 : }
991 :
992 : // return health status of sensor
993 119798442 : bool AP_Airspeed::healthy(uint8_t i) const {
994 119798442 : if (!enabled(i)) {
995 66634060 : return false;
996 : }
997 53164383 : bool ok = state[i].healthy && sensor[i] != nullptr;
998 : #ifndef HAL_BUILD_AP_PERIPH
999 : // sanity check the offset parameter. Zero is permitted if we are skipping calibration.
1000 106328406 : const bool allowZeroOffset = (state[i].cal.state == CalibrationState::NOT_REQUIRED_ZERO_OFFSET) ||
1001 53164023 : ((AP_Airspeed_Params::SkipCalType)param[i].skip_cal == AP_Airspeed_Params::SkipCalType::NoCalRequired);
1002 53164383 : ok &= allowZeroOffset || !is_zero(param[i].offset);
1003 : #endif
1004 53164383 : return ok;
1005 : }
1006 :
1007 : // return the current airspeed in m/s
1008 36068979 : float AP_Airspeed::get_airspeed(uint8_t i) const {
1009 36068979 : if (!enabled(i)) {
1010 : // we can't have negative airspeed so sending an obviously invalid value
1011 8258173 : return -1.0;
1012 : }
1013 27810806 : return state[i].airspeed;
1014 : }
1015 :
1016 : // return the unfiltered airspeed in m/s
1017 555991 : float AP_Airspeed::get_raw_airspeed(uint8_t i) const {
1018 555991 : if (!enabled(i)) {
1019 : // we can't have negative airspeed so sending an obviously invalid value
1020 0 : return -1.0;
1021 : }
1022 555991 : return state[i].raw_airspeed;
1023 : }
1024 :
1025 : // return the differential pressure in Pascal for the last airspeed reading
1026 1694282 : float AP_Airspeed::get_differential_pressure(uint8_t i) const {
1027 1694282 : if (!enabled(i)) {
1028 0 : return 0.0;
1029 : }
1030 1694282 : return state[i].last_pressure;
1031 : }
1032 :
1033 : // return the current corrected pressure
1034 555991 : float AP_Airspeed::get_corrected_pressure(uint8_t i) const {
1035 555991 : if (!enabled(i)) {
1036 0 : return 0.0;
1037 : }
1038 555991 : return state[i].corrected_pressure;
1039 : }
1040 :
1041 : // return the current calibration offset
1042 1134840 : float AP_Airspeed::get_offset(uint8_t i) const {
1043 : #ifndef HAL_BUILD_AP_PERIPH
1044 1134840 : if (state[i].cal.state == CalibrationState::NOT_REQUIRED_ZERO_OFFSET) {
1045 55 : return 0.0;
1046 : }
1047 1134785 : return param[i].offset;
1048 : #else
1049 : return 0.0;
1050 : #endif
1051 : }
1052 :
1053 : #if AP_AIRSPEED_HYGROMETER_ENABLE
1054 1104814 : bool AP_Airspeed::get_hygrometer(uint8_t i, uint32_t &last_sample_ms, float &temperature, float &humidity) const
1055 : {
1056 1104814 : if (!enabled(i) || sensor[i] == nullptr) {
1057 540168 : return false;
1058 : }
1059 564646 : return sensor[i]->get_hygrometer(last_sample_ms, temperature, humidity);
1060 : }
1061 : #endif // AP_AIRSPEED_HYGROMETER_ENABLE
1062 :
1063 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
1064 : #ifndef HAL_BUILD_AP_PERIPH
1065 116152 : bool AP_Airspeed::arming_checks(size_t buflen, char *buffer) const
1066 : {
1067 345650 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
1068 230902 : if (!enabled(i) || !use(i)) {
1069 204973 : continue;
1070 : }
1071 :
1072 25929 : switch (state[i].cal.state) {
1073 24525 : case CalibrationState::SUCCESS:
1074 : case CalibrationState::NOT_REQUIRED_ZERO_OFFSET:
1075 24525 : break;
1076 :
1077 42 : case CalibrationState::NOT_STARTED:
1078 : case CalibrationState::FAILED:
1079 42 : hal.util->snprintf(buffer, buflen, "%d need pre-flight calibration", i + 1);
1080 42 : return false;
1081 :
1082 1362 : case CalibrationState::IN_PROGRESS:
1083 1362 : hal.util->snprintf(buffer, buflen, "%d pre-flight calibration in progress", i + 1);
1084 1362 : return false;
1085 : }
1086 :
1087 24525 : if (!healthy(i)) {
1088 0 : hal.util->snprintf(buffer, buflen, "%d not healthy", i + 1);
1089 0 : return false;
1090 : }
1091 : }
1092 :
1093 :
1094 : // If the primary sensor is marked to not use then user should either:
1095 : // - change primary to a sensor which is marked to to use
1096 : // - allow using the primary
1097 : // If no sensors are marked for use then the check passes
1098 114748 : if (!use(primary_sensor.get())) {
1099 270859 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
1100 180573 : if (use(i)) {
1101 1 : hal.util->snprintf(buffer, buflen, "not using Primary (%i)", primary_sensor.get() + 1);
1102 1 : return false;
1103 : }
1104 : }
1105 : }
1106 :
1107 114747 : return true;
1108 : }
1109 : #endif
1110 :
1111 : #if AP_AIRSPEED_SCRIPTING_ENABLED
1112 : // get backend for a given instance, used by scripting
1113 1 : AP_Airspeed_Backend *AP_Airspeed::get_backend(uint8_t id) const
1114 : {
1115 1 : if (id >= AIRSPEED_MAX_SENSORS) {
1116 0 : return nullptr;
1117 : }
1118 1 : return sensor[id];
1119 : }
1120 : #endif // AP_AIRSPEED_SCRIPTING_ENABLED
1121 :
1122 : #else // build type is not appropriate; provide a dummy implementation:
1123 : const AP_Param::GroupInfo AP_Airspeed::var_info[] = { AP_GROUPEND };
1124 :
1125 17294 : void AP_Airspeed::update() {};
1126 0 : bool AP_Airspeed::get_temperature(uint8_t i, float &temperature) const { return false; }
1127 0 : void AP_Airspeed::calibrate(bool in_startup) {}
1128 0 : AP_Airspeed::CalibrationState AP_Airspeed::get_calibration_state() const { return CalibrationState::NOT_STARTED; }
1129 0 : bool AP_Airspeed::use(uint8_t i) const { return false; }
1130 32 : bool AP_Airspeed::enabled(uint8_t i) const { return false; }
1131 0 : bool AP_Airspeed::healthy(uint8_t i) const { return false; }
1132 0 : float AP_Airspeed::get_airspeed(uint8_t i) const { return 0.0; }
1133 0 : float AP_Airspeed::get_differential_pressure(uint8_t i) const { return 0.0; }
1134 0 : bool AP_Airspeed::arming_checks(size_t buflen, char *buffer) const { return true; }
1135 :
1136 : #if AP_AIRSPEED_MSP_ENABLED
1137 0 : void AP_Airspeed::handle_msp(const MSP::msp_airspeed_data_message_t &pkt) {}
1138 : #endif
1139 :
1140 0 : bool AP_Airspeed::all_healthy(void) const { return false; }
1141 32 : void AP_Airspeed::init(void) {};
1142 165 : AP_Airspeed::AP_Airspeed() {}
1143 :
1144 : #if AP_AIRSPEED_SCRIPTING_ENABLED
1145 0 : AP_Airspeed_Backend *AP_Airspeed::get_backend(uint8_t id) const { return nullptr; }
1146 : #endif // AP_AIRSPEED_SCRIPTING_ENABLED
1147 :
1148 : #endif // #if AP_AIRSPEED_DUMMY_METHODS_ENABLED
1149 :
1150 : // singleton instance
1151 : AP_Airspeed *AP_Airspeed::_singleton;
1152 :
1153 : namespace AP {
1154 :
1155 281285891 : AP_Airspeed *airspeed()
1156 : {
1157 281285891 : return AP_Airspeed::get_singleton();
1158 : }
1159 :
1160 : };
1161 :
1162 : #endif // AP_AIRSPEED_ENABLED
|
