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 : * APM_Baro.cpp - barometer driver
18 : *
19 : */
20 : #include "AP_Baro.h"
21 :
22 : #include <stdio.h>
23 :
24 : #include <GCS_MAVLink/GCS.h>
25 : #include <AP_Common/AP_Common.h>
26 : #include <AP_HAL/AP_HAL.h>
27 : #include <AP_Math/AP_Math.h>
28 : #include <AP_BoardConfig/AP_BoardConfig.h>
29 : #include <AP_CANManager/AP_CANManager.h>
30 : #include <AP_Vehicle/AP_Vehicle_Type.h>
31 : #include <AP_HAL/I2CDevice.h>
32 :
33 : #include "AP_Baro_SITL.h"
34 : #include "AP_Baro_BMP085.h"
35 : #include "AP_Baro_BMP280.h"
36 : #include "AP_Baro_BMP388.h"
37 : #include "AP_Baro_BMP581.h"
38 : #include "AP_Baro_SPL06.h"
39 : #include "AP_Baro_KellerLD.h"
40 : #include "AP_Baro_MS5611.h"
41 : #include "AP_Baro_ICM20789.h"
42 : #include "AP_Baro_LPS2XH.h"
43 : #include "AP_Baro_FBM320.h"
44 : #include "AP_Baro_DPS280.h"
45 : #include "AP_Baro_Dummy.h"
46 : #include "AP_Baro_DroneCAN.h"
47 : #include "AP_Baro_MSP.h"
48 : #include "AP_Baro_ExternalAHRS.h"
49 : #include "AP_Baro_ICP101XX.h"
50 : #include "AP_Baro_ICP201XX.h"
51 : #include "AP_Baro_AUAV.h"
52 :
53 : #include <AP_Airspeed/AP_Airspeed.h>
54 : #include <AP_AHRS/AP_AHRS.h>
55 : #include <AP_Arming/AP_Arming.h>
56 : #include <AP_Logger/AP_Logger.h>
57 : #include <AP_GPS/AP_GPS.h>
58 : #include <AP_Vehicle/AP_Vehicle.h>
59 : #if AP_BARO_THST_COMP_ENABLED
60 : #include <AP_Motors/AP_Motors.h>
61 : #endif
62 :
63 : #define INTERNAL_TEMPERATURE_CLAMP 35.0f
64 :
65 : #ifndef HAL_BARO_FILTER_DEFAULT
66 : #define HAL_BARO_FILTER_DEFAULT 0 // turned off by default
67 : #endif
68 :
69 : #ifndef HAL_BARO_PROBE_EXT_DEFAULT
70 : #define HAL_BARO_PROBE_EXT_DEFAULT 0
71 : #endif
72 :
73 : #ifndef HAL_BARO_EXTERNAL_BUS_DEFAULT
74 : #define HAL_BARO_EXTERNAL_BUS_DEFAULT -1
75 : #endif
76 :
77 : #ifdef HAL_BUILD_AP_PERIPH
78 : #define HAL_BARO_ALLOW_INIT_NO_BARO
79 : #endif
80 :
81 : #ifndef AP_FIELD_ELEVATION_ENABLED
82 : #define AP_FIELD_ELEVATION_ENABLED !defined(HAL_BUILD_AP_PERIPH) && !APM_BUILD_TYPE(APM_BUILD_ArduSub)
83 : #endif
84 :
85 : extern const AP_HAL::HAL& hal;
86 :
87 : // table of user settable parameters
88 : const AP_Param::GroupInfo AP_Baro::var_info[] = {
89 : // NOTE: Index numbers 0 and 1 were for the old integer
90 : // ground temperature and pressure
91 :
92 : #ifndef HAL_BUILD_AP_PERIPH
93 : // @Param: 1_GND_PRESS
94 : // @DisplayName: Ground Pressure
95 : // @Description: calibrated ground pressure in Pascals
96 : // @Units: Pa
97 : // @Increment: 1
98 : // @ReadOnly: True
99 : // @Volatile: True
100 : // @User: Advanced
101 : AP_GROUPINFO_FLAGS("1_GND_PRESS", 2, AP_Baro, sensors[0].ground_pressure, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
102 :
103 : // @Param: _GND_TEMP
104 : // @DisplayName: ground temperature
105 : // @Description: User provided ambient ground temperature in degrees Celsius. This is used to improve the calculation of the altitude the vehicle is at. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. A value of 0 means use the internal measurement ambient temperature.
106 : // @Units: degC
107 : // @Increment: 1
108 : // @Volatile: True
109 : // @User: Advanced
110 : AP_GROUPINFO("_GND_TEMP", 3, AP_Baro, _user_ground_temperature, 0),
111 :
112 : // index 4 reserved for old AP_Int8 version in legacy FRAM
113 : //AP_GROUPINFO("ALT_OFFSET", 4, AP_Baro, _alt_offset, 0),
114 :
115 : // @Param: _ALT_OFFSET
116 : // @DisplayName: altitude offset
117 : // @Description: altitude offset in meters added to barometric altitude. This is used to allow for automatic adjustment of the base barometric altitude by a ground station equipped with a barometer. The value is added to the barometric altitude read by the aircraft. It is automatically reset to 0 when the barometer is calibrated on each reboot or when a preflight calibration is performed.
118 : // @Units: m
119 : // @Increment: 0.1
120 : // @User: Advanced
121 : AP_GROUPINFO("_ALT_OFFSET", 5, AP_Baro, _alt_offset, 0),
122 :
123 : // @Param: _PRIMARY
124 : // @DisplayName: Primary barometer
125 : // @Description: This selects which barometer will be the primary if multiple barometers are found
126 : // @Values: 0:FirstBaro,1:2ndBaro,2:3rdBaro
127 : // @User: Advanced
128 : AP_GROUPINFO("_PRIMARY", 6, AP_Baro, _primary_baro, 0),
129 : #endif // HAL_BUILD_AP_PERIPH
130 :
131 : // @Param: _EXT_BUS
132 : // @DisplayName: External baro bus
133 : // @Description: This selects the bus number for looking for an I2C barometer. When set to -1 it will probe all external i2c buses based on the BARO_PROBE_EXT parameter.
134 : // @Values: -1:Disabled,0:Bus0,1:Bus1,6:Bus6
135 : // @User: Advanced
136 : AP_GROUPINFO("_EXT_BUS", 7, AP_Baro, _ext_bus, HAL_BARO_EXTERNAL_BUS_DEFAULT),
137 :
138 : // @Param{Sub}: _SPEC_GRAV
139 : // @DisplayName: Specific Gravity (For water depth measurement)
140 : // @Description: This sets the specific gravity of the fluid when flying an underwater ROV.
141 : // @Values: 1.0:Freshwater,1.024:Saltwater
142 : // @Range: 0.98 1.05
143 : AP_GROUPINFO_FRAME("_SPEC_GRAV", 8, AP_Baro, _specific_gravity, 1.0, AP_PARAM_FRAME_SUB),
144 :
145 : #if BARO_MAX_INSTANCES > 1
146 : // @Param: 2_GND_PRESS
147 : // @DisplayName: Ground Pressure
148 : // @Description: calibrated ground pressure in Pascals
149 : // @Units: Pa
150 : // @Increment: 1
151 : // @ReadOnly: True
152 : // @Volatile: True
153 : // @User: Advanced
154 : AP_GROUPINFO_FLAGS("2_GND_PRESS", 9, AP_Baro, sensors[1].ground_pressure, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
155 :
156 : // Slot 10 used to be TEMP2
157 : #endif
158 :
159 : #if BARO_MAX_INSTANCES > 2
160 : // @Param: 3_GND_PRESS
161 : // @DisplayName: Absolute Pressure
162 : // @Description: calibrated ground pressure in Pascals
163 : // @Units: Pa
164 : // @Increment: 1
165 : // @ReadOnly: True
166 : // @Volatile: True
167 : // @User: Advanced
168 : AP_GROUPINFO_FLAGS("3_GND_PRESS", 11, AP_Baro, sensors[2].ground_pressure, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
169 :
170 : // Slot 12 used to be TEMP3
171 : #endif
172 :
173 : // @Param: _FLTR_RNG
174 : // @DisplayName: Range in which sample is accepted
175 : // @Description: This sets the range around the average value that new samples must be within to be accepted. This can help reduce the impact of noise on sensors that are on long I2C cables. The value is a percentage from the average value. A value of zero disables this filter.
176 : // @Units: %
177 : // @Range: 0 100
178 : // @Increment: 1
179 : AP_GROUPINFO("_FLTR_RNG", 13, AP_Baro, _filter_range, HAL_BARO_FILTER_DEFAULT),
180 :
181 : #if AP_BARO_PROBE_EXT_PARAMETER_ENABLED
182 : // @Param: _PROBE_EXT
183 : // @DisplayName: External barometers to probe
184 : // @Description: This sets which types of external i2c barometer to look for. It is a bitmask of barometer types. The I2C buses to probe is based on BARO_EXT_BUS. If BARO_EXT_BUS is -1 then it will probe all external buses, otherwise it will probe just the bus number given in BARO_EXT_BUS.
185 : // @Bitmask: 0:BMP085,1:BMP280,2:MS5611,3:MS5607,4:MS5637,5:FBM320,6:DPS280,7:LPS25H,8:Keller,9:MS5837,10:BMP388,11:SPL06,12:MSP,13:BMP581,14:AUAV
186 : // @User: Advanced
187 : AP_GROUPINFO("_PROBE_EXT", 14, AP_Baro, _baro_probe_ext, HAL_BARO_PROBE_EXT_DEFAULT),
188 : #endif
189 :
190 : // @Param: 1_DEVID
191 : // @DisplayName: Baro ID
192 : // @Description: Barometer sensor ID, taking into account its type, bus and instance
193 : // @ReadOnly: True
194 : // @User: Advanced
195 : AP_GROUPINFO_FLAGS("1_DEVID", 15, AP_Baro, sensors[0].bus_id, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
196 :
197 : #if BARO_MAX_INSTANCES > 1
198 : // @Param: 2_DEVID
199 : // @DisplayName: Baro ID2
200 : // @Description: Barometer2 sensor ID, taking into account its type, bus and instance
201 : // @ReadOnly: True
202 : // @User: Advanced
203 : AP_GROUPINFO_FLAGS("2_DEVID", 16, AP_Baro, sensors[1].bus_id, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
204 : #endif
205 :
206 : #if BARO_MAX_INSTANCES > 2
207 : // @Param: 3_DEVID
208 : // @DisplayName: Baro ID3
209 : // @Description: Barometer3 sensor ID, taking into account its type, bus and instance
210 : // @ReadOnly: True
211 : // @User: Advanced
212 : AP_GROUPINFO_FLAGS("3_DEVID", 17, AP_Baro, sensors[2].bus_id, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
213 : #endif
214 :
215 : #if HAL_BARO_WIND_COMP_ENABLED
216 : // @Group: 1_WCF_
217 : // @Path: AP_Baro_Wind.cpp
218 : AP_SUBGROUPINFO(sensors[0].wind_coeff, "1_WCF_", 18, AP_Baro, WindCoeff),
219 :
220 : #if BARO_MAX_INSTANCES > 1
221 : // @Group: 2_WCF_
222 : // @Path: AP_Baro_Wind.cpp
223 : AP_SUBGROUPINFO(sensors[1].wind_coeff, "2_WCF_", 19, AP_Baro, WindCoeff),
224 : #endif
225 : #if BARO_MAX_INSTANCES > 2
226 : // @Group: 3_WCF_
227 : // @Path: AP_Baro_Wind.cpp
228 : AP_SUBGROUPINFO(sensors[2].wind_coeff, "3_WCF_", 20, AP_Baro, WindCoeff),
229 : #endif
230 : #endif // HAL_BARO_WIND_COMP_ENABLED
231 :
232 : #if AP_FIELD_ELEVATION_ENABLED
233 : // @Param{Blimp,Copter,Plane,Rover,Tracker}: _FIELD_ELV
234 : // @DisplayName: field elevation
235 : // @Description: User provided field elevation in meters. This is used to improve the calculation of the altitude the vehicle is at. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. Changes to this parameter will only be used when disarmed. A value of 0 means the EKF origin height is used for takeoff height above sea level.
236 : // @Units: m
237 : // @Increment: 0.1
238 : // @Volatile: True
239 : // @User: Advanced
240 : AP_GROUPINFO("_FIELD_ELV", 22, AP_Baro, _field_elevation, 0),
241 : #endif
242 :
243 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
244 : // @Param{Copter,Plane}: _ALTERR_MAX
245 : // @DisplayName: Altitude error maximum
246 : // @Description: This is the maximum acceptable altitude discrepancy between GPS altitude and barometric presssure altitude calculated against a standard atmosphere for arming checks to pass. If you are getting an arming error due to this parameter then you may have a faulty or substituted barometer. A common issue is vendors replacing a MS5611 in a "Pixhawk" with a MS5607. If you have that issue then please see BARO_OPTIONS parameter to force the MS5611 to be treated as a MS5607. This check is disabled if the value is zero.
247 : // @Units: m
248 : // @Increment: 1
249 : // @Range: 0 5000
250 : // @User: Advanced
251 : AP_GROUPINFO("_ALTERR_MAX", 23, AP_Baro, _alt_error_max, 2000),
252 :
253 : // @Param{Copter,Plane}: _OPTIONS
254 : // @DisplayName: Barometer options
255 : // @Description: Barometer options
256 : // @Bitmask: 0:Treat MS5611 as MS5607
257 : // @User: Advanced
258 : AP_GROUPINFO("_OPTIONS", 24, AP_Baro, _options, 0),
259 : #endif
260 :
261 : #if AP_BARO_THST_COMP_ENABLED
262 : // @Param: 1_THST_SCALE
263 : // @DisplayName: Thrust compensation
264 : // @Description: Thrust scaling in Pascals. This value scaled by the normalized thrust is subtracted from the barometer pressure. This is used to adjust linearly based on the thrust output for local pressure difference induced by the props.
265 : // @Range: -300 300
266 : // @User: Advanced
267 : AP_GROUPINFO("1_THST_SCALE", 25, AP_Baro, sensors[0].mot_scale, 0),
268 :
269 : // @Param: _THST_FILT
270 : // @DisplayName: Thrust compensation filter cutoff
271 : // @Description: Low-pass filter cutoff frequency for thrust compensation. This smooths the throttle input to prevent rapid baro altitude changes during throttle transients. Set to 0 to disable filtering.
272 : // @Range: 0 10
273 : // @Units: Hz
274 : // @User: Advanced
275 : AP_GROUPINFO("_THST_FILT", 26, AP_Baro, _thst_filt_cutoff, 1.0f),
276 : #endif // AP_BARO_THST_COMP_ENABLED
277 : AP_GROUPEND
278 : };
279 :
280 : // singleton instance
281 : AP_Baro *AP_Baro::_singleton;
282 :
283 : /*
284 : AP_Baro constructor
285 : */
286 8408 : AP_Baro::AP_Baro()
287 : {
288 2102 : _singleton = this;
289 :
290 2102 : AP_Param::setup_object_defaults(this, var_info);
291 2102 : _field_elevation_active = _field_elevation;
292 2102 : }
293 :
294 : #if AP_BARO_CALIBRATION_ENABLED
295 : // calibrate the barometer. This must be called at least once before
296 : // the altitude() or climb_rate() interfaces can be used
297 2094 : void AP_Baro::calibrate(bool save)
298 : {
299 : // start by assuming all sensors are calibrated (for healthy() test)
300 6331 : for (uint8_t i=0; i<_num_sensors; i++) {
301 4237 : sensors[i].calibrated = true;
302 4237 : sensors[i].alt_ok = true;
303 : }
304 :
305 2094 : if (hal.util->was_watchdog_reset()) {
306 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Baro: skipping calibration after WDG reset");
307 1 : return;
308 : }
309 :
310 : #if AP_SIM_BARO_ENABLED
311 2093 : if (AP::sitl()->baro_count == 0) {
312 0 : return;
313 : }
314 : #endif
315 :
316 : #ifdef HAL_BARO_ALLOW_INIT_NO_BARO
317 : if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr) {
318 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Baro: no sensors found, skipping calibration");
319 : return;
320 : }
321 : #endif
322 :
323 2093 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Calibrating barometer");
324 :
325 : // reset the altitude offset when we calibrate. The altitude
326 : // offset is supposed to be for within a flight
327 2093 : _alt_offset.set_and_save(0);
328 :
329 : // let the barometer settle for a full second after startup
330 : // the MS5611 reads quite a long way off for the first second,
331 : // leading to about 1m of error if we don't wait
332 22943 : for (uint8_t i = 0; i < 10; i++) {
333 20858 : uint32_t tstart = AP_HAL::millis();
334 : do {
335 21266 : update();
336 21266 : if (AP_HAL::millis() - tstart > 500) {
337 8 : AP_BoardConfig::config_error("Baro: unable to calibrate");
338 : }
339 21258 : hal.scheduler->delay(10);
340 21258 : } while (!healthy());
341 20850 : hal.scheduler->delay(100);
342 : }
343 :
344 : // now average over 5 values for the ground pressure settings
345 2085 : float sum_pressure[BARO_MAX_INSTANCES] = {0};
346 2085 : uint8_t count[BARO_MAX_INSTANCES] = {0};
347 2085 : const uint8_t num_samples = 5;
348 :
349 12510 : for (uint8_t c = 0; c < num_samples; c++) {
350 10425 : uint32_t tstart = AP_HAL::millis();
351 : do {
352 10425 : update();
353 10425 : if (AP_HAL::millis() - tstart > 500) {
354 0 : AP_BoardConfig::config_error("Baro: unable to calibrate");
355 : }
356 10425 : } while (!healthy());
357 31590 : for (uint8_t i=0; i<_num_sensors; i++) {
358 21165 : if (healthy(i)) {
359 21165 : sum_pressure[i] += sensors[i].pressure;
360 21165 : count[i] += 1;
361 : }
362 : }
363 10425 : hal.scheduler->delay(100);
364 : }
365 6318 : for (uint8_t i=0; i<_num_sensors; i++) {
366 4233 : if (count[i] == 0) {
367 0 : sensors[i].calibrated = false;
368 : } else {
369 4233 : if (save) {
370 4069 : float p0_sealevel = get_sealevel_pressure(sum_pressure[i] / count[i], _field_elevation_active);
371 4069 : sensors[i].ground_pressure.set_and_save(p0_sealevel);
372 : }
373 : }
374 : }
375 :
376 2085 : _guessed_ground_temperature = get_external_temperature();
377 :
378 : // panic if all sensors are not calibrated
379 2085 : uint8_t num_calibrated = 0;
380 6318 : for (uint8_t i=0; i<_num_sensors; i++) {
381 4233 : if (sensors[i].calibrated) {
382 4233 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Barometer %u calibration complete", i+1);
383 4233 : num_calibrated++;
384 : }
385 : }
386 2085 : if (num_calibrated) {
387 2085 : return;
388 : }
389 0 : AP_BoardConfig::config_error("Baro: all sensors uncalibrated");
390 : }
391 :
392 : /*
393 : update the barometer calibration
394 : this updates the baro ground calibration to the current values. It
395 : can be used before arming to keep the baro well calibrated
396 : */
397 17647 : void AP_Baro::update_calibration()
398 : {
399 17647 : const uint32_t now = AP_HAL::millis();
400 17647 : const bool do_notify = now - _last_notify_ms > 10000;
401 17647 : if (do_notify) {
402 2032 : _last_notify_ms = now;
403 : }
404 53514 : for (uint8_t i=0; i<_num_sensors; i++) {
405 35867 : if (healthy(i)) {
406 35847 : float corrected_pressure = get_sealevel_pressure(get_pressure(i) + sensors[i].p_correction, _field_elevation_active);
407 35847 : sensors[i].ground_pressure.set(corrected_pressure);
408 : }
409 :
410 : // don't notify the GCS too rapidly or we flood the link
411 35867 : if (do_notify) {
412 4112 : sensors[i].ground_pressure.notify();
413 : }
414 : }
415 :
416 : // always update the guessed ground temp
417 17647 : _guessed_ground_temperature = get_external_temperature();
418 17647 : }
419 : #endif // AP_BARO_CALIBRATION_ENABLED
420 :
421 : // return current climb_rate estimate relative to time that calibrate()
422 : // was called. Returns climb rate in meters/s, positive means up
423 : // note that this relies on read() being called regularly to get new data
424 8677043 : float AP_Baro::get_climb_rate(void)
425 : {
426 : // we use a 7 point derivative filter on the climb rate. This seems
427 : // to produce somewhat reasonable results on real hardware
428 8677043 : return _climb_rate_filter.slope() * 1.0e3f;
429 : }
430 :
431 : // returns the ground temperature in degrees C, selecting either a user
432 : // provided one, or the internal estimate
433 2949525 : float AP_Baro::get_ground_temperature(void) const
434 : {
435 2949525 : if (is_zero(_user_ground_temperature)) {
436 2949525 : return _guessed_ground_temperature;
437 : } else {
438 0 : return _user_ground_temperature;
439 : }
440 : }
441 :
442 :
443 : /*
444 : set external temperature to be used for calibration (degrees C)
445 : */
446 0 : void AP_Baro::set_external_temperature(float temperature)
447 : {
448 0 : _external_temperature = temperature;
449 0 : _last_external_temperature_ms = AP_HAL::millis();
450 0 : }
451 :
452 : /*
453 : get the temperature in degrees C to be used for calibration purposes
454 : */
455 19732 : float AP_Baro::get_external_temperature(const uint8_t instance) const
456 : {
457 : // if we have a recent external temperature then use it
458 19732 : if (_last_external_temperature_ms != 0 && AP_HAL::millis() - _last_external_temperature_ms < 10000) {
459 0 : return _external_temperature;
460 : }
461 :
462 : #ifndef HAL_BUILD_AP_PERIPH
463 : #if AP_AIRSPEED_ENABLED
464 : // if we don't have an external temperature then try to use temperature
465 : // from the airspeed sensor
466 19732 : AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
467 19732 : if (airspeed != nullptr) {
468 : float temperature;
469 19637 : if (airspeed->healthy() && airspeed->get_temperature(temperature)) {
470 879 : return temperature;
471 : }
472 : }
473 : #endif
474 : #endif
475 :
476 : // if we don't have an external temperature and airspeed temperature
477 : // then use the minimum of the barometer temperature and 35 degrees C.
478 : // The reason for not just using the baro temperature is it tends to read high,
479 : // often 30 degrees above the actual temperature. That means the
480 : // EAS2TAS tends to be off by quite a large margin, as well as
481 : // the calculation of altitude difference between two pressures
482 : // reporting a high temperature will cause the aircraft to
483 : // estimate itself as flying higher then it actually is.
484 18853 : return MIN(get_temperature(instance), INTERNAL_TEMPERATURE_CLAMP);
485 : }
486 :
487 :
488 10519 : bool AP_Baro::_add_backend(AP_Baro_Backend *backend)
489 : {
490 10519 : if (!backend) {
491 6282 : return false;
492 : }
493 4237 : if (_num_drivers >= ARRAY_SIZE(drivers)) {
494 0 : AP_HAL::panic("Too many barometer drivers");
495 : }
496 4237 : drivers[_num_drivers++] = backend;
497 4237 : return true;
498 : }
499 :
500 : /*
501 : wrapper around hal.i2c_mgr->get_device() that prevents duplicate devices being opened
502 : */
503 2032 : bool AP_Baro::_i2c_sensor_is_registered(uint8_t bus, uint8_t address) const
504 : {
505 6082 : for (int i=0; i<_num_sensors; ++i) {
506 8100 : if (AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_I2C, bus, address, 0) ==
507 4050 : AP_HAL::Device::change_bus_id(uint32_t(sensors[i].bus_id.get()), 0)) {
508 : // device already has been defined.
509 0 : return true;
510 : }
511 : }
512 2032 : return false;
513 : }
514 :
515 : #define RETURN_IF_NO_SPACE \
516 : do { \
517 : if (_num_drivers == BARO_MAX_DRIVERS || \
518 : _num_sensors == BARO_MAX_INSTANCES) { \
519 : return; \
520 : } \
521 : } while (0)
522 :
523 : // macro for use by HAL_INS_PROBE_LIST and various helper functions
524 : #define GET_I2C_DEVICE_PTR(bus, address) _i2c_sensor_is_registered(bus, address)?nullptr:hal.i2c_mgr->get_device_ptr(bus, address)
525 :
526 : // probe for an I2C barometer.
527 2032 : void AP_Baro::probe_i2c_dev(AP_Baro_Backend* (*probefn)(AP_Baro&, AP_HAL::Device&), uint8_t bus, uint8_t addr)
528 : {
529 2032 : auto *dev = GET_I2C_DEVICE_PTR(bus, addr); // dev may be freed by probe_dev
530 2032 : probe_dev(probefn, dev);
531 2032 : }
532 :
533 : // probe for an I2C barometer
534 0 : void AP_Baro::probe_spi_dev(AP_Baro_Backend* (*probefn)(AP_Baro&, AP_HAL::Device&), const char *name)
535 : {
536 0 : auto *dev = hal.spi->get_device_ptr(name); // dev may be freed by probe_dev
537 0 : probe_dev(probefn, dev);
538 0 : }
539 :
540 : // see if Device dev exists. If it does not delete it.
541 2032 : void AP_Baro::probe_dev(AP_Baro_Backend* (*probefn)(AP_Baro&, AP_HAL::Device&), AP_HAL::Device *dev)
542 : {
543 2032 : if (dev == nullptr) {
544 1949 : return;
545 : }
546 83 : AP_Baro_Backend *backend = probefn(*this, *dev);
547 83 : if (backend == nullptr) {
548 82 : delete dev;
549 82 : return;
550 : }
551 1 : if (!_add_backend(backend)) {
552 0 : delete backend;
553 0 : delete dev;
554 0 : return;
555 : }
556 : }
557 :
558 : #if AP_BARO_LPS2XH_ENABLED
559 0 : void AP_Baro::probe_lps2xh_via_Invensense_IMU(uint8_t bus, uint8_t addr, uint8_t mpu_addr)
560 : {
561 0 : auto *i2c_dev = GET_I2C_DEVICE_PTR(bus, addr);
562 0 : AP_Baro_Backend *backend = AP_Baro_LPS2XH::probe_InvensenseIMU(*this, *i2c_dev, mpu_addr);
563 0 : if (!_add_backend(backend)) {
564 0 : delete i2c_dev;
565 : }
566 0 : }
567 : #endif // AP_BARO_LPS2XH_ENABLED
568 :
569 : #if AP_BARO_ICM20789_ENABLED
570 0 : void AP_Baro::probe_icm20789(uint8_t bus, uint8_t addr, uint8_t mpu_bus, uint8_t mpu_addr)
571 : {
572 0 : auto *i2c_dev = GET_I2C_DEVICE_PTR(bus, addr);
573 0 : AP_HAL::I2CDevice *mpu_dev = GET_I2C_DEVICE_PTR(mpu_bus, mpu_addr);
574 0 : _probe_icm20789(i2c_dev, mpu_dev); // deletes devices on failure
575 0 : }
576 :
577 0 : void AP_Baro::probe_icm20789(uint8_t bus, uint8_t addr, const char *mpu_name)
578 : {
579 0 : auto *i2c_dev = GET_I2C_DEVICE_PTR(bus, addr);
580 0 : AP_HAL::SPIDevice *mpu_dev = hal.spi->get_device_ptr(mpu_name);
581 0 : _probe_icm20789(i2c_dev, mpu_dev); // deletes devices on failure
582 0 : }
583 :
584 : // convenience underlying method for other probe functions;
585 : // will. delete the passed-in devices if a backend is not found
586 0 : void AP_Baro::_probe_icm20789(AP_HAL::I2CDevice *i2c_dev, AP_HAL::Device *mpu_dev)
587 : {
588 0 : if (i2c_dev == nullptr) {
589 0 : return;
590 : }
591 0 : if (mpu_dev == nullptr) {
592 0 : delete i2c_dev;
593 0 : return;
594 : }
595 0 : AP_Baro_Backend *backend = AP_Baro_ICM20789::probe(*this, *i2c_dev, *mpu_dev);
596 0 : if (!_add_backend(backend)) {
597 0 : delete i2c_dev;
598 0 : delete mpu_dev;
599 : }
600 : }
601 : #endif // AP_BARO_ICM20789_ENABLED
602 :
603 : /*
604 : initialise the barometer object, loading backend drivers
605 : */
606 2094 : void AP_Baro::init(void)
607 : {
608 2094 : init_done = true;
609 :
610 : // always set field elevation to zero on reboot in the case user
611 : // fails to update. TBD automate sanity checking error bounds on
612 : // on previously saved value at new location etc.
613 2094 : if (!is_zero(_field_elevation)) {
614 0 : _field_elevation.set_and_save(0.0f);
615 0 : _field_elevation.notify();
616 : }
617 :
618 : // zero bus IDs before probing
619 8376 : for (auto &sensor : sensors) {
620 6282 : sensor.bus_id.set(0);
621 : }
622 :
623 : #if AP_SIM_BARO_ENABLED
624 2094 : SITL::SIM *sitl = AP::sitl();
625 2094 : if (sitl == nullptr) {
626 0 : AP_HAL::panic("No SITL pointer");
627 : }
628 : #if !AP_TEST_DRONECAN_DRIVERS
629 : // use dronecan instances instead of SITL instances
630 6268 : for(uint8_t i = 0; i < sitl->baro_count; i++) {
631 4174 : _add_backend(NEW_NOTHROW AP_Baro_SITL(*this));
632 4174 : RETURN_IF_NO_SPACE;
633 : }
634 : #endif
635 : #endif
636 :
637 : #if AP_BARO_DRONECAN_ENABLED
638 : // Detect UAVCAN Modules, try as many times as there are driver slots
639 8376 : for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) {
640 6282 : _add_backend(AP_Baro_DroneCAN::probe(*this));
641 6282 : RETURN_IF_NO_SPACE;
642 : }
643 : #endif
644 :
645 : #if AP_BARO_EXTERNALAHRS_ENABLED
646 2094 : const int8_t serial_port = AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::BARO);
647 2094 : if (serial_port >= 0) {
648 62 : _add_backend(NEW_NOTHROW AP_Baro_ExternalAHRS(*this, serial_port));
649 62 : RETURN_IF_NO_SPACE;
650 : }
651 : #endif
652 :
653 : #if AP_SIM_BARO_ENABLED
654 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && AP_BARO_MS5611_ENABLED
655 2032 : probe_i2c_dev(AP_Baro_MS5611::probe, _ext_bus, HAL_BARO_MS5611_I2C_ADDR);
656 2032 : RETURN_IF_NO_SPACE;
657 : #endif
658 : // do not probe for other drivers when using simulation:
659 2031 : return;
660 : #endif
661 :
662 : #if defined(HAL_BARO_PROBE_LIST)
663 : // probe list from BARO lines in hwdef.dat
664 : HAL_BARO_PROBE_LIST;
665 : #elif AP_FEATURE_BOARD_DETECT
666 : switch (AP_BoardConfig::get_board_type()) {
667 : case AP_BoardConfig::PX4_BOARD_PX4V1:
668 : #if AP_BARO_MS5611_ENABLED && defined(HAL_BARO_MS5611_I2C_BUS)
669 : probe_i2c_dev(AP_Baro_MS5611::probe, HAL_BARO_MS5611_I2C_BUS, HAL_BARO_MS5611_I2C_ADDR);
670 : RETURN_IF_NO_SPACE;
671 : #endif
672 : break;
673 :
674 : case AP_BoardConfig::PX4_BOARD_PIXHAWK:
675 : case AP_BoardConfig::PX4_BOARD_PHMINI:
676 : case AP_BoardConfig::PX4_BOARD_AUAV21:
677 : case AP_BoardConfig::PX4_BOARD_PH2SLIM:
678 : case AP_BoardConfig::PX4_BOARD_FMUV6:
679 : #if AP_BARO_MS5611_ENABLED
680 : probe_spi_dev(AP_Baro_MS5611::probe, HAL_BARO_MS5611_NAME);
681 : RETURN_IF_NO_SPACE;
682 : #endif
683 : break;
684 :
685 : case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
686 : #if AP_BARO_MS5611_ENABLED
687 : probe_spi_dev(AP_Baro_MS5611::probe, HAL_BARO_MS5611_SPI_EXT_NAME);
688 : RETURN_IF_NO_SPACE;
689 : probe_spi_dev(AP_Baro_MS5611::probe, HAL_BARO_MS5611_NAME);
690 : RETURN_IF_NO_SPACE;
691 : #endif
692 : break;
693 :
694 : case AP_BoardConfig::PX4_BOARD_AEROFC:
695 : #if AP_BARO_MS5607_ENABLED
696 : #ifdef HAL_BARO_MS5607_I2C_BUS
697 : probe_i2c_dev(AP_Baro_MS5607::probe, HAL_BARO_MS5607_I2C_BUS, HAL_BARO_MS5607_I2C_ADDR);
698 : RETURN_IF_NO_SPACE;
699 : #endif
700 : #endif // AP_BARO_MS5607_ENABLED
701 : break;
702 :
703 : default:
704 : break;
705 : }
706 : #endif // defined(HAL_BARO_PROBE_LIST) / AP_FEATURE_BOARD_DETECT
707 :
708 : // can optionally have baro on I2C too
709 0 : if (_ext_bus >= 0) {
710 : #if APM_BUILD_TYPE(APM_BUILD_ArduSub)
711 : #if AP_BARO_MS5837_ENABLED
712 : probe_i2c_dev(AP_Baro_MS5837::probe, _ext_bus, HAL_BARO_MS5837_I2C_ADDR);
713 : RETURN_IF_NO_SPACE;
714 : #endif
715 : #if AP_BARO_KELLERLD_ENABLED
716 : probe_i2c_dev(AP_Baro_KellerLD::probe, _ext_bus, HAL_BARO_KELLERLD_I2C_ADDR);
717 : RETURN_IF_NO_SPACE;
718 : #endif
719 : #else
720 : #if AP_BARO_MS5611_ENABLED
721 0 : probe_i2c_dev(AP_Baro_MS5611::probe, _ext_bus, HAL_BARO_MS5611_I2C_ADDR);
722 0 : RETURN_IF_NO_SPACE;
723 : #endif
724 : #endif
725 : }
726 :
727 : #if AP_BARO_PROBE_EXTERNAL_I2C_BUSES
728 0 : _probe_i2c_barometers();
729 0 : RETURN_IF_NO_SPACE;
730 : #endif
731 :
732 : #if AP_BARO_MSP_ENABLED
733 0 : if ((_baro_probe_ext.get() & PROBE_MSP) && msp_instance_mask == 0) {
734 : // allow for late addition of MSP sensor
735 0 : msp_instance_mask |= 1;
736 : }
737 0 : for (uint8_t i=0; i<8; i++) {
738 0 : if (msp_instance_mask & (1U<<i)) {
739 0 : _add_backend(NEW_NOTHROW AP_Baro_MSP(*this, i));
740 0 : RETURN_IF_NO_SPACE;
741 : }
742 : }
743 : #endif
744 :
745 : #if !defined(HAL_BARO_ALLOW_INIT_NO_BARO) // most boards requires external baro
746 : #if AP_SIM_BARO_ENABLED
747 : if (sitl->baro_count == 0) {
748 : return;
749 : }
750 : #endif
751 0 : if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr) {
752 0 : AP_BoardConfig::config_error("Baro: unable to initialise driver");
753 : }
754 : #endif
755 : #ifdef HAL_BUILD_AP_PERIPH
756 : // AP_Periph always is set calibrated. We only want the pressure,
757 : // so ground calibration is unnecessary
758 : for (uint8_t i=0; i<_num_sensors; i++) {
759 : sensors[i].calibrated = true;
760 : sensors[i].alt_ok = true;
761 : }
762 : #endif
763 :
764 : #if AP_BARO_THST_COMP_ENABLED
765 : const float cutoff_freq = _thst_filt_cutoff.get();
766 : if (cutoff_freq > 0) {
767 : _thrust_filter.set_cutoff_frequency(cutoff_freq);
768 : _thrust_filter.reset(0.0f);
769 : }
770 : _thrust_filter_last_update_us = AP_HAL::micros();
771 : #endif
772 : }
773 :
774 : #if AP_BARO_PROBE_EXTERNAL_I2C_BUSES
775 : /*
776 : probe all the i2c barometers enabled with BARO_PROBE_EXT. This is
777 : used on boards without a builtin barometer
778 : */
779 0 : void AP_Baro::_probe_i2c_barometers(void)
780 : {
781 0 : uint32_t probe = _baro_probe_ext.get();
782 : (void)probe; // may be unused if most baros compiled out
783 0 : uint32_t mask = hal.i2c_mgr->get_bus_mask_external();
784 0 : if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
785 : // for the purpose of baro probing, treat CubeBlack internal i2c as external. It has
786 : // no internal i2c baros, so this is safe
787 0 : mask |= hal.i2c_mgr->get_bus_mask_internal();
788 : }
789 : // if the user has set BARO_EXT_BUS then probe the bus given by that parameter
790 0 : int8_t ext_bus = _ext_bus;
791 0 : if (ext_bus >= 0) {
792 0 : mask = 1U << (uint8_t)ext_bus;
793 : }
794 :
795 : static const struct BaroProbeSpec {
796 : uint32_t bit;
797 : AP_Baro_Backend* (*probefn)(AP_Baro&, AP_HAL::Device&);
798 : uint8_t addr;
799 : } baroprobespec[] {
800 : #if AP_BARO_BMP085_ENABLED
801 : { PROBE_BMP085, AP_Baro_BMP085::probe, HAL_BARO_BMP085_I2C_ADDR },
802 : #endif
803 : #if AP_BARO_BMP280_ENABLED
804 : { PROBE_BMP280, AP_Baro_BMP280::probe, HAL_BARO_BMP280_I2C_ADDR },
805 : { PROBE_BMP280, AP_Baro_BMP280::probe, HAL_BARO_BMP280_I2C_ADDR2 },
806 : #endif
807 : #if AP_BARO_SPL06_ENABLED
808 : { PROBE_SPL06, AP_Baro_SPL06::probe, HAL_BARO_SPL06_I2C_ADDR },
809 : { PROBE_SPL06, AP_Baro_SPL06::probe, HAL_BARO_SPL06_I2C_ADDR2 },
810 : #endif
811 : #if AP_BARO_BMP388_ENABLED
812 : { PROBE_BMP388, AP_Baro_BMP388::probe, HAL_BARO_BMP388_I2C_ADDR },
813 : { PROBE_BMP388, AP_Baro_BMP388::probe, HAL_BARO_BMP388_I2C_ADDR2 },
814 : #endif
815 : #if AP_BARO_BMP581_ENABLED
816 : { PROBE_BMP581, AP_Baro_BMP581::probe, HAL_BARO_BMP581_I2C_ADDR },
817 : { PROBE_BMP581, AP_Baro_BMP581::probe, HAL_BARO_BMP581_I2C_ADDR2 },
818 : #endif
819 : #if AP_BARO_MS5611_ENABLED
820 : { PROBE_MS5611, AP_Baro_MS5611::probe, HAL_BARO_MS5611_I2C_ADDR },
821 : { PROBE_MS5611, AP_Baro_MS5611::probe, HAL_BARO_MS5611_I2C_ADDR2 },
822 : #endif // AP_BARO_MS5611_ENABLED
823 : #if AP_BARO_MS5607_ENABLED
824 : { PROBE_MS5607, AP_Baro_MS5607::probe, HAL_BARO_MS5607_I2C_ADDR },
825 : #endif // AP_BARO_MS5607_ENABLED
826 : #if AP_BARO_MS5637_ENABLED
827 : { PROBE_MS5637, AP_Baro_MS5637::probe, HAL_BARO_MS5637_I2C_ADDR },
828 : #endif // AP_BARO_MS5637_ENABLED
829 : #if AP_BARO_FBM320_ENABLED
830 : { PROBE_FBM320, AP_Baro_FBM320::probe, HAL_BARO_FBM320_I2C_ADDR },
831 : { PROBE_FBM320, AP_Baro_FBM320::probe, HAL_BARO_FBM320_I2C_ADDR2 },
832 : #endif
833 : #if AP_BARO_DPS280_ENABLED
834 : { PROBE_DPS280, AP_Baro_DPS280::probe, HAL_BARO_DPS280_I2C_ADDR },
835 : { PROBE_DPS280, AP_Baro_DPS280::probe, HAL_BARO_DPS280_I2C_ADDR2 },
836 : #endif
837 : #if AP_BARO_LPS2XH_ENABLED
838 : { PROBE_LPS25H, AP_Baro_LPS2XH::probe, HAL_BARO_LPS25H_I2C_ADDR },
839 : #endif
840 : #if AP_BARO_AUAV_ENABLED
841 : { PROBE_AUAV, AP_Baro_AUAV::probe, HAL_BARO_AUAV_I2C_ADDR },
842 : #endif
843 :
844 : #if APM_BUILD_TYPE(APM_BUILD_ArduSub)
845 : #if AP_BARO_KELLERLD_ENABLED
846 : { PROBE_KELLER, AP_Baro_KellerLD::probe, HAL_BARO_KELLERLD_I2C_ADDR },
847 : #endif
848 : #if AP_BARO_MS5837_ENABLED
849 : { PROBE_MS5837, AP_Baro_MS5837::probe, HAL_BARO_MS5837_I2C_ADDR },
850 : #endif
851 : #endif // APM_BUILD_TYPE(APM_BUILD_ArduSub)
852 : };
853 :
854 0 : for (const auto &spec : baroprobespec) {
855 0 : if (!(probe & spec.bit)) {
856 : // not in mask to be probed for
857 0 : continue;
858 : }
859 0 : FOREACH_I2C_MASK(i, mask) {
860 0 : probe_i2c_dev(spec.probefn, i, spec.addr);
861 0 : RETURN_IF_NO_SPACE;
862 : }
863 : }
864 : }
865 : #endif // AP_BARO_PROBE_EXTERNAL_I2C_BUSES
866 :
867 : #if HAL_LOGGING_ENABLED
868 1535898 : bool AP_Baro::should_log() const
869 : {
870 1535898 : AP_Logger *logger = AP_Logger::get_singleton();
871 1535898 : if (logger == nullptr) {
872 0 : return false;
873 : }
874 1535898 : if (_log_baro_bit == (uint32_t)-1) {
875 0 : return false;
876 : }
877 1535898 : if (!logger->should_log(_log_baro_bit)) {
878 68484 : return false;
879 : }
880 1467414 : return true;
881 : }
882 : #endif
883 :
884 : /*
885 : call update on all drivers
886 : */
887 1535898 : void AP_Baro::update(void)
888 : {
889 1535898 : WITH_SEMAPHORE(_rsem);
890 :
891 1535898 : if (fabsf(_alt_offset - _alt_offset_active) > 0.01f) {
892 : // If there's more than 1cm difference then slowly slew to it via LPF.
893 : // The EKF does not like step inputs so this keeps it happy.
894 0 : _alt_offset_active = (0.98f*_alt_offset_active) + (0.02f*_alt_offset);
895 : } else {
896 1535898 : _alt_offset_active = _alt_offset;
897 : }
898 :
899 : #if HAL_LOGGING_ENABLED
900 1535898 : bool old_primary_healthy = sensors[_primary].healthy;
901 : #endif
902 :
903 4622391 : for (uint8_t i=0; i<_num_drivers; i++) {
904 3086493 : drivers[i]->backend_update(i);
905 : }
906 :
907 : #if AP_BARO_THST_COMP_ENABLED
908 : update_thrust_filter();
909 : #endif
910 :
911 4622391 : for (uint8_t i=0; i<_num_sensors; i++) {
912 3086493 : if (sensors[i].healthy) {
913 : // update altitude calculation
914 3084790 : float ground_pressure = sensors[i].ground_pressure;
915 3084790 : if (!is_positive(ground_pressure) || isnan(ground_pressure) || isinf(ground_pressure)) {
916 749 : sensors[i].ground_pressure.set(sensors[i].pressure);
917 : }
918 3084790 : float altitude = sensors[i].altitude;
919 3084790 : float corrected_pressure = sensors[i].pressure + sensors[i].p_correction;
920 3084790 : if (sensors[i].type == BARO_TYPE_AIR) {
921 : #if HAL_BARO_WIND_COMP_ENABLED
922 2973723 : corrected_pressure -= wind_pressure_correction(i);
923 : #endif
924 : #if AP_BARO_THST_COMP_ENABLED
925 : corrected_pressure -= thrust_pressure_correction(i);
926 : #endif
927 : #if (HAL_BARO_WIND_COMP_ENABLED || AP_BARO_THST_COMP_ENABLED)
928 2973723 : sensors[i].corrected_pressure = corrected_pressure;
929 : #endif
930 2973723 : altitude = get_altitude_difference(sensors[i].ground_pressure, corrected_pressure);
931 :
932 : // the ground pressure is references against the field elevation
933 2973723 : altitude -= _field_elevation_active;
934 111067 : } else if (sensors[i].type == BARO_TYPE_WATER) {
935 : //101325Pa is sea level air pressure, 9800 Pascal/ m depth in water.
936 : //No temperature or depth compensation for density of water.
937 111067 : altitude = (sensors[i].ground_pressure - corrected_pressure) / 9800.0f / _specific_gravity;
938 : }
939 : // sanity check altitude
940 3084790 : sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
941 3084790 : if (sensors[i].alt_ok) {
942 3084790 : sensors[i].altitude = altitude + _alt_offset_active;
943 : }
944 : }
945 : }
946 :
947 : // ensure the climb rate filter is updated
948 1535898 : if (healthy()) {
949 1534969 : _climb_rate_filter.update(get_altitude(), get_last_update());
950 : }
951 :
952 : // choose primary sensor
953 1535898 : if (_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)) {
954 1534409 : _primary = _primary_baro;
955 : } else {
956 1489 : _primary = 0;
957 3177 : for (uint8_t i=0; i<_num_sensors; i++) {
958 2250 : if (healthy(i)) {
959 562 : _primary = i;
960 562 : break;
961 : }
962 : }
963 : }
964 : #if AP_FIELD_ELEVATION_ENABLED
965 1480032 : update_field_elevation();
966 : #endif
967 :
968 : // update the cached EAS2TAS value, a function of the primary
969 : // baro's altitude:
970 1535898 : _EAS2TAS = _get_EAS2TAS();
971 :
972 : // logging
973 : #if HAL_LOGGING_ENABLED
974 1535898 : if (should_log()) {
975 1467414 : Write_Baro();
976 : }
977 :
978 : #define MASK_LOG_ANY 0xFFFF
979 :
980 : // log sensor healthy state change:
981 1535898 : if (sensors[_primary].healthy != old_primary_healthy) {
982 2091 : if (AP::logger().should_log(MASK_LOG_ANY)) {
983 1673 : const LogErrorCode code = sensors[_primary].healthy ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY;
984 1673 : AP::logger().Write_Error(LogErrorSubsystem::BARO, code);
985 : }
986 : }
987 : #endif
988 1535898 : }
989 :
990 : #ifdef HAL_BUILD_AP_PERIPH
991 : // calibration and alt check not valid for AP_Periph
992 : bool AP_Baro::healthy(uint8_t instance) const {
993 : // If the requested instance was outside max instances it is not healthy (it doesn't exist)
994 : if (instance >= BARO_MAX_INSTANCES) {
995 : return false;
996 : }
997 : return sensors[instance].healthy;
998 : }
999 : #else
1000 96102009 : bool AP_Baro::healthy(uint8_t instance) const {
1001 : // If the requested instance was outside max instances it is not healthy (it doesn't exist)
1002 96102009 : if (instance >= BARO_MAX_INSTANCES) {
1003 0 : return false;
1004 : }
1005 96102009 : return sensors[instance].healthy && sensors[instance].alt_ok && sensors[instance].calibrated;
1006 : }
1007 : #endif
1008 :
1009 : /*
1010 : update field elevation value
1011 : */
1012 1480032 : void AP_Baro::update_field_elevation(void)
1013 : {
1014 : #if AP_FIELD_ELEVATION_ENABLED
1015 1480032 : const uint32_t now_ms = AP_HAL::millis();
1016 1480032 : bool new_field_elev = false;
1017 1480032 : const bool armed = hal.util->get_soft_armed();
1018 1480032 : if (now_ms - _field_elevation_last_ms >= 1000) {
1019 1667684 : if (is_zero(_field_elevation_active) &&
1020 210859 : is_zero(_field_elevation)) {
1021 : // auto-set based on origin
1022 210859 : Location origin;
1023 210859 : if (!armed && AP::ahrs().get_origin(origin)) {
1024 923 : _field_elevation_active = origin.alt * 0.01;
1025 923 : if (is_zero(_field_elevation_active)) {
1026 0 : _field_elevation_active = 0.001f; // prevent zero value so that we don't keep trying to auto-set
1027 : }
1028 923 : new_field_elev = true;
1029 : }
1030 2491692 : } else if (fabsf(_field_elevation_active-_field_elevation) > 1.0 &&
1031 1245726 : !is_zero(_field_elevation)) {
1032 : // user has set field elevation
1033 0 : if (!armed) {
1034 0 : _field_elevation_active = _field_elevation;
1035 0 : new_field_elev = true;
1036 : } else {
1037 0 : _field_elevation.set(_field_elevation_active);
1038 0 : _field_elevation.notify();
1039 0 : GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Failed to Set Field Elevation: Armed");
1040 : }
1041 : }
1042 : }
1043 1480032 : if (new_field_elev && !armed) {
1044 923 : _field_elevation_last_ms = now_ms;
1045 923 : AP::ahrs().resetHeightDatum();
1046 923 : update_calibration();
1047 923 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Field Elevation Set: %.0fm", _field_elevation_active);
1048 : }
1049 : #endif
1050 1480032 : }
1051 :
1052 : #if AP_BARO_THST_COMP_ENABLED
1053 : // update filtered throttle value once per update cycle
1054 : void AP_Baro::update_thrust_filter(void)
1055 : {
1056 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_COPTER_OR_HELI
1057 : const AP_Motors* motors = AP::motors();
1058 : if (motors == nullptr) {
1059 : _filtered_throttle = 0.0f;
1060 : return;
1061 : }
1062 : const float motors_throttle = MAX(0, motors->get_throttle_out());
1063 :
1064 : if (_thst_filt_cutoff.get() <= 0) {
1065 : _filtered_throttle = motors_throttle;
1066 : return;
1067 : }
1068 :
1069 : const uint32_t now_us = AP_HAL::micros();
1070 : const float dt = (now_us - _thrust_filter_last_update_us) * 1.0e-6f;
1071 : _thrust_filter_last_update_us = now_us;
1072 : _filtered_throttle = _thrust_filter.apply(motors_throttle, dt);
1073 : #else
1074 : _filtered_throttle = 0.0f;
1075 : #endif
1076 : }
1077 :
1078 : // scale the baro linearly with thrust using pre-filtered throttle
1079 : float AP_Baro::thrust_pressure_correction(uint8_t instance)
1080 : {
1081 : return sensors[instance].mot_scale * _filtered_throttle;
1082 : }
1083 : #endif
1084 :
1085 : /* register a new sensor, claiming a sensor slot. If we are out of
1086 : slots it will panic
1087 : */
1088 4237 : uint8_t AP_Baro::register_sensor(void)
1089 : {
1090 4237 : if (_num_sensors >= ARRAY_SIZE(sensors)) {
1091 0 : AP_HAL::panic("Too many barometers");
1092 : }
1093 4237 : return _num_sensors++;
1094 : }
1095 :
1096 :
1097 : /*
1098 : check if all barometers are healthy
1099 : */
1100 1254034 : bool AP_Baro::all_healthy(void) const
1101 : {
1102 3841693 : for (uint8_t i=0; i<_num_sensors; i++) {
1103 2588581 : if (!healthy(i)) {
1104 922 : return false;
1105 : }
1106 : }
1107 1253112 : return _num_sensors > 0;
1108 : }
1109 :
1110 : // set a pressure correction from AP_TempCalibration
1111 0 : void AP_Baro::set_pressure_correction(uint8_t instance, float p_correction)
1112 : {
1113 0 : if (instance < _num_sensors) {
1114 0 : sensors[instance].p_correction = p_correction;
1115 : }
1116 0 : }
1117 :
1118 : #if AP_BARO_MSP_ENABLED
1119 : /*
1120 : handle MSP barometer data
1121 : */
1122 0 : void AP_Baro::handle_msp(const MSP::msp_baro_data_message_t &pkt)
1123 : {
1124 0 : if (pkt.instance > 7) {
1125 0 : return;
1126 : }
1127 0 : if (!init_done) {
1128 0 : msp_instance_mask |= 1U<<pkt.instance;
1129 0 : } else if (msp_instance_mask != 0) {
1130 0 : for (uint8_t i=0; i<_num_drivers; i++) {
1131 0 : drivers[i]->handle_msp(pkt);
1132 : }
1133 : }
1134 : }
1135 : #endif
1136 :
1137 : #if AP_BARO_EXTERNALAHRS_ENABLED
1138 : /*
1139 : handle ExternalAHRS barometer data
1140 : */
1141 131684 : void AP_Baro::handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt)
1142 : {
1143 517361 : for (uint8_t i=0; i<_num_drivers; i++) {
1144 385677 : drivers[i]->handle_external(pkt);
1145 : }
1146 131684 : }
1147 : #endif // AP_BARO_EXTERNALAHRS_ENABLED
1148 :
1149 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
1150 133080 : bool AP_Baro::arming_checks(size_t buflen, char *buffer) const
1151 : {
1152 133080 : if (!all_healthy()) {
1153 34 : hal.util->snprintf(buffer, buflen, "not healthy");
1154 34 : return false;
1155 : }
1156 :
1157 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1158 : /*
1159 : check for a pressure altitude discrepancy between GPS alt and
1160 : baro alt this catches bad barometers, such as when a MS5607 has
1161 : been substituted for a MS5611
1162 : */
1163 116118 : const auto &gps = AP::gps();
1164 116118 : if (_alt_error_max > 0 && gps.status() >= AP_GPS_FixType::FIX_3D) {
1165 104915 : const float alt_amsl = gps.location().alt*0.01;
1166 : // note the addition of _field_elevation_active as this is subtracted in get_altitude_difference()
1167 104915 : const float alt_pressure = get_altitude_difference(SSL_AIR_PRESSURE, get_pressure());
1168 104915 : const float error = fabsf(alt_amsl - alt_pressure);
1169 104915 : if (error > _alt_error_max) {
1170 0 : hal.util->snprintf(buffer, buflen, "GPS alt error %.0fm (see BARO_ALTERR_MAX)", error);
1171 0 : return false;
1172 : }
1173 : }
1174 : #endif
1175 133046 : return true;
1176 : }
1177 :
1178 : namespace AP {
1179 :
1180 221978768 : AP_Baro &baro()
1181 : {
1182 221978768 : return *AP_Baro::get_singleton();
1183 : }
1184 :
1185 : };
|
