Line data Source code
1 : #pragma once
2 :
3 : #include "AP_Baro_config.h"
4 :
5 : #include <AP_HAL/AP_HAL.h>
6 : #include <AP_Param/AP_Param.h>
7 : #include <AP_Math/AP_Math.h>
8 : #include <Filter/DerivativeFilter.h>
9 : #include <Filter/LowPassFilter.h>
10 : #include <AP_MSP/msp.h>
11 : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
12 :
13 : // maximum number of sensor instances
14 : #ifndef BARO_MAX_INSTANCES
15 : #define BARO_MAX_INSTANCES 3
16 : #endif
17 :
18 : // maximum number of drivers. Note that a single driver can provide
19 : // multiple sensor instances
20 : #define BARO_MAX_DRIVERS 3
21 :
22 : // timeouts for health reporting
23 : #define BARO_TIMEOUT_MS 500 // timeout in ms since last successful read
24 :
25 : class AP_Baro_Backend;
26 :
27 : class AP_Baro
28 : {
29 : friend class AP_Baro_Backend;
30 : friend class AP_Baro_SITL; // for access to sensors[]
31 : friend class AP_Baro_DroneCAN; // for access to sensors[]
32 :
33 : public:
34 : AP_Baro();
35 :
36 : /* Do not allow copies */
37 : CLASS_NO_COPY(AP_Baro);
38 :
39 : // get singleton
40 221978769 : static AP_Baro *get_singleton(void) {
41 221978769 : return _singleton;
42 : }
43 :
44 : // barometer types
45 : typedef enum {
46 : BARO_TYPE_AIR,
47 : BARO_TYPE_WATER
48 : } baro_type_t;
49 :
50 : // initialise the barometer object, loading backend drivers
51 : __INITFUNC__ void init(void);
52 :
53 : // update the barometer object, asking backends to push data to
54 : // the frontend
55 : void update(void);
56 :
57 : // healthy - returns true if sensor and derived altitude are good
58 6435215 : bool healthy(void) const { return healthy(_primary); }
59 :
60 : bool healthy(uint8_t instance) const;
61 :
62 : // check if all baros are healthy - used for SYS_STATUS report
63 : bool all_healthy(void) const;
64 :
65 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
66 : bool arming_checks(size_t buflen, char *buffer) const;
67 :
68 : // get primary sensor
69 41156285 : uint8_t get_primary(void) const { return _primary; }
70 :
71 : // pressure in Pascal. Divide by 100 for millibars or hectopascals
72 1414401 : float get_pressure(void) const { return get_pressure(_primary); }
73 6647350 : float get_pressure(uint8_t instance) const { return sensors[instance].pressure; }
74 : #if HAL_BARO_WIND_COMP_ENABLED
75 : // dynamic pressure in Pascal. Divide by 100 for millibars or hectopascals
76 3228 : const Vector3f& get_dynamic_pressure(uint8_t instance) const { return sensors[instance].dynamic_pressure; }
77 : #endif
78 : #if (HAL_BARO_WIND_COMP_ENABLED || AP_BARO_THST_COMP_ENABLED)
79 2949525 : float get_corrected_pressure(uint8_t instance) const { return sensors[instance].corrected_pressure; }
80 : #endif
81 :
82 : // temperature in degrees C
83 283388 : float get_temperature(void) const { return get_temperature(_primary); }
84 5499343 : float get_temperature(uint8_t instance) const { return sensors[instance].temperature; }
85 :
86 : // get pressure correction in Pascal. Divide by 100 for millibars or hectopascals
87 : float get_pressure_correction(void) const { return get_pressure_correction(_primary); }
88 : float get_pressure_correction(uint8_t instance) const { return sensors[instance].p_correction; }
89 :
90 : #if AP_BARO_CALIBRATION_ENABLED
91 : // calibrate the barometer. This must be called on startup if the
92 : // altitude/climb_rate/acceleration interfaces are ever used
93 : void calibrate(bool save=true);
94 :
95 : // update the barometer calibration to the current pressure. Can
96 : // be used for incremental preflight update of baro
97 : void update_calibration(void);
98 : #endif // AP_BARO_CALIBRATION_ENABLED
99 :
100 : // get current altitude in meters relative to altitude at the time
101 : // of the last calibrate() call
102 43218153 : float get_altitude(void) const { return get_altitude(_primary); }
103 133132389 : float get_altitude(uint8_t instance) const { return sensors[instance].altitude; }
104 :
105 : // get altitude above mean sea level
106 4485423 : float get_altitude_AMSL(uint8_t instance) const { return get_altitude(instance) + _field_elevation_active; }
107 1535898 : float get_altitude_AMSL(void) const { return get_altitude_AMSL(_primary); }
108 :
109 : // returns which i2c bus is considered "the" external bus
110 83 : uint8_t external_bus() const { return _ext_bus; }
111 :
112 : // Atmospheric Model Functions
113 : static float geometric_alt_to_geopotential(float alt);
114 : static float geopotential_alt_to_geometric(float alt);
115 :
116 : float get_temperature_from_altitude(float alt) const;
117 : float get_altitude_from_pressure(float pressure) const;
118 :
119 : // EAS2TAS for SITL
120 : static float get_EAS2TAS_for_alt_amsl(float alt_amsl);
121 :
122 : #if AP_BARO_1976_STANDARD_ATMOSPHERE_ENABLED
123 : // lookup expected pressure for a given altitude. Used for SITL backend
124 : static void get_pressure_temperature_for_alt_amsl(float alt_amsl, float &pressure, float &temperature_K);
125 : #endif
126 :
127 : // lookup expected temperature in degrees C for a given altitude. Used for SITL backend
128 : static float get_temperatureC_for_alt_amsl(const float alt_amsl);
129 :
130 : // lookup expected pressure in Pa for a given altitude. Used for SITL backend
131 : static float get_pressure_for_alt_amsl(const float alt_amsl);
132 :
133 : // get air density for SITL
134 : static float get_air_density_for_alt_amsl(float alt_amsl);
135 :
136 : // get altitude difference in meters relative given a base
137 : // pressure in Pascal
138 : float get_altitude_difference(float base_pressure, float pressure) const;
139 :
140 : // get sea level pressure relative to 1976 standard atmosphere model
141 : // pressure in Pascal
142 : float get_sealevel_pressure(float pressure, float altitude) const;
143 :
144 : // get scale factor required to convert equivalent to true
145 : // airspeed. This is a cached value, updated once per loop in
146 : // update(). You probably want AP::ahrs().get_EAS2TAS() - the
147 : // value could reasonably come from an ExternalAHRS system instead
148 : // of this library.
149 128330955 : float get_EAS2TAS(void) const { return _EAS2TAS; }
150 :
151 : // get current climb rate in meters/s. A positive number means
152 : // going up
153 : float get_climb_rate(void);
154 :
155 : // ground temperature in degrees C
156 : // the ground values are only valid after calibration
157 : float get_ground_temperature(void) const;
158 :
159 : // ground pressure in Pascal
160 : // the ground values are only valid after calibration
161 0 : float get_ground_pressure(void) const { return get_ground_pressure(_primary); }
162 0 : float get_ground_pressure(uint8_t i) const { return sensors[i].ground_pressure.get(); }
163 :
164 : // set the temperature to be used for altitude calibration. This
165 : // allows an external temperature source (such as a digital
166 : // airspeed sensor) to be used as the temperature source
167 : void set_external_temperature(float temperature);
168 :
169 : // get last time sample was taken (in ms)
170 1535228 : uint32_t get_last_update(void) const { return get_last_update(_primary); }
171 86964041 : uint32_t get_last_update(uint8_t instance) const { return sensors[instance].last_update_ms; }
172 :
173 : // settable parameters
174 : static const struct AP_Param::GroupInfo var_info[];
175 :
176 19732 : float get_external_temperature(void) const { return get_external_temperature(_primary); };
177 : float get_external_temperature(const uint8_t instance) const;
178 :
179 : // Set the primary baro
180 82 : void set_primary_baro(uint8_t primary) { _primary_baro.set_and_save(primary); };
181 :
182 : // Set the type (Air or Water) of a particular instance
183 164 : void set_type(uint8_t instance, baro_type_t type) { sensors[instance].type = type; };
184 :
185 : // Get the type (Air or Water) of a particular instance
186 82 : baro_type_t get_type(uint8_t instance) { return sensors[instance].type; };
187 :
188 : // register a new sensor, claiming a sensor slot. If we are out of
189 : // slots it will panic
190 : uint8_t register_sensor(void);
191 :
192 : // return number of registered sensors
193 45576968 : uint8_t num_instances(void) const { return _num_sensors; }
194 :
195 : // set baro drift amount
196 : void set_baro_drift_altitude(float alt) { _alt_offset.set(alt); }
197 :
198 : // get baro drift amount
199 2949525 : float get_baro_drift_offset(void) const { return _alt_offset_active; }
200 :
201 : // simple underwater atmospheric model
202 : static void SimpleUnderWaterAtmosphere(float alt, float &rho, float &delta, float &theta);
203 :
204 : // set a pressure correction from AP_TempCalibration
205 : void set_pressure_correction(uint8_t instance, float p_correction);
206 :
207 19321 : uint8_t get_filter_range() const { return _filter_range; }
208 :
209 : // indicate which bit in LOG_BITMASK indicates baro logging enabled
210 2094 : void set_log_baro_bit(uint32_t bit) { _log_baro_bit = bit; }
211 : bool should_log() const;
212 :
213 : // allow threads to lock against baro update
214 165 : HAL_Semaphore &get_semaphore(void) {
215 165 : return _rsem;
216 : }
217 :
218 : #if AP_BARO_MSP_ENABLED
219 : void handle_msp(const MSP::msp_baro_data_message_t &pkt);
220 : #endif
221 : #if AP_BARO_EXTERNALAHRS_ENABLED
222 : void handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt);
223 : #endif
224 :
225 : enum Options : uint16_t {
226 : TreatMS5611AsMS5607 = (1U << 0U),
227 : };
228 :
229 : // check if an option is set
230 83 : bool option_enabled(const Options option) const
231 : {
232 83 : return (uint16_t(_options.get()) & uint16_t(option)) != 0;
233 : }
234 :
235 : private:
236 : // singleton
237 : static AP_Baro *_singleton;
238 :
239 : // how many drivers do we have?
240 : uint8_t _num_drivers;
241 : AP_Baro_Backend *drivers[BARO_MAX_DRIVERS];
242 :
243 : // how many sensors do we have?
244 : uint8_t _num_sensors;
245 :
246 : // what is the primary sensor at the moment?
247 : uint8_t _primary;
248 :
249 : uint32_t _log_baro_bit = -1;
250 :
251 : bool init_done;
252 :
253 : uint8_t msp_instance_mask;
254 :
255 : // bitmask values for GND_PROBE_EXT
256 : enum {
257 : PROBE_BMP085=(1<<0),
258 : PROBE_BMP280=(1<<1),
259 : PROBE_MS5611=(1<<2),
260 : PROBE_MS5607=(1<<3),
261 : PROBE_MS5637=(1<<4),
262 : PROBE_FBM320=(1<<5),
263 : PROBE_DPS280=(1<<6),
264 : PROBE_LPS25H=(1<<7),
265 : PROBE_KELLER=(1<<8),
266 : PROBE_MS5837=(1<<9),
267 : PROBE_BMP388=(1<<10),
268 : PROBE_SPL06 =(1<<11),
269 : PROBE_MSP =(1<<12),
270 : PROBE_BMP581=(1<<13),
271 : PROBE_AUAV =(1<<14),
272 : };
273 :
274 : #if HAL_BARO_WIND_COMP_ENABLED
275 : class WindCoeff {
276 : public:
277 : static const struct AP_Param::GroupInfo var_info[];
278 :
279 : AP_Int8 enable; // enable compensation for this barometer
280 : AP_Float xp; // ratio of static pressure rise to dynamic pressure when flying forwards
281 : AP_Float xn; // ratio of static pressure rise to dynamic pressure when flying backwards
282 : AP_Float yp; // ratio of static pressure rise to dynamic pressure when flying to the right
283 : AP_Float yn; // ratio of static pressure rise to dynamic pressure when flying to the left
284 : AP_Float zp; // ratio of static pressure rise to dynamic pressure when flying up
285 : AP_Float zn; // ratio of static pressure rise to dynamic pressure when flying down
286 : };
287 : #endif
288 :
289 : struct {
290 : uint32_t last_update_ms; // last update time in ms
291 : uint32_t last_change_ms; // last update time in ms that included a change in reading from previous readings
292 : float pressure; // pressure in Pascal
293 : float temperature; // temperature in degrees C
294 : float altitude; // calculated altitude
295 : AP_Float ground_pressure;
296 : float p_correction;
297 : baro_type_t type; // 0 for air pressure (default), 1 for water pressure
298 : bool healthy; // true if sensor is healthy
299 : bool alt_ok; // true if calculated altitude is ok
300 : bool calibrated; // true if calculated calibrated successfully
301 : AP_Int32 bus_id;
302 : #if HAL_BARO_WIND_COMP_ENABLED
303 : WindCoeff wind_coeff;
304 : Vector3f dynamic_pressure; // calculated dynamic pressure
305 : #endif
306 : #if AP_BARO_THST_COMP_ENABLED
307 : AP_Float mot_scale; // thrust-based pressure scaling
308 : #endif
309 : #if (HAL_BARO_WIND_COMP_ENABLED || AP_BARO_THST_COMP_ENABLED)
310 : float corrected_pressure;
311 : #endif
312 : } sensors[BARO_MAX_INSTANCES];
313 :
314 : AP_Float _alt_offset;
315 : float _alt_offset_active;
316 : float _EAS2TAS = 1.0; // cached scale factor converting equivalent to true airspeed, updated in update()
317 :
318 : // calculate scale factor required to convert equivalent to true
319 : // airspeed. This is called once per loop from update() to
320 : // refresh the value returned by get_EAS2TAS():
321 : float _get_EAS2TAS(void) const;
322 :
323 : AP_Float _field_elevation; // field elevation in meters
324 : float _field_elevation_active;
325 : uint32_t _field_elevation_last_ms;
326 : AP_Int8 _primary_baro; // primary chosen by user
327 : AP_Int8 _ext_bus; // bus number for external barometer
328 : float _external_temperature;
329 : uint32_t _last_external_temperature_ms;
330 : DerivativeFilterFloat_Size7 _climb_rate_filter;
331 : AP_Float _specific_gravity; // the specific gravity of fluid for an ROV 1.00 for freshwater, 1.024 for salt water
332 : AP_Float _user_ground_temperature; // user override of the ground temperature used for EAS2TAS
333 : float _guessed_ground_temperature; // currently ground temperature estimate using our best available source
334 :
335 : // when did we last notify the GCS of new pressure reference?
336 : uint32_t _last_notify_ms;
337 :
338 : // see if we already have probed a i2c sensor by bus number and address
339 : bool _i2c_sensor_is_registered(uint8_t bus_num, uint8_t address) const;
340 : bool _add_backend(AP_Baro_Backend *backend);
341 : void _probe_i2c_barometers(void);
342 :
343 : void probe_i2c_dev(AP_Baro_Backend* (*probefn)(AP_Baro&, AP_HAL::Device&), uint8_t bus, uint8_t addr);
344 : void probe_spi_dev(AP_Baro_Backend* (*probefn)(AP_Baro&, AP_HAL::Device&), const char *name);
345 : void probe_dev(AP_Baro_Backend* (*probefn)(AP_Baro&, AP_HAL::Device&), AP_HAL::Device *dev);
346 : #if AP_BARO_ICM20789_ENABLED
347 : void probe_icm20789(uint8_t bus, uint8_t addr, const char *mpu_name);
348 : void probe_icm20789(uint8_t bus, uint8_t addr, uint8_t mpu_bus, uint8_t mpu_addr);
349 : // convenience underlying method for other probe functions;
350 : // will. delete the passed-in devices if a backend is not found
351 : void _probe_icm20789(AP_HAL::I2CDevice *i2c_dev, AP_HAL::Device *mpu_dev);
352 : #endif // AP_BARO_ICM20789_ENABLED
353 :
354 : #if AP_BARO_LPS2XH_ENABLED
355 : void probe_lps2xh_via_Invensense_IMU(uint8_t bus, uint8_t addr, uint8_t mpu_addr);
356 : #endif // AP_BARO_LPS2XH_ENABLED
357 :
358 : AP_Int8 _filter_range; // valid value range from mean value
359 : AP_Int32 _baro_probe_ext;
360 :
361 : #ifndef HAL_BUILD_AP_PERIPH
362 : AP_Float _alt_error_max;
363 : #endif
364 :
365 : AP_Int16 _options;
366 :
367 : // semaphore for API access from threads
368 : HAL_Semaphore _rsem;
369 :
370 : #if HAL_BARO_WIND_COMP_ENABLED
371 : /*
372 : return pressure correction for wind based on GND_WCOEF parameters
373 : */
374 : float wind_pressure_correction(uint8_t instance);
375 : #endif
376 : #if AP_BARO_THST_COMP_ENABLED
377 : float thrust_pressure_correction(uint8_t instance);
378 : void update_thrust_filter(void); // update filtered throttle once per update cycle
379 : LowPassFilterFloat _thrust_filter; // low-pass filter for thrust compensation
380 : uint32_t _thrust_filter_last_update_us; // last update time for filter dt calculation
381 : AP_Float _thst_filt_cutoff; // thrust filter cutoff frequency in Hz
382 : float _filtered_throttle; // filtered throttle value for thrust compensation
383 : #endif
384 : // Logging function
385 : void Write_Baro(void);
386 : void Write_Baro_instance(uint64_t time_us, uint8_t baro_instance);
387 :
388 : void update_field_elevation();
389 :
390 : // atmosphere model functions
391 : float get_altitude_difference_extended(float base_pressure, float pressure) const;
392 : float get_EAS2TAS_extended(float pressure) const;
393 : static float get_temperature_by_altitude_layer(float alt, int8_t idx);
394 :
395 : float get_altitude_difference_simple(float base_pressure, float pressure) const;
396 : float get_EAS2TAS_simple(float altitude, float pressure) const;
397 : };
398 :
399 : namespace AP {
400 : AP_Baro &baro();
401 : };
|
