2013-08-29 02:34:34 -03:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
2012-06-19 23:25:19 -03:00
/*
2012-08-17 03:09:23 -03:00
* APM_Baro . cpp - barometer driver
*
*/
2015-12-01 12:07:15 -04:00
# include "AP_Baro.h"
2012-06-19 23:25:19 -03:00
2016-01-04 09:59:19 -04:00
# include <utility>
2018-03-12 04:58:10 -03:00
# include <stdio.h>
2016-01-04 09:59:19 -04:00
2018-03-18 01:13:37 -03:00
# include <GCS_MAVLink/GCS.h>
2015-08-11 03:28:42 -03:00
# include <AP_Common/AP_Common.h>
# include <AP_HAL/AP_HAL.h>
2015-12-01 12:07:15 -04:00
# include <AP_Math/AP_Math.h>
2016-11-01 07:29:18 -03:00
# include <AP_BoardConfig/AP_BoardConfig.h>
2020-05-31 09:02:33 -03:00
# include <AP_CANManager/AP_CANManager.h>
2017-02-03 00:18:25 -04:00
# include <AP_Vehicle/AP_Vehicle_Type.h>
2022-01-26 23:58:56 -04:00
# include <AP_HAL/I2CDevice.h>
2015-12-01 12:07:15 -04:00
2017-04-12 08:17:19 -03:00
# include "AP_Baro_SITL.h"
2015-12-01 12:07:15 -04:00
# include "AP_Baro_BMP085.h"
2016-12-29 10:45:02 -04:00
# include "AP_Baro_BMP280.h"
2019-10-05 19:59:36 -03:00
# include "AP_Baro_SPL06.h"
2017-06-16 21:45:17 -03:00
# include "AP_Baro_KellerLD.h"
2015-12-01 12:07:15 -04:00
# include "AP_Baro_MS5611.h"
2017-03-09 19:39:17 -04:00
# include "AP_Baro_ICM20789.h"
2018-02-06 06:39:55 -04:00
# include "AP_Baro_LPS2XH.h"
2017-07-21 01:27:32 -03:00
# include "AP_Baro_FBM320.h"
2017-10-14 00:27:29 -03:00
# include "AP_Baro_DPS280.h"
2019-07-26 03:55:30 -03:00
# include "AP_Baro_BMP388.h"
2020-01-16 02:51:11 -04:00
# include "AP_Baro_Dummy.h"
2023-04-08 00:55:39 -03:00
# include "AP_Baro_DroneCAN.h"
2020-09-03 19:09:52 -03:00
# include "AP_Baro_MSP.h"
2020-12-27 22:05:33 -04:00
# include "AP_Baro_ExternalAHRS.h"
2022-06-01 22:53:24 -03:00
# include "AP_Baro_ICP101XX.h"
2022-06-24 03:17:22 -03:00
# include "AP_Baro_ICP201XX.h"
2012-10-11 14:53:21 -03:00
2019-08-27 17:06:27 -03:00
# include <AP_Airspeed/AP_Airspeed.h>
# include <AP_AHRS/AP_AHRS.h>
2022-01-27 15:02:53 -04:00
# include <AP_Arming/AP_Arming.h>
2019-04-04 07:45:02 -03:00
# include <AP_Logger/AP_Logger.h>
2022-10-16 07:52:18 -03:00
# include <AP_GPS/AP_GPS.h>
# include <AP_Vehicle/AP_Vehicle.h>
2019-04-04 07:45:02 -03:00
2017-03-24 01:58:53 -03:00
# define INTERNAL_TEMPERATURE_CLAMP 35.0f
2018-03-12 04:58:10 -03:00
# ifndef HAL_BARO_FILTER_DEFAULT
# define HAL_BARO_FILTER_DEFAULT 0 // turned off by default
# endif
2017-03-24 01:58:53 -03:00
2018-08-22 21:42:06 -03:00
# ifndef HAL_BARO_PROBE_EXT_DEFAULT
# define HAL_BARO_PROBE_EXT_DEFAULT 0
# endif
2021-08-16 21:18:48 -03:00
# ifndef HAL_BARO_EXTERNAL_BUS_DEFAULT
# define HAL_BARO_EXTERNAL_BUS_DEFAULT -1
# endif
2020-01-12 21:14:28 -04:00
# ifdef HAL_BUILD_AP_PERIPH
# define HAL_BARO_ALLOW_INIT_NO_BARO
# endif
2024-04-02 11:38:41 -03:00
# ifndef AP_FIELD_ELEVATION_ENABLED
# define AP_FIELD_ELEVATION_ENABLED !defined(HAL_BUILD_AP_PERIPH) && !APM_BUILD_TYPE(APM_BUILD_ArduSub)
# endif
2012-10-11 14:53:21 -03:00
extern const AP_HAL : : HAL & hal ;
2012-06-19 23:25:19 -03:00
2012-06-27 02:59:52 -03:00
// table of user settable parameters
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_Baro : : var_info [ ] = {
2012-07-06 02:11:22 -03:00
// NOTE: Index numbers 0 and 1 were for the old integer
// ground temperature and pressure
2021-04-29 02:40:28 -03:00
# ifndef HAL_BUILD_AP_PERIPH
2020-12-03 04:12:39 -04:00
// @Param: 1_GND_PRESS
// @DisplayName: Ground Pressure
2013-06-26 05:32:20 -03:00
// @Description: calibrated ground pressure in Pascals
2017-05-02 10:41:06 -03:00
// @Units: Pa
2012-08-17 03:09:23 -03:00
// @Increment: 1
2016-02-28 19:35:40 -04:00
// @ReadOnly: True
// @Volatile: True
2016-07-26 02:35:15 -03:00
// @User: Advanced
2020-12-03 04:12:39 -04:00
AP_GROUPINFO_FLAGS ( " 1_GND_PRESS " , 2 , AP_Baro , sensors [ 0 ] . ground_pressure , 0 , AP_PARAM_FLAG_INTERNAL_USE_ONLY ) ,
2012-06-27 02:59:52 -03:00
2020-12-03 04:12:39 -04:00
// @Param: _GND_TEMP
2012-08-17 03:09:23 -03:00
// @DisplayName: ground temperature
2017-07-15 18:41:11 -03:00
// @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.
2017-05-02 10:41:06 -03:00
// @Units: degC
2012-08-17 03:09:23 -03:00
// @Increment: 1
2016-02-28 19:35:40 -04:00
// @Volatile: True
2016-07-26 02:35:15 -03:00
// @User: Advanced
2020-12-03 04:12:39 -04:00
AP_GROUPINFO ( " _GND_TEMP " , 3 , AP_Baro , _user_ground_temperature , 0 ) ,
2013-06-26 05:32:20 -03:00
2015-08-25 15:36:58 -03:00
// index 4 reserved for old AP_Int8 version in legacy FRAM
//AP_GROUPINFO("ALT_OFFSET", 4, AP_Baro, _alt_offset, 0),
2020-12-03 04:12:39 -04:00
// @Param: _ALT_OFFSET
2013-07-10 01:38:57 -03:00
// @DisplayName: altitude offset
// @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.
2017-05-02 10:41:06 -03:00
// @Units: m
2015-08-25 15:36:58 -03:00
// @Increment: 0.1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2020-12-03 04:12:39 -04:00
AP_GROUPINFO ( " _ALT_OFFSET " , 5 , AP_Baro , _alt_offset , 0 ) ,
2013-07-10 01:38:57 -03:00
2020-12-03 04:12:39 -04:00
// @Param: _PRIMARY
2015-09-10 07:24:59 -03:00
// @DisplayName: Primary barometer
// @Description: This selects which barometer will be the primary if multiple barometers are found
// @Values: 0:FirstBaro,1:2ndBaro,2:3rdBaro
2016-07-26 02:35:15 -03:00
// @User: Advanced
2020-12-03 04:12:39 -04:00
AP_GROUPINFO ( " _PRIMARY " , 6 , AP_Baro , _primary_baro , 0 ) ,
2021-04-29 02:40:28 -03:00
# endif // HAL_BUILD_AP_PERIPH
2016-01-04 09:59:19 -04:00
2020-12-03 04:12:39 -04:00
// @Param: _EXT_BUS
2016-12-01 05:59:50 -04:00
// @DisplayName: External baro bus
2022-11-07 21:37:09 -04:00
// @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.
2022-11-07 21:28:02 -04:00
// @Values: -1:Disabled,0:Bus0,1:Bus1,6:Bus6
2016-12-01 05:59:50 -04:00
// @User: Advanced
2021-08-16 21:18:48 -03:00
AP_GROUPINFO ( " _EXT_BUS " , 7 , AP_Baro , _ext_bus , HAL_BARO_EXTERNAL_BUS_DEFAULT ) ,
2017-02-03 00:18:25 -04:00
2022-04-27 04:55:02 -03:00
// @Param{Sub}: _SPEC_GRAV
2017-02-03 00:18:25 -04:00
// @DisplayName: Specific Gravity (For water depth measurement)
// @Description: This sets the specific gravity of the fluid when flying an underwater ROV.
// @Values: 1.0:Freshwater,1.024:Saltwater
2020-12-03 04:12:39 -04:00
AP_GROUPINFO_FRAME ( " _SPEC_GRAV " , 8 , AP_Baro , _specific_gravity , 1.0 , AP_PARAM_FRAME_SUB ) ,
2017-02-03 00:18:25 -04:00
# if BARO_MAX_INSTANCES > 1
2020-12-03 04:12:39 -04:00
// @Param: 2_GND_PRESS
// @DisplayName: Ground Pressure
2017-02-03 00:18:25 -04:00
// @Description: calibrated ground pressure in Pascals
2017-05-02 10:41:06 -03:00
// @Units: Pa
2017-02-03 00:18:25 -04:00
// @Increment: 1
// @ReadOnly: True
// @Volatile: True
// @User: Advanced
2020-12-03 04:12:39 -04:00
AP_GROUPINFO_FLAGS ( " 2_GND_PRESS " , 9 , AP_Baro , sensors [ 1 ] . ground_pressure , 0 , AP_PARAM_FLAG_INTERNAL_USE_ONLY ) ,
2017-02-03 00:18:25 -04:00
2017-03-24 01:58:53 -03:00
// Slot 10 used to be TEMP2
2017-02-03 00:18:25 -04:00
# endif
# if BARO_MAX_INSTANCES > 2
2020-12-03 04:12:39 -04:00
// @Param: 3_GND_PRESS
2017-02-03 00:18:25 -04:00
// @DisplayName: Absolute Pressure
// @Description: calibrated ground pressure in Pascals
2017-05-02 10:41:06 -03:00
// @Units: Pa
2017-02-03 00:18:25 -04:00
// @Increment: 1
// @ReadOnly: True
// @Volatile: True
// @User: Advanced
2020-12-03 04:12:39 -04:00
AP_GROUPINFO_FLAGS ( " 3_GND_PRESS " , 11 , AP_Baro , sensors [ 2 ] . ground_pressure , 0 , AP_PARAM_FLAG_INTERNAL_USE_ONLY ) ,
2017-02-03 00:18:25 -04:00
2017-03-24 01:58:53 -03:00
// Slot 12 used to be TEMP3
2017-02-03 00:18:25 -04:00
# endif
2020-12-03 04:12:39 -04:00
// @Param: _FLTR_RNG
2018-03-12 04:58:10 -03:00
// @DisplayName: Range in which sample is accepted
// @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.
// @Units: %
// @Range: 0 100
// @Increment: 1
2020-12-03 04:12:39 -04:00
AP_GROUPINFO ( " _FLTR_RNG " , 13 , AP_Baro , _filter_range , HAL_BARO_FILTER_DEFAULT ) ,
2018-03-12 04:58:10 -03:00
2024-05-07 20:51:26 -03:00
# if AP_BARO_PROBE_EXT_PARAMETER_ENABLED
2020-12-03 04:12:39 -04:00
// @Param: _PROBE_EXT
2018-08-22 21:42:06 -03:00
// @DisplayName: External barometers to probe
2022-11-07 21:37:09 -04:00
// @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.
2020-09-03 19:09:52 -03:00
// @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
2018-08-22 21:42:06 -03:00
// @User: Advanced
2020-12-03 04:12:39 -04:00
AP_GROUPINFO ( " _PROBE_EXT " , 14 , AP_Baro , _baro_probe_ext , HAL_BARO_PROBE_EXT_DEFAULT ) ,
2018-08-22 21:42:06 -03:00
# endif
2018-07-20 06:44:55 -03:00
2020-12-03 04:12:39 -04:00
// @Param: 1_DEVID
2020-07-18 04:12:41 -03:00
// @DisplayName: Baro ID
// @Description: Barometer sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
2020-12-03 04:12:39 -04:00
AP_GROUPINFO_FLAGS ( " 1_DEVID " , 15 , AP_Baro , sensors [ 0 ] . bus_id , 0 , AP_PARAM_FLAG_INTERNAL_USE_ONLY ) ,
2020-07-18 04:12:41 -03:00
# if BARO_MAX_INSTANCES > 1
2020-12-03 04:12:39 -04:00
// @Param: 2_DEVID
2020-07-18 04:12:41 -03:00
// @DisplayName: Baro ID2
// @Description: Barometer2 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
2020-12-03 04:12:39 -04:00
AP_GROUPINFO_FLAGS ( " 2_DEVID " , 16 , AP_Baro , sensors [ 1 ] . bus_id , 0 , AP_PARAM_FLAG_INTERNAL_USE_ONLY ) ,
2020-07-18 04:12:41 -03:00
# endif
# if BARO_MAX_INSTANCES > 2
2020-12-03 04:12:39 -04:00
// @Param: 3_DEVID
2020-07-18 04:12:41 -03:00
// @DisplayName: Baro ID3
// @Description: Barometer3 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
2020-12-03 04:12:39 -04:00
AP_GROUPINFO_FLAGS ( " 3_DEVID " , 17 , AP_Baro , sensors [ 2 ] . bus_id , 0 , AP_PARAM_FLAG_INTERNAL_USE_ONLY ) ,
2020-07-18 04:12:41 -03:00
# endif
2020-11-20 17:40:58 -04:00
2020-11-23 03:58:27 -04:00
# if HAL_BARO_WIND_COMP_ENABLED
2020-12-06 16:37:08 -04:00
// @Group: 1_WCF_
2020-11-23 03:58:27 -04:00
// @Path: AP_Baro_Wind.cpp
2020-12-06 16:37:08 -04:00
AP_SUBGROUPINFO ( sensors [ 0 ] . wind_coeff , " 1_WCF_ " , 18 , AP_Baro , WindCoeff ) ,
2020-11-20 17:40:58 -04:00
2020-11-23 03:58:27 -04:00
# if BARO_MAX_INSTANCES > 1
2020-12-06 16:37:08 -04:00
// @Group: 2_WCF_
2020-11-23 03:58:27 -04:00
// @Path: AP_Baro_Wind.cpp
2020-12-06 16:37:08 -04:00
AP_SUBGROUPINFO ( sensors [ 1 ] . wind_coeff , " 2_WCF_ " , 19 , AP_Baro , WindCoeff ) ,
2020-11-23 03:58:27 -04:00
# endif
# if BARO_MAX_INSTANCES > 2
2020-12-06 16:37:08 -04:00
// @Group: 3_WCF_
2020-11-23 03:58:27 -04:00
// @Path: AP_Baro_Wind.cpp
2020-12-06 16:37:08 -04:00
AP_SUBGROUPINFO ( sensors [ 2 ] . wind_coeff , " 3_WCF_ " , 20 , AP_Baro , WindCoeff ) ,
2020-11-23 03:58:27 -04:00
# endif
2024-04-18 19:50:53 -03:00
# endif // HAL_BARO_WIND_COMP_ENABLED
2024-04-02 11:38:41 -03:00
# if AP_FIELD_ELEVATION_ENABLED
2022-01-31 18:13:10 -04:00
// @Param: _FIELD_ELV
// @DisplayName: field elevation
2022-05-28 21:03:39 -03:00
// @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.
2022-01-31 18:13:10 -04:00
// @Units: m
// @Increment: 0.1
// @Volatile: True
// @User: Advanced
AP_GROUPINFO ( " _FIELD_ELV " , 22 , AP_Baro , _field_elevation , 0 ) ,
# endif
2022-10-16 07:52:18 -03:00
# if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// @Param: _ALTERR_MAX
// @DisplayName: Altitude error maximum
// @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.
// @Units: m
// @Increment: 1
// @Range: 0 5000
// @User: Advanced
AP_GROUPINFO ( " _ALTERR_MAX " , 23 , AP_Baro , _alt_error_max , 2000 ) ,
// @Param: _OPTIONS
// @DisplayName: Barometer options
// @Description: Barometer options
// @Bitmask: 0:Treat MS5611 as MS5607
// @User: Advanced
AP_GROUPINFO ( " _OPTIONS " , 24 , AP_Baro , _options , 0 ) ,
# endif
2020-11-23 03:58:27 -04:00
AP_GROUPEND
2012-06-27 02:59:52 -03:00
} ;
2017-11-03 01:43:44 -03:00
// singleton instance
2019-02-10 14:33:05 -04:00
AP_Baro * AP_Baro : : _singleton ;
2017-11-03 01:43:44 -03:00
2021-08-18 08:42:16 -03:00
# if HAL_GCS_ENABLED
2019-07-26 03:55:52 -03:00
# define BARO_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args)
2021-08-18 08:42:16 -03:00
# else
# define BARO_SEND_TEXT(severity, format, args...)
2019-07-26 03:55:52 -03:00
# endif
2014-10-19 16:22:51 -03:00
/*
AP_Baro constructor
*/
2016-07-12 02:11:34 -03:00
AP_Baro : : AP_Baro ( )
2014-10-19 16:22:51 -03:00
{
2019-02-10 14:33:05 -04:00
_singleton = this ;
2018-07-20 06:44:55 -03:00
2014-10-19 16:22:51 -03:00
AP_Param : : setup_object_defaults ( this , var_info ) ;
2022-01-27 15:02:53 -04:00
_field_elevation_active = _field_elevation ;
2014-10-19 16:22:51 -03:00
}
2012-06-19 23:25:19 -03:00
// calibrate the barometer. This must be called at least once before
// the altitude() or climb_rate() interfaces can be used
2017-02-03 00:18:25 -04:00
void AP_Baro : : calibrate ( bool save )
2012-06-19 23:25:19 -03:00
{
2019-05-02 05:56:41 -03:00
// start by assuming all sensors are calibrated (for healthy() test)
for ( uint8_t i = 0 ; i < _num_sensors ; i + + ) {
sensors [ i ] . calibrated = true ;
sensors [ i ] . alt_ok = true ;
}
2019-04-11 06:57:28 -03:00
if ( hal . util - > was_watchdog_reset ( ) ) {
2020-05-08 13:03:07 -03:00
BARO_SEND_TEXT ( MAV_SEVERITY_INFO , " Baro: skipping calibration after WDG reset " ) ;
2019-04-11 06:57:28 -03:00
return ;
}
2021-10-29 22:15:47 -03:00
# if AP_SIM_BARO_ENABLED
2021-01-18 01:15:59 -04:00
if ( AP : : sitl ( ) - > baro_count = = 0 ) {
return ;
}
# endif
2020-05-08 13:03:07 -03:00
# ifdef HAL_BARO_ALLOW_INIT_NO_BARO
if ( _num_drivers = = 0 | | _num_sensors = = 0 | | drivers [ 0 ] = = nullptr ) {
BARO_SEND_TEXT ( MAV_SEVERITY_INFO , " Baro: no sensors found, skipping calibration " ) ;
return ;
}
# endif
2019-07-26 03:55:52 -03:00
BARO_SEND_TEXT ( MAV_SEVERITY_INFO , " Calibrating barometer " ) ;
2018-03-18 01:13:37 -03:00
2013-07-10 01:38:57 -03:00
// reset the altitude offset when we calibrate. The altitude
// offset is supposed to be for within a flight
_alt_offset . set_and_save ( 0 ) ;
2012-07-06 02:11:22 -03:00
// let the barometer settle for a full second after startup
// the MS5611 reads quite a long way off for the first second,
2012-08-17 03:09:23 -03:00
// leading to about 1m of error if we don't wait
2012-12-05 21:18:04 -04:00
for ( uint8_t i = 0 ; i < 10 ; i + + ) {
2015-11-19 23:07:59 -04:00
uint32_t tstart = AP_HAL : : millis ( ) ;
2012-08-17 03:09:23 -03:00
do {
2014-10-19 16:22:51 -03:00
update ( ) ;
2015-11-19 23:07:59 -04:00
if ( AP_HAL : : millis ( ) - tstart > 500 ) {
2019-11-06 15:44:04 -04:00
AP_BoardConfig : : config_error ( " Baro: unable to calibrate " ) ;
2012-12-05 21:18:04 -04:00
}
2015-03-12 23:48:41 -03:00
hal . scheduler - > delay ( 10 ) ;
2014-10-19 16:22:51 -03:00
} while ( ! healthy ( ) ) ;
2012-10-11 14:53:21 -03:00
hal . scheduler - > delay ( 100 ) ;
2012-07-06 02:11:22 -03:00
}
2012-06-19 23:25:19 -03:00
2018-10-11 08:48:11 -03:00
// now average over 5 values for the ground pressure settings
2014-10-19 16:22:51 -03:00
float sum_pressure [ BARO_MAX_INSTANCES ] = { 0 } ;
2015-01-08 21:59:01 -04:00
uint8_t count [ BARO_MAX_INSTANCES ] = { 0 } ;
2014-10-19 16:22:51 -03:00
const uint8_t num_samples = 5 ;
for ( uint8_t c = 0 ; c < num_samples ; c + + ) {
2015-11-19 23:07:59 -04:00
uint32_t tstart = AP_HAL : : millis ( ) ;
2012-08-17 03:09:23 -03:00
do {
2014-10-19 16:22:51 -03:00
update ( ) ;
2015-11-19 23:07:59 -04:00
if ( AP_HAL : : millis ( ) - tstart > 500 ) {
2019-11-06 15:44:04 -04:00
AP_BoardConfig : : config_error ( " Baro: unable to calibrate " ) ;
2012-12-05 21:18:04 -04:00
}
2015-01-08 21:59:01 -04:00
} while ( ! healthy ( ) ) ;
2014-10-19 16:22:51 -03:00
for ( uint8_t i = 0 ; i < _num_sensors ; i + + ) {
2015-01-08 21:59:01 -04:00
if ( healthy ( i ) ) {
sum_pressure [ i ] + = sensors [ i ] . pressure ;
count [ i ] + = 1 ;
}
2014-10-19 16:22:51 -03:00
}
2012-10-11 14:53:21 -03:00
hal . scheduler - > delay ( 100 ) ;
2012-08-17 03:09:23 -03:00
}
2014-10-19 16:22:51 -03:00
for ( uint8_t i = 0 ; i < _num_sensors ; i + + ) {
2015-01-08 21:59:01 -04:00
if ( count [ i ] = = 0 ) {
sensors [ i ] . calibrated = false ;
} else {
2017-02-03 00:18:25 -04:00
if ( save ) {
2024-04-28 03:53:12 -03:00
float p0_sealevel = get_sealevel_pressure ( sum_pressure [ i ] / count [ i ] , _field_elevation_active ) ;
2022-01-27 15:02:53 -04:00
sensors [ i ] . ground_pressure . set_and_save ( p0_sealevel ) ;
2017-02-03 00:18:25 -04:00
}
2015-01-08 21:59:01 -04:00
}
}
2017-03-24 01:58:53 -03:00
_guessed_ground_temperature = get_external_temperature ( ) ;
2015-01-08 21:59:01 -04:00
// panic if all sensors are not calibrated
2018-10-10 17:38:53 -03:00
uint8_t num_calibrated = 0 ;
2015-01-08 21:59:01 -04:00
for ( uint8_t i = 0 ; i < _num_sensors ; i + + ) {
if ( sensors [ i ] . calibrated ) {
2019-07-26 03:55:52 -03:00
BARO_SEND_TEXT ( MAV_SEVERITY_INFO , " Barometer %u calibration complete " , i + 1 ) ;
2018-10-10 17:38:53 -03:00
num_calibrated + + ;
2015-01-08 21:59:01 -04:00
}
2014-10-19 16:22:51 -03:00
}
2018-10-10 17:38:53 -03:00
if ( num_calibrated ) {
return ;
}
2020-02-14 00:16:33 -04:00
AP_BoardConfig : : config_error ( " Baro: all sensors uncalibrated " ) ;
2012-06-19 23:25:19 -03:00
}
2014-10-19 16:22:51 -03:00
/*
2013-10-05 05:44:00 -03:00
update the barometer calibration
this updates the baro ground calibration to the current values . It
can be used before arming to keep the baro well calibrated
*/
void AP_Baro : : update_calibration ( )
{
2018-10-09 23:02:48 -03:00
const uint32_t now = AP_HAL : : millis ( ) ;
const bool do_notify = now - _last_notify_ms > 10000 ;
if ( do_notify ) {
_last_notify_ms = now ;
}
2014-10-19 16:22:51 -03:00
for ( uint8_t i = 0 ; i < _num_sensors ; i + + ) {
if ( healthy ( i ) ) {
2024-04-28 03:53:12 -03:00
float corrected_pressure = get_sealevel_pressure ( get_pressure ( i ) + sensors [ i ] . p_correction , _field_elevation_active ) ;
2018-01-05 20:48:40 -04:00
sensors [ i ] . ground_pressure . set ( corrected_pressure ) ;
2014-10-19 16:22:51 -03:00
}
2015-11-08 23:34:07 -04:00
// don't notify the GCS too rapidly or we flood the link
2018-10-09 23:02:48 -03:00
if ( do_notify ) {
2015-11-08 23:34:07 -04:00
sensors [ i ] . ground_pressure . notify ( ) ;
}
2014-11-11 20:26:28 -04:00
}
2017-03-24 01:58:53 -03:00
// always update the guessed ground temp
_guessed_ground_temperature = get_external_temperature ( ) ;
2013-10-05 05:44:00 -03:00
}
2013-06-26 05:32:37 -03:00
2015-04-29 02:12:41 -03:00
// return air density / sea level density - decreases as altitude climbs
2024-04-28 03:53:12 -03:00
float AP_Baro : : _get_air_density_ratio ( void )
2015-04-28 15:15:49 -03:00
{
2024-04-28 03:53:12 -03:00
const float eas2tas = _get_EAS2TAS ( ) ;
2015-04-29 02:12:41 -03:00
if ( eas2tas > 0.0f ) {
2017-03-24 01:58:53 -03:00
return 1.0f / ( sq ( eas2tas ) ) ;
2015-04-29 02:12:41 -03:00
} else {
return 1.0f ;
}
2015-04-28 15:15:49 -03:00
}
2017-02-21 06:23:23 -04:00
// return current climb_rate estimate relative to time that calibrate()
2012-06-19 23:25:19 -03:00
// was called. Returns climb rate in meters/s, positive means up
// note that this relies on read() being called regularly to get new data
float AP_Baro : : get_climb_rate ( void )
{
2012-07-06 02:11:22 -03:00
// we use a 7 point derivative filter on the climb rate. This seems
2012-06-19 23:25:19 -03:00
// to produce somewhat reasonable results on real hardware
2013-01-10 14:42:24 -04:00
return _climb_rate_filter . slope ( ) * 1.0e3 f ;
2012-06-19 23:25:19 -03:00
}
2012-08-07 22:29:53 -03:00
2017-03-24 01:58:53 -03:00
// returns the ground temperature in degrees C, selecting either a user
// provided one, or the internal estimate
float AP_Baro : : get_ground_temperature ( void ) const
{
if ( is_zero ( _user_ground_temperature ) ) {
return _guessed_ground_temperature ;
} else {
return _user_ground_temperature ;
}
}
2014-11-11 20:26:28 -04:00
/*
set external temperature to be used for calibration ( degrees C )
*/
void AP_Baro : : set_external_temperature ( float temperature )
{
_external_temperature = temperature ;
2015-11-19 23:07:59 -04:00
_last_external_temperature_ms = AP_HAL : : millis ( ) ;
2014-11-11 20:26:28 -04:00
}
/*
get the temperature in degrees C to be used for calibration purposes
*/
2017-03-24 01:58:53 -03:00
float AP_Baro : : get_external_temperature ( const uint8_t instance ) const
2014-11-11 20:26:28 -04:00
{
// if we have a recent external temperature then use it
2015-11-19 23:07:59 -04:00
if ( _last_external_temperature_ms ! = 0 & & AP_HAL : : millis ( ) - _last_external_temperature_ms < 10000 ) {
2014-11-11 20:26:28 -04:00
return _external_temperature ;
}
2018-11-16 12:16:50 -04:00
2019-07-26 03:55:52 -03:00
# ifndef HAL_BUILD_AP_PERIPH
2021-11-01 05:09:03 -03:00
# if AP_AIRSPEED_ENABLED
2018-11-16 12:16:50 -04:00
// if we don't have an external temperature then try to use temperature
// from the airspeed sensor
AP_Airspeed * airspeed = AP_Airspeed : : get_singleton ( ) ;
if ( airspeed ! = nullptr ) {
float temperature ;
if ( airspeed - > healthy ( ) & & airspeed - > get_temperature ( temperature ) ) {
return temperature ;
}
}
2021-11-01 05:09:03 -03:00
# endif
2019-07-26 03:55:52 -03:00
# endif
2018-11-16 12:16:50 -04:00
// if we don't have an external temperature and airspeed temperature
// then use the minimum of the barometer temperature and 35 degrees C.
// The reason for not just using the baro temperature is it tends to read high,
2014-11-11 20:26:28 -04:00
// often 30 degrees above the actual temperature. That means the
2017-03-24 01:58:53 -03:00
// EAS2TAS tends to be off by quite a large margin, as well as
2023-10-11 04:41:51 -03:00
// the calculation of altitude difference between two pressures
2017-03-24 01:58:53 -03:00
// reporting a high temperature will cause the aircraft to
// estimate itself as flying higher then it actually is.
return MIN ( get_temperature ( instance ) , INTERNAL_TEMPERATURE_CLAMP ) ;
2014-11-11 20:26:28 -04:00
}
2014-10-19 16:22:51 -03:00
2016-11-24 20:59:14 -04:00
bool AP_Baro : : _add_backend ( AP_Baro_Backend * backend )
{
if ( ! backend ) {
return false ;
}
if ( _num_drivers > = BARO_MAX_DRIVERS ) {
AP_HAL : : panic ( " Too many barometer drivers " ) ;
}
drivers [ _num_drivers + + ] = backend ;
return true ;
}
2022-06-11 22:59:54 -03:00
/*
wrapper around hal . i2c_mgr - > get_device ( ) that prevents duplicate devices being opened
*/
bool AP_Baro : : _have_i2c_driver ( uint8_t bus , uint8_t address ) const
{
for ( int i = 0 ; i < _num_drivers ; + + i ) {
if ( AP_HAL : : Device : : make_bus_id ( AP_HAL : : Device : : BUS_TYPE_I2C , bus , address , 0 ) = =
AP_HAL : : Device : : change_bus_id ( uint32_t ( sensors [ i ] . bus_id . get ( ) ) , 0 ) ) {
// device already has been defined.
return true ;
}
}
return false ;
}
2016-11-24 20:59:14 -04:00
/*
macro to add a backend with check for too many sensors
We don ' t try to start more than the maximum allowed
*/
# define ADD_BACKEND(backend) \
do { _add_backend ( backend ) ; \
if ( _num_drivers = = BARO_MAX_DRIVERS | | \
_num_sensors = = BARO_MAX_INSTANCES ) { \
return ; \
} \
} while ( 0 )
2014-10-19 16:22:51 -03:00
/*
initialise the barometer object , loading backend drivers
*/
void AP_Baro : : init ( void )
{
2020-09-03 19:09:52 -03:00
init_done = true ;
2023-10-11 04:41:51 -03:00
// always set field elevation to zero on reboot in the case user
2022-01-27 15:02:53 -04:00
// fails to update. TBD automate sanity checking error bounds on
// on previously saved value at new location etc.
if ( ! is_zero ( _field_elevation ) ) {
_field_elevation . set_and_save ( 0.0f ) ;
_field_elevation . notify ( ) ;
2017-03-24 01:58:53 -03:00
}
2020-07-18 04:12:41 -03:00
// zero bus IDs before probing
for ( uint8_t i = 0 ; i < BARO_MAX_INSTANCES ; i + + ) {
sensors [ i ] . bus_id . set ( 0 ) ;
}
2021-10-29 22:15:47 -03:00
# if AP_SIM_BARO_ENABLED
SITL : : SIM * sitl = AP : : sitl ( ) ;
if ( sitl = = nullptr ) {
AP_HAL : : panic ( " No SITL pointer " ) ;
}
2023-01-03 21:44:30 -04:00
# if !AP_TEST_DRONECAN_DRIVERS
// use dronecan instances instead of SITL instances
2021-10-29 22:15:47 -03:00
for ( uint8_t i = 0 ; i < sitl - > baro_count ; i + + ) {
ADD_BACKEND ( new AP_Baro_SITL ( * this ) ) ;
}
# endif
2023-01-03 21:44:30 -04:00
# endif
2021-10-29 22:15:47 -03:00
2023-04-08 00:58:12 -03:00
# if AP_BARO_DRONECAN_ENABLED
2018-07-20 06:44:55 -03:00
// Detect UAVCAN Modules, try as many times as there are driver slots
for ( uint8_t i = 0 ; i < BARO_MAX_DRIVERS ; i + + ) {
2023-04-08 01:09:10 -03:00
ADD_BACKEND ( AP_Baro_DroneCAN : : probe ( * this ) ) ;
2018-07-20 06:44:55 -03:00
}
2017-05-06 06:13:05 -03:00
# endif
2022-01-26 23:58:56 -04:00
# if AP_BARO_EXTERNALAHRS_ENABLED
2023-01-24 00:40:17 -04:00
const int8_t serial_port = AP : : externalAHRS ( ) . get_port ( AP_ExternalAHRS : : AvailableSensor : : BARO ) ;
2021-08-10 19:30:52 -03:00
if ( serial_port > = 0 ) {
2020-12-27 22:05:33 -04:00
ADD_BACKEND ( new AP_Baro_ExternalAHRS ( * this , serial_port ) ) ;
}
# endif
2019-09-01 21:02:24 -03:00
// macro for use by HAL_INS_PROBE_LIST
2022-06-11 22:59:54 -03:00
# define GET_I2C_DEVICE(bus, address) _have_i2c_driver(bus, address)?nullptr:hal.i2c_mgr->get_device(bus, address)
2019-09-01 19:13:37 -03:00
2021-10-29 22:15:47 -03:00
# if AP_SIM_BARO_ENABLED
2022-01-26 23:58:56 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL && AP_BARO_MS56XX_ENABLED
2021-10-29 22:15:47 -03:00
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( GET_I2C_DEVICE ( _ext_bus , HAL_BARO_MS5611_I2C_ADDR ) ) ) ) ;
# endif
// do not probe for other drivers when using simulation:
return ;
# endif
2019-05-29 08:00:57 -03:00
# if defined(HAL_BARO_PROBE_LIST)
// probe list from BARO lines in hwdef.dat
HAL_BARO_PROBE_LIST ;
# elif AP_FEATURE_BOARD_DETECT
2016-12-20 08:21:20 -04:00
switch ( AP_BoardConfig : : get_board_type ( ) ) {
case AP_BoardConfig : : PX4_BOARD_PX4V1 :
2022-01-26 23:58:56 -04:00
# if AP_BARO_MS56XX_ENABLED && defined(HAL_BARO_MS5611_I2C_BUS)
2016-11-24 20:59:14 -04:00
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
2019-09-01 21:02:24 -03:00
std : : move ( GET_I2C_DEVICE ( HAL_BARO_MS5611_I2C_BUS , HAL_BARO_MS5611_I2C_ADDR ) ) ) ) ;
2016-11-08 20:34:16 -04:00
# endif
2016-12-20 08:21:20 -04:00
break ;
case AP_BoardConfig : : PX4_BOARD_PIXHAWK :
case AP_BoardConfig : : PX4_BOARD_PHMINI :
2017-02-28 21:42:15 -04:00
case AP_BoardConfig : : PX4_BOARD_AUAV21 :
2016-12-20 08:21:20 -04:00
case AP_BoardConfig : : PX4_BOARD_PH2SLIM :
2017-07-15 02:12:39 -03:00
case AP_BoardConfig : : PX4_BOARD_PIXHAWK_PRO :
2022-01-26 23:58:56 -04:00
# if AP_BARO_MS56XX_ENABLED
2016-11-24 20:59:14 -04:00
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_NAME ) ) ) ) ;
2022-01-26 23:58:56 -04:00
# endif
2016-12-20 08:21:20 -04:00
break ;
case AP_BoardConfig : : PX4_BOARD_PIXHAWK2 :
2018-02-14 01:41:00 -04:00
case AP_BoardConfig : : PX4_BOARD_SP01 :
2022-01-26 23:58:56 -04:00
# if AP_BARO_MS56XX_ENABLED
2016-11-24 20:59:14 -04:00
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_SPI_EXT_NAME ) ) ) ) ;
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_NAME ) ) ) ) ;
2022-01-26 23:58:56 -04:00
# endif
2016-12-20 08:21:20 -04:00
break ;
2018-01-11 17:26:04 -04:00
case AP_BoardConfig : : PX4_BOARD_MINDPXV2 :
2022-01-26 23:58:56 -04:00
# if AP_BARO_MS56XX_ENABLED
2018-01-11 17:26:04 -04:00
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_NAME ) ) ) ) ;
2022-01-26 23:58:56 -04:00
# endif
2018-01-11 17:26:04 -04:00
break ;
2018-07-20 06:44:55 -03:00
2017-02-03 20:52:55 -04:00
case AP_BoardConfig : : PX4_BOARD_AEROFC :
2022-01-26 23:58:56 -04:00
# if AP_BARO_MS56XX_ENABLED
2017-02-12 22:51:46 -04:00
# ifdef HAL_BARO_MS5607_I2C_BUS
2017-02-03 20:52:55 -04:00
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
2019-09-01 21:02:24 -03:00
std : : move ( GET_I2C_DEVICE ( HAL_BARO_MS5607_I2C_BUS , HAL_BARO_MS5607_I2C_ADDR ) ) ,
2017-02-03 20:52:55 -04:00
AP_Baro_MS56XX : : BARO_MS5607 ) ) ;
2017-02-12 22:51:46 -04:00
# endif
2022-01-26 23:58:56 -04:00
# endif // AP_BARO_MS56XX_ENABLED
2017-02-03 20:52:55 -04:00
break ;
2018-02-03 09:55:26 -04:00
case AP_BoardConfig : : VRX_BOARD_BRAIN54 :
2022-01-26 23:58:56 -04:00
# if AP_BARO_MS56XX_ENABLED
2018-02-03 09:55:26 -04:00
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_NAME ) ) ) ) ;
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_SPI_EXT_NAME ) ) ) ) ;
# ifdef HAL_BARO_MS5611_SPI_IMU_NAME
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_SPI_IMU_NAME ) ) ) ) ;
# endif
2022-01-26 23:58:56 -04:00
# endif // AP_BARO_MS56XX_ENABLED
2018-02-03 09:55:26 -04:00
break ;
case AP_BoardConfig : : VRX_BOARD_BRAIN51 :
case AP_BoardConfig : : VRX_BOARD_BRAIN52 :
case AP_BoardConfig : : VRX_BOARD_BRAIN52E :
case AP_BoardConfig : : VRX_BOARD_CORE10 :
case AP_BoardConfig : : VRX_BOARD_UBRAIN51 :
case AP_BoardConfig : : VRX_BOARD_UBRAIN52 :
2022-01-26 23:58:56 -04:00
# if AP_BARO_MS56XX_ENABLED
2018-02-03 09:55:26 -04:00
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_NAME ) ) ) ) ;
2022-01-26 23:58:56 -04:00
# endif // AP_BARO_MS56XX_ENABLED
2018-02-03 09:55:26 -04:00
break ;
2017-06-21 04:03:03 -03:00
case AP_BoardConfig : : PX4_BOARD_PCNC1 :
2022-01-26 23:58:56 -04:00
# if AP_BARO_ICM20789_ENABLED
2017-06-21 04:03:03 -03:00
ADD_BACKEND ( AP_Baro_ICM20789 : : probe ( * this ,
2019-09-01 21:02:24 -03:00
std : : move ( GET_I2C_DEVICE ( 1 , 0x63 ) ) ,
2018-01-07 18:57:57 -04:00
std : : move ( hal . spi - > get_device ( HAL_INS_MPU60x0_NAME ) ) ) ) ;
2022-01-26 23:58:56 -04:00
# endif
2017-06-21 04:03:03 -03:00
break ;
2018-05-29 08:43:12 -03:00
case AP_BoardConfig : : PX4_BOARD_FMUV5 :
2019-02-06 17:09:24 -04:00
case AP_BoardConfig : : PX4_BOARD_FMUV6 :
2022-01-26 23:58:56 -04:00
# if AP_BARO_MS56XX_ENABLED
2018-05-29 08:43:12 -03:00
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_NAME ) ) ) ) ;
2022-01-26 23:58:56 -04:00
# endif
2018-05-29 08:43:12 -03:00
break ;
2018-07-20 06:44:55 -03:00
2016-12-20 08:21:20 -04:00
default :
break ;
2016-11-01 07:29:18 -03:00
}
2018-02-15 00:00:23 -04:00
# elif HAL_BARO_DEFAULT == HAL_BARO_LPS25H_IMU_I2C
ADD_BACKEND ( AP_Baro_LPS2XH : : probe_InvensenseIMU ( * this ,
2019-09-01 21:02:24 -03:00
std : : move ( GET_I2C_DEVICE ( HAL_BARO_LPS25H_I2C_BUS , HAL_BARO_LPS25H_I2C_ADDR ) ) ,
2018-02-15 00:00:23 -04:00
HAL_BARO_LPS25H_I2C_IMU_ADDR ) ) ;
2018-01-19 01:06:34 -04:00
# elif HAL_BARO_DEFAULT == HAL_BARO_20789_I2C_I2C
2018-07-20 06:44:55 -03:00
ADD_BACKEND ( AP_Baro_ICM20789 : : probe ( * this ,
2019-09-01 21:02:24 -03:00
std : : move ( GET_I2C_DEVICE ( HAL_BARO_20789_I2C_BUS , HAL_BARO_20789_I2C_ADDR_PRESS ) ) ,
std : : move ( GET_I2C_DEVICE ( HAL_BARO_20789_I2C_BUS , HAL_BARO_20789_I2C_ADDR_ICM ) ) ) ) ;
2018-01-19 01:06:34 -04:00
# elif HAL_BARO_DEFAULT == HAL_BARO_20789_I2C_SPI
2018-07-20 06:44:55 -03:00
ADD_BACKEND ( AP_Baro_ICM20789 : : probe ( * this ,
2019-09-01 21:02:24 -03:00
std : : move ( GET_I2C_DEVICE ( HAL_BARO_20789_I2C_BUS , HAL_BARO_20789_I2C_ADDR_PRESS ) ) ,
2018-07-20 06:44:55 -03:00
std : : move ( hal . spi - > get_device ( " icm20789 " ) ) ) ) ;
2015-09-28 14:32:21 -03:00
# endif
2016-01-04 09:59:19 -04:00
2016-12-01 05:59:50 -04:00
// can optionally have baro on I2C too
if ( _ext_bus > = 0 ) {
2017-02-03 00:18:25 -04:00
# if APM_BUILD_TYPE(APM_BUILD_ArduSub)
2022-01-26 23:58:56 -04:00
# if AP_BARO_MS56XX_ENABLED
2017-02-03 00:18:25 -04:00
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
2019-09-01 21:02:24 -03:00
std : : move ( GET_I2C_DEVICE ( _ext_bus , HAL_BARO_MS5837_I2C_ADDR ) ) , AP_Baro_MS56XX : : BARO_MS5837 ) ) ;
2022-01-26 23:58:56 -04:00
# endif
# if AP_BARO_KELLERLD_ENABLED
2017-06-16 21:45:17 -03:00
ADD_BACKEND ( AP_Baro_KellerLD : : probe ( * this ,
2019-09-01 21:02:24 -03:00
std : : move ( GET_I2C_DEVICE ( _ext_bus , HAL_BARO_KELLERLD_I2C_ADDR ) ) ) ) ;
2022-01-26 23:58:56 -04:00
# endif
2017-02-03 00:18:25 -04:00
# else
2022-01-26 23:58:56 -04:00
# if AP_BARO_MS56XX_ENABLED
2016-12-01 05:59:50 -04:00
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
2019-09-01 21:02:24 -03:00
std : : move ( GET_I2C_DEVICE ( _ext_bus , HAL_BARO_MS5611_I2C_ADDR ) ) ) ) ;
2022-01-26 23:58:56 -04:00
# endif
2017-02-03 00:18:25 -04:00
# endif
2016-12-01 05:59:50 -04:00
}
2017-04-02 11:56:03 -03:00
2023-04-14 01:35:51 -03:00
# if AP_BARO_PROBE_EXTERNAL_I2C_BUSES
2018-08-22 21:42:06 -03:00
_probe_i2c_barometers ( ) ;
# endif
2018-07-20 06:44:55 -03:00
2022-01-26 23:58:56 -04:00
# if AP_BARO_MSP_ENABLED
2020-09-03 19:09:52 -03:00
if ( ( _baro_probe_ext . get ( ) & PROBE_MSP ) & & msp_instance_mask = = 0 ) {
// allow for late addition of MSP sensor
msp_instance_mask | = 1 ;
}
for ( uint8_t i = 0 ; i < 8 ; i + + ) {
if ( msp_instance_mask & ( 1U < < i ) ) {
ADD_BACKEND ( new AP_Baro_MSP ( * this , i ) ) ;
}
}
# endif
2019-01-20 20:48:23 -04:00
# if !defined(HAL_BARO_ALLOW_INIT_NO_BARO) // most boards requires external baro
2021-10-29 22:15:47 -03:00
# if AP_SIM_BARO_ENABLED
2021-01-18 01:15:59 -04:00
if ( sitl - > baro_count = = 0 ) {
return ;
}
# endif
2016-10-30 02:24:21 -03:00
if ( _num_drivers = = 0 | | _num_sensors = = 0 | | drivers [ 0 ] = = nullptr ) {
2019-11-06 15:44:04 -04:00
AP_BoardConfig : : config_error ( " Baro: unable to initialise driver " ) ;
2014-10-19 16:22:51 -03:00
}
2018-02-28 03:02:09 -04:00
# endif
2020-01-12 21:14:28 -04:00
# ifdef HAL_BUILD_AP_PERIPH
// AP_Periph always is set calibrated. We only want the pressure,
// so ground calibration is unnecessary
for ( uint8_t i = 0 ; i < _num_sensors ; i + + ) {
sensors [ i ] . calibrated = true ;
sensors [ i ] . alt_ok = true ;
}
# endif
2014-10-19 16:22:51 -03:00
}
2018-08-22 21:42:06 -03:00
/*
2020-12-03 04:12:39 -04:00
probe all the i2c barometers enabled with BARO_PROBE_EXT . This is
2018-08-22 21:42:06 -03:00
used on boards without a builtin barometer
*/
void AP_Baro : : _probe_i2c_barometers ( void )
{
uint32_t probe = _baro_probe_ext . get ( ) ;
2022-01-26 23:58:56 -04:00
( void ) probe ; // may be unused if most baros compiled out
2018-09-06 00:48:52 -03:00
uint32_t mask = hal . i2c_mgr - > get_bus_mask_external ( ) ;
if ( AP_BoardConfig : : get_board_type ( ) = = AP_BoardConfig : : PX4_BOARD_PIXHAWK2 ) {
// for the purpose of baro probing, treat CubeBlack internal i2c as external. It has
// no internal i2c baros, so this is safe
mask | = hal . i2c_mgr - > get_bus_mask_internal ( ) ;
}
2022-11-07 21:37:09 -04:00
// if the user has set BARO_EXT_BUS then probe the bus given by that parameter
2018-09-06 19:02:42 -03:00
int8_t ext_bus = _ext_bus ;
if ( ext_bus > = 0 ) {
mask = 1U < < ( uint8_t ) ext_bus ;
}
2022-01-28 22:36:46 -04:00
static const struct BaroProbeSpec {
uint32_t bit ;
AP_Baro_Backend * ( * probefn ) ( AP_Baro & , AP_HAL : : OwnPtr < AP_HAL : : Device > ) ;
uint8_t addr ;
} baroprobespec [ ] {
2022-01-26 23:58:56 -04:00
# if AP_BARO_BMP085_ENABLED
2022-01-28 22:36:46 -04:00
{ PROBE_BMP085 , AP_Baro_BMP085 : : probe , HAL_BARO_BMP085_I2C_ADDR } ,
2022-01-26 23:58:56 -04:00
# endif
# if AP_BARO_BMP280_ENABLED
2022-01-28 22:36:46 -04:00
{ PROBE_BMP280 , AP_Baro_BMP280 : : probe , HAL_BARO_BMP280_I2C_ADDR } ,
{ PROBE_BMP280 , AP_Baro_BMP280 : : probe , HAL_BARO_BMP280_I2C_ADDR2 } ,
2022-01-26 23:58:56 -04:00
# endif
# if AP_BARO_SPL06_ENABLED
2022-01-28 22:36:46 -04:00
{ PROBE_SPL06 , AP_Baro_SPL06 : : probe , HAL_BARO_SPL06_I2C_ADDR } ,
{ PROBE_SPL06 , AP_Baro_SPL06 : : probe , HAL_BARO_SPL06_I2C_ADDR2 } ,
2022-01-26 23:58:56 -04:00
# endif
# if AP_BARO_BMP388_ENABLED
2022-01-28 22:36:46 -04:00
{ PROBE_BMP388 , AP_Baro_BMP388 : : probe , HAL_BARO_BMP388_I2C_ADDR } ,
{ PROBE_BMP388 , AP_Baro_BMP388 : : probe , HAL_BARO_BMP388_I2C_ADDR2 } ,
2022-01-26 23:58:56 -04:00
# endif
# if AP_BARO_MS56XX_ENABLED
2022-01-28 22:36:46 -04:00
{ PROBE_MS5611 , AP_Baro_MS56XX : : probe_5611 , HAL_BARO_MS5611_I2C_ADDR } ,
{ PROBE_MS5611 , AP_Baro_MS56XX : : probe_5611 , HAL_BARO_MS5611_I2C_ADDR2 } ,
{ PROBE_MS5607 , AP_Baro_MS56XX : : probe_5607 , HAL_BARO_MS5607_I2C_ADDR } ,
{ PROBE_MS5637 , AP_Baro_MS56XX : : probe_5637 , HAL_BARO_MS5637_I2C_ADDR } ,
# endif
2022-01-26 23:58:56 -04:00
# if AP_BARO_FBM320_ENABLED
2022-01-28 22:36:46 -04:00
{ PROBE_FBM320 , AP_Baro_FBM320 : : probe , HAL_BARO_FBM320_I2C_ADDR } ,
{ PROBE_FBM320 , AP_Baro_FBM320 : : probe , HAL_BARO_FBM320_I2C_ADDR2 } ,
2022-01-26 23:58:56 -04:00
# endif
# if AP_BARO_DPS280_ENABLED
2022-01-28 22:36:46 -04:00
{ PROBE_DPS280 , AP_Baro_DPS280 : : probe_280 , HAL_BARO_DPS280_I2C_ADDR } ,
{ PROBE_DPS280 , AP_Baro_DPS280 : : probe_280 , HAL_BARO_DPS280_I2C_ADDR2 } ,
2022-01-26 23:58:56 -04:00
# endif
# if AP_BARO_LPS2XH_ENABLED
2022-01-28 22:36:46 -04:00
{ PROBE_LPS25H , AP_Baro_LPS2XH : : probe , HAL_BARO_LPS25H_I2C_ADDR } ,
2022-01-26 23:58:56 -04:00
# endif
2022-01-28 22:36:46 -04:00
2019-09-27 02:16:18 -03:00
# if APM_BUILD_TYPE(APM_BUILD_ArduSub)
2022-01-26 23:58:56 -04:00
# if AP_BARO_KELLERLD_ENABLED
2022-01-28 22:36:46 -04:00
{ PROBE_KELLER , AP_Baro_KellerLD : : probe , HAL_BARO_KELLERLD_I2C_ADDR } ,
2022-01-26 23:58:56 -04:00
# endif
# if AP_BARO_MS56XX_ENABLED
2022-01-28 22:36:46 -04:00
{ PROBE_MS5837 , AP_Baro_MS56XX : : probe_5837 , HAL_BARO_MS5837_I2C_ADDR } ,
# endif
# endif // APM_BUILD_TYPE(APM_BUILD_ArduSub)
} ;
for ( const auto & spec : baroprobespec ) {
if ( ! ( probe & spec . bit ) ) {
// not in mask to be probed for
continue ;
}
FOREACH_I2C_MASK ( i , mask ) {
ADD_BACKEND ( spec . probefn ( * this , std : : move ( GET_I2C_DEVICE ( i , spec . addr ) ) ) ) ;
2018-08-22 21:42:06 -03:00
}
}
}
2023-07-13 21:58:05 -03:00
# if HAL_LOGGING_ENABLED
2019-02-11 04:16:31 -04:00
bool AP_Baro : : should_log ( ) const
2018-04-11 09:46:46 -03:00
{
2019-02-11 04:16:31 -04:00
AP_Logger * logger = AP_Logger : : get_singleton ( ) ;
if ( logger = = nullptr ) {
2018-04-11 09:46:46 -03:00
return false ;
}
if ( _log_baro_bit = = ( uint32_t ) - 1 ) {
return false ;
}
2019-02-11 04:16:31 -04:00
if ( ! logger - > should_log ( _log_baro_bit ) ) {
2018-04-11 09:46:46 -03:00
return false ;
}
return true ;
}
2023-07-13 21:58:05 -03:00
# endif
2014-10-19 16:22:51 -03:00
/*
call update on all drivers
*/
void AP_Baro : : update ( void )
{
2019-04-25 19:43:11 -03:00
WITH_SEMAPHORE ( _rsem ) ;
2016-05-05 19:20:53 -03:00
if ( fabsf ( _alt_offset - _alt_offset_active ) > 0.01f ) {
// If there's more than 1cm difference then slowly slew to it via LPF.
// The EKF does not like step inputs so this keeps it happy.
2023-12-06 19:01:07 -04:00
_alt_offset_active = ( 0.98f * _alt_offset_active ) + ( 0.02f * _alt_offset ) ;
2016-01-30 05:43:43 -04:00
} else {
_alt_offset_active = _alt_offset ;
}
2022-04-27 07:57:42 -03:00
# if HAL_LOGGING_ENABLED
bool old_primary_healthy = sensors [ _primary ] . healthy ;
# endif
2021-06-09 08:31:34 -03:00
for ( uint8_t i = 0 ; i < _num_drivers ; i + + ) {
drivers [ i ] - > backend_update ( i ) ;
2014-10-19 16:22:51 -03:00
}
for ( uint8_t i = 0 ; i < _num_sensors ; i + + ) {
if ( sensors [ i ] . healthy ) {
// update altitude calculation
2016-02-22 23:15:39 -04:00
float ground_pressure = sensors [ i ] . ground_pressure ;
2017-07-14 20:23:57 -03:00
if ( ! is_positive ( ground_pressure ) | | isnan ( ground_pressure ) | | isinf ( ground_pressure ) ) {
2022-07-05 00:08:56 -03:00
sensors [ i ] . ground_pressure . set ( sensors [ i ] . pressure ) ;
2014-10-19 16:22:51 -03:00
}
2017-02-03 00:18:25 -04:00
float altitude = sensors [ i ] . altitude ;
2018-01-05 20:48:40 -04:00
float corrected_pressure = sensors [ i ] . pressure + sensors [ i ] . p_correction ;
2017-02-03 00:18:25 -04:00
if ( sensors [ i ] . type = = BARO_TYPE_AIR ) {
2020-11-23 03:58:27 -04:00
# if HAL_BARO_WIND_COMP_ENABLED
corrected_pressure - = wind_pressure_correction ( i ) ;
# endif
2018-01-05 20:48:40 -04:00
altitude = get_altitude_difference ( sensors [ i ] . ground_pressure , corrected_pressure ) ;
2024-04-28 03:53:12 -03:00
// the ground pressure is references against the field elevation
altitude - = _field_elevation_active ;
2017-02-03 00:18:25 -04:00
} else if ( sensors [ i ] . type = = BARO_TYPE_WATER ) {
//101325Pa is sea level air pressure, 9800 Pascal/ m depth in water.
//No temperature or depth compensation for density of water.
2018-01-05 20:48:40 -04:00
altitude = ( sensors [ i ] . ground_pressure - corrected_pressure ) / 9800.0f / _specific_gravity ;
2017-02-03 00:18:25 -04:00
}
2014-10-19 16:22:51 -03:00
// sanity check altitude
2015-08-25 15:36:58 -03:00
sensors [ i ] . alt_ok = ! ( isnan ( altitude ) | | isinf ( altitude ) ) ;
if ( sensors [ i ] . alt_ok ) {
2016-01-30 05:43:43 -04:00
sensors [ i ] . altitude = altitude + _alt_offset_active ;
2015-08-25 15:36:58 -03:00
}
2014-10-19 16:22:51 -03:00
}
}
// ensure the climb rate filter is updated
2015-07-29 02:47:51 -03:00
if ( healthy ( ) ) {
_climb_rate_filter . update ( get_altitude ( ) , get_last_update ( ) ) ;
}
2015-09-10 07:24:59 -03:00
// choose primary sensor
if ( _primary_baro > = 0 & & _primary_baro < _num_sensors & & healthy ( _primary_baro ) ) {
_primary = _primary_baro ;
} else {
_primary = 0 ;
for ( uint8_t i = 0 ; i < _num_sensors ; i + + ) {
if ( healthy ( i ) ) {
_primary = i ;
break ;
}
}
}
2024-04-02 11:38:41 -03:00
# if AP_FIELD_ELEVATION_ENABLED
2022-05-28 21:03:39 -03:00
update_field_elevation ( ) ;
2022-01-31 18:07:31 -04:00
# endif
2022-01-27 15:02:53 -04:00
2018-04-11 09:46:46 -03:00
// logging
2021-05-17 03:51:35 -03:00
# if HAL_LOGGING_ENABLED
2020-11-07 01:58:05 -04:00
if ( should_log ( ) ) {
2021-01-23 00:26:33 -04:00
Write_Baro ( ) ;
2018-04-11 09:46:46 -03:00
}
2022-04-27 07:57:42 -03:00
# define MASK_LOG_ANY 0xFFFF
// log sensor healthy state change:
if ( sensors [ _primary ] . healthy ! = old_primary_healthy ) {
if ( AP : : logger ( ) . should_log ( MASK_LOG_ANY ) ) {
const LogErrorCode code = sensors [ _primary ] . healthy ? LogErrorCode : : ERROR_RESOLVED : LogErrorCode : : UNHEALTHY ;
AP : : logger ( ) . Write_Error ( LogErrorSubsystem : : BARO , code ) ;
}
}
2019-07-26 03:55:52 -03:00
# endif
2014-10-19 16:22:51 -03:00
}
2023-05-22 20:19:49 -03:00
# ifdef HAL_BUILD_AP_PERIPH
// calibration and alt check not valid for AP_Periph
bool AP_Baro : : healthy ( uint8_t instance ) const {
// If the requested instance was outside max instances it is not healthy (it doesn't exist)
if ( instance > = BARO_MAX_INSTANCES ) {
return false ;
}
return sensors [ instance ] . healthy ;
}
# else
bool AP_Baro : : healthy ( uint8_t instance ) const {
// If the requested instance was outside max instances it is not healthy (it doesn't exist)
if ( instance > = BARO_MAX_INSTANCES ) {
return false ;
}
return sensors [ instance ] . healthy & & sensors [ instance ] . alt_ok & & sensors [ instance ] . calibrated ;
}
# endif
2022-05-28 21:03:39 -03:00
/*
update field elevation value
*/
void AP_Baro : : update_field_elevation ( void )
{
2024-04-02 11:38:41 -03:00
# if AP_FIELD_ELEVATION_ENABLED
2022-05-28 21:03:39 -03:00
const uint32_t now_ms = AP_HAL : : millis ( ) ;
bool new_field_elev = false ;
const bool armed = hal . util - > get_soft_armed ( ) ;
if ( now_ms - _field_elevation_last_ms > = 1000 ) {
if ( is_zero ( _field_elevation_active ) & &
is_zero ( _field_elevation ) ) {
// auto-set based on origin
Location origin ;
if ( ! armed & & AP : : ahrs ( ) . get_origin ( origin ) ) {
_field_elevation_active = origin . alt * 0.01 ;
new_field_elev = true ;
}
} else if ( fabsf ( _field_elevation_active - _field_elevation ) > 1.0 & &
! is_zero ( _field_elevation ) ) {
// user has set field elevation
if ( ! armed ) {
_field_elevation_active = _field_elevation ;
new_field_elev = true ;
} else {
_field_elevation . set ( _field_elevation_active ) ;
_field_elevation . notify ( ) ;
BARO_SEND_TEXT ( MAV_SEVERITY_ALERT , " Failed to Set Field Elevation: Armed " ) ;
}
}
}
if ( new_field_elev & & ! armed ) {
_field_elevation_last_ms = now_ms ;
AP : : ahrs ( ) . resetHeightDatum ( ) ;
update_calibration ( ) ;
BARO_SEND_TEXT ( MAV_SEVERITY_INFO , " Field Elevation Set: %.0fm " , _field_elevation_active ) ;
}
2024-04-02 11:38:41 -03:00
# endif
2022-05-28 21:03:39 -03:00
}
2014-10-19 16:22:51 -03:00
/* register a new sensor, claiming a sensor slot. If we are out of
slots it will panic
*/
uint8_t AP_Baro : : register_sensor ( void )
{
if ( _num_sensors > = BARO_MAX_INSTANCES ) {
2015-11-19 23:07:59 -04:00
AP_HAL : : panic ( " Too many barometers " ) ;
2014-10-19 16:22:51 -03:00
}
return _num_sensors + + ;
}
2015-01-06 21:45:01 -04:00
/*
check if all barometers are healthy
*/
bool AP_Baro : : all_healthy ( void ) const
{
for ( uint8_t i = 0 ; i < _num_sensors ; i + + ) {
if ( ! healthy ( i ) ) {
return false ;
}
}
return _num_sensors > 0 ;
}
2017-03-31 06:08:52 -03:00
// set a pressure correction from AP_TempCalibration
void AP_Baro : : set_pressure_correction ( uint8_t instance , float p_correction )
{
if ( instance < _num_sensors ) {
sensors [ instance ] . p_correction = p_correction ;
}
}
2017-05-06 06:13:05 -03:00
2022-01-26 23:58:56 -04:00
# if AP_BARO_MSP_ENABLED
2020-09-03 19:09:52 -03:00
/*
handle MSP barometer data
*/
void AP_Baro : : handle_msp ( const MSP : : msp_baro_data_message_t & pkt )
{
if ( pkt . instance > 7 ) {
return ;
}
if ( ! init_done ) {
msp_instance_mask | = 1U < < pkt . instance ;
} else if ( msp_instance_mask ! = 0 ) {
for ( uint8_t i = 0 ; i < _num_drivers ; i + + ) {
drivers [ i ] - > handle_msp ( pkt ) ;
}
}
}
2022-01-26 23:58:56 -04:00
# endif
2020-09-03 19:09:52 -03:00
2022-01-26 23:58:56 -04:00
# if AP_BARO_EXTERNALAHRS_ENABLED
2020-12-27 22:05:33 -04:00
/*
handle ExternalAHRS barometer data
*/
void AP_Baro : : handle_external ( const AP_ExternalAHRS : : baro_data_message_t & pkt )
{
for ( uint8_t i = 0 ; i < _num_drivers ; i + + ) {
drivers [ i ] - > handle_external ( pkt ) ;
}
}
2022-01-26 23:58:56 -04:00
# endif // AP_BARO_EXTERNALAHRS_ENABLED
2020-12-27 22:05:33 -04:00
2022-10-16 07:52:18 -03:00
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool AP_Baro : : arming_checks ( size_t buflen , char * buffer ) const
{
if ( ! all_healthy ( ) ) {
hal . util - > snprintf ( buffer , buflen , " not healthy " ) ;
return false ;
}
# if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
/*
check for a pressure altitude discrepancy between GPS alt and
baro alt this catches bad barometers , such as when a MS5607 has
been substituted for a MS5611
*/
const auto & gps = AP : : gps ( ) ;
if ( _alt_error_max > 0 & & gps . status ( ) > = AP_GPS : : GPS_Status : : GPS_OK_FIX_3D ) {
const float alt_amsl = gps . location ( ) . alt * 0.01 ;
2023-02-07 16:59:29 -04:00
// note the addition of _field_elevation_active as this is subtracted in get_altitude_difference()
2024-04-28 03:53:12 -03:00
const float alt_pressure = get_altitude_difference ( SSL_AIR_PRESSURE , get_pressure ( ) ) ;
2022-10-16 07:52:18 -03:00
const float error = fabsf ( alt_amsl - alt_pressure ) ;
if ( error > _alt_error_max ) {
hal . util - > snprintf ( buffer , buflen , " GPS alt error %.0fm (see BARO_ALTERR_MAX) " , error ) ;
return false ;
}
}
# endif
return true ;
}
2018-03-08 17:14:58 -04:00
namespace AP {
AP_Baro & baro ( )
{
2019-02-10 14:33:05 -04:00
return * AP_Baro : : get_singleton ( ) ;
2018-03-08 17:14:58 -04:00
}
} ;