@ -29,6 +29,7 @@
@@ -29,6 +29,7 @@
# include <AP_BoardConfig/AP_BoardConfig.h>
# include <AP_CANManager/AP_CANManager.h>
# include <AP_Vehicle/AP_Vehicle_Type.h>
# include <AP_HAL/I2CDevice.h>
# include "AP_Baro_SITL.h"
# include "AP_Baro_BMP085.h"
@ -42,9 +43,7 @@
@@ -42,9 +43,7 @@
# include "AP_Baro_DPS280.h"
# include "AP_Baro_BMP388.h"
# include "AP_Baro_Dummy.h"
# if HAL_ENABLE_LIBUAVCAN_DRIVERS
# include "AP_Baro_UAVCAN.h"
# endif
# include "AP_Baro_MSP.h"
# include "AP_Baro_ExternalAHRS.h"
@ -169,7 +168,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
@@ -169,7 +168,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
// @Increment: 1
AP_GROUPINFO ( " _FLTR_RNG " , 13 , AP_Baro , _filter_range , HAL_BARO_FILTER_DEFAULT ) ,
# if defined(HAL_PROBE_EXTERNAL_I2C_BAROS) || defined(H AL_MS P_BARO_ENABLED)
# if defined(HAL_PROBE_EXTERNAL_I2C_BAROS) || defined(AP_BARO_MSP _ENABLED)
// @Param: _PROBE_EXT
// @DisplayName: External barometers to probe
// @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 GND_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 GND_EXT_BUS.
@ -545,14 +544,14 @@ void AP_Baro::init(void)
@@ -545,14 +544,14 @@ void AP_Baro::init(void)
}
# endif
# if HAL_ENABLE_LIBUAVCAN_DRIVERS && !AP_SIM_BARO _ENABLED
# if AP_BARO_UAVCAN _ENABLED
// Detect UAVCAN Modules, try as many times as there are driver slots
for ( uint8_t i = 0 ; i < BARO_MAX_DRIVERS ; i + + ) {
ADD_BACKEND ( AP_Baro_UAVCAN : : probe ( * this ) ) ;
}
# endif
# if HAL_EXTERNAL_ AHRS_ENABLED
# if AP_BARO_EXTERNAL AHRS_ENABLED
const int8_t serial_port = AP : : externalAHRS ( ) . get_port ( ) ;
if ( serial_port > = 0 ) {
ADD_BACKEND ( new AP_Baro_ExternalAHRS ( * this , serial_port ) ) ;
@ -563,7 +562,7 @@ void AP_Baro::init(void)
@@ -563,7 +562,7 @@ void AP_Baro::init(void)
# define GET_I2C_DEVICE(bus, address) hal.i2c_mgr->get_device(bus, address)
# if AP_SIM_BARO_ENABLED
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL && AP_BARO_MS56XX_ENABLED
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( GET_I2C_DEVICE ( _ext_bus , HAL_BARO_MS5611_I2C_ADDR ) ) ) ) ;
# endif
@ -577,7 +576,7 @@ void AP_Baro::init(void)
@@ -577,7 +576,7 @@ void AP_Baro::init(void)
# elif AP_FEATURE_BOARD_DETECT
switch ( AP_BoardConfig : : get_board_type ( ) ) {
case AP_BoardConfig : : PX4_BOARD_PX4V1 :
# ifdef HAL_BARO_MS5611_I2C_BUS
# if AP_BARO_MS56XX_ENABLED && defined(HAL_BARO_MS5611_I2C_BUS)
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( GET_I2C_DEVICE ( HAL_BARO_MS5611_I2C_BUS , HAL_BARO_MS5611_I2C_ADDR ) ) ) ) ;
# endif
@ -588,32 +587,41 @@ void AP_Baro::init(void)
@@ -588,32 +587,41 @@ void AP_Baro::init(void)
case AP_BoardConfig : : PX4_BOARD_AUAV21 :
case AP_BoardConfig : : PX4_BOARD_PH2SLIM :
case AP_BoardConfig : : PX4_BOARD_PIXHAWK_PRO :
# if AP_BARO_MS56XX_ENABLED
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_NAME ) ) ) ) ;
# endif
break ;
case AP_BoardConfig : : PX4_BOARD_PIXHAWK2 :
case AP_BoardConfig : : PX4_BOARD_SP01 :
# if AP_BARO_MS56XX_ENABLED
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 ) ) ) ) ;
# endif
break ;
case AP_BoardConfig : : PX4_BOARD_MINDPXV2 :
# if AP_BARO_MS56XX_ENABLED
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_NAME ) ) ) ) ;
# endif
break ;
case AP_BoardConfig : : PX4_BOARD_AEROFC :
# if AP_BARO_MS56XX_ENABLED
# ifdef HAL_BARO_MS5607_I2C_BUS
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( GET_I2C_DEVICE ( HAL_BARO_MS5607_I2C_BUS , HAL_BARO_MS5607_I2C_ADDR ) ) ,
AP_Baro_MS56XX : : BARO_MS5607 ) ) ;
# endif
# endif // AP_BARO_MS56XX_ENABLED
break ;
case AP_BoardConfig : : VRX_BOARD_BRAIN54 :
# if AP_BARO_MS56XX_ENABLED
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_NAME ) ) ) ) ;
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
@ -622,6 +630,7 @@ void AP_Baro::init(void)
@@ -622,6 +630,7 @@ void AP_Baro::init(void)
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_SPI_IMU_NAME ) ) ) ) ;
# endif
# endif // AP_BARO_MS56XX_ENABLED
break ;
case AP_BoardConfig : : VRX_BOARD_BRAIN51 :
@ -630,20 +639,26 @@ void AP_Baro::init(void)
@@ -630,20 +639,26 @@ void AP_Baro::init(void)
case AP_BoardConfig : : VRX_BOARD_CORE10 :
case AP_BoardConfig : : VRX_BOARD_UBRAIN51 :
case AP_BoardConfig : : VRX_BOARD_UBRAIN52 :
# if AP_BARO_MS56XX_ENABLED
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_NAME ) ) ) ) ;
# endif // AP_BARO_MS56XX_ENABLED
break ;
case AP_BoardConfig : : PX4_BOARD_PCNC1 :
# if AP_BARO_ICM20789_ENABLED
ADD_BACKEND ( AP_Baro_ICM20789 : : probe ( * this ,
std : : move ( GET_I2C_DEVICE ( 1 , 0x63 ) ) ,
std : : move ( hal . spi - > get_device ( HAL_INS_MPU60x0_NAME ) ) ) ) ;
# endif
break ;
case AP_BoardConfig : : PX4_BOARD_FMUV5 :
case AP_BoardConfig : : PX4_BOARD_FMUV6 :
# if AP_BARO_MS56XX_ENABLED
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( hal . spi - > get_device ( HAL_BARO_MS5611_NAME ) ) ) ) ;
# endif
break ;
default :
@ -666,14 +681,19 @@ void AP_Baro::init(void)
@@ -666,14 +681,19 @@ void AP_Baro::init(void)
// can optionally have baro on I2C too
if ( _ext_bus > = 0 ) {
# if APM_BUILD_TYPE(APM_BUILD_ArduSub)
# if AP_BARO_MS56XX_ENABLED
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( GET_I2C_DEVICE ( _ext_bus , HAL_BARO_MS5837_I2C_ADDR ) ) , AP_Baro_MS56XX : : BARO_MS5837 ) ) ;
# endif
# if AP_BARO_KELLERLD_ENABLED
ADD_BACKEND ( AP_Baro_KellerLD : : probe ( * this ,
std : : move ( GET_I2C_DEVICE ( _ext_bus , HAL_BARO_KELLERLD_I2C_ADDR ) ) ) ) ;
# endif
# else
# if AP_BARO_MS56XX_ENABLED
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
std : : move ( GET_I2C_DEVICE ( _ext_bus , HAL_BARO_MS5611_I2C_ADDR ) ) ) ) ;
# endif
# endif
}
@ -681,7 +701,7 @@ void AP_Baro::init(void)
@@ -681,7 +701,7 @@ void AP_Baro::init(void)
_probe_i2c_barometers ( ) ;
# endif
# if H AL_MS P_BARO_ENABLED
# if AP_BARO_MSP _ENABLED
if ( ( _baro_probe_ext . get ( ) & PROBE_MSP ) & & msp_instance_mask = = 0 ) {
// allow for late addition of MSP sensor
msp_instance_mask | = 1 ;
@ -720,6 +740,7 @@ void AP_Baro::init(void)
@@ -720,6 +740,7 @@ void AP_Baro::init(void)
void AP_Baro : : _probe_i2c_barometers ( void )
{
uint32_t probe = _baro_probe_ext . get ( ) ;
( void ) probe ; // may be unused if most baros compiled out
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
@ -731,12 +752,15 @@ void AP_Baro::_probe_i2c_barometers(void)
@@ -731,12 +752,15 @@ void AP_Baro::_probe_i2c_barometers(void)
if ( ext_bus > = 0 ) {
mask = 1U < < ( uint8_t ) ext_bus ;
}
# if AP_BARO_BMP085_ENABLED
if ( probe & PROBE_BMP085 ) {
FOREACH_I2C_MASK ( i , mask ) {
ADD_BACKEND ( AP_Baro_BMP085 : : probe ( * this ,
std : : move ( GET_I2C_DEVICE ( i , HAL_BARO_BMP085_I2C_ADDR ) ) ) ) ;
}
}
# endif
# if AP_BARO_BMP280_ENABLED
if ( probe & PROBE_BMP280 ) {
FOREACH_I2C_MASK ( i , mask ) {
ADD_BACKEND ( AP_Baro_BMP280 : : probe ( * this ,
@ -745,6 +769,8 @@ void AP_Baro::_probe_i2c_barometers(void)
@@ -745,6 +769,8 @@ void AP_Baro::_probe_i2c_barometers(void)
std : : move ( GET_I2C_DEVICE ( i , HAL_BARO_BMP280_I2C_ADDR2 ) ) ) ) ;
}
}
# endif
# if AP_BARO_SPL06_ENABLED
if ( probe & PROBE_SPL06 ) {
FOREACH_I2C_MASK ( i , mask ) {
ADD_BACKEND ( AP_Baro_SPL06 : : probe ( * this ,
@ -753,6 +779,8 @@ void AP_Baro::_probe_i2c_barometers(void)
@@ -753,6 +779,8 @@ void AP_Baro::_probe_i2c_barometers(void)
std : : move ( GET_I2C_DEVICE ( i , HAL_BARO_SPL06_I2C_ADDR2 ) ) ) ) ;
}
}
# endif
# if AP_BARO_BMP388_ENABLED
if ( probe & PROBE_BMP388 ) {
FOREACH_I2C_MASK ( i , mask ) {
ADD_BACKEND ( AP_Baro_BMP388 : : probe ( * this ,
@ -761,6 +789,8 @@ void AP_Baro::_probe_i2c_barometers(void)
@@ -761,6 +789,8 @@ void AP_Baro::_probe_i2c_barometers(void)
std : : move ( GET_I2C_DEVICE ( i , HAL_BARO_BMP388_I2C_ADDR2 ) ) ) ) ;
}
}
# endif
# if AP_BARO_MS56XX_ENABLED
if ( probe & PROBE_MS5611 ) {
FOREACH_I2C_MASK ( i , mask ) {
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
@ -783,6 +813,8 @@ void AP_Baro::_probe_i2c_barometers(void)
@@ -783,6 +813,8 @@ void AP_Baro::_probe_i2c_barometers(void)
AP_Baro_MS56XX : : BARO_MS5637 ) ) ;
}
}
# endif // AP_BARO_MS56XX_ENABLED
# if AP_BARO_FBM320_ENABLED
if ( probe & PROBE_FBM320 ) {
FOREACH_I2C_MASK ( i , mask ) {
ADD_BACKEND ( AP_Baro_FBM320 : : probe ( * this ,
@ -791,6 +823,8 @@ void AP_Baro::_probe_i2c_barometers(void)
@@ -791,6 +823,8 @@ void AP_Baro::_probe_i2c_barometers(void)
std : : move ( GET_I2C_DEVICE ( i , HAL_BARO_FBM320_I2C_ADDR2 ) ) ) ) ;
}
}
# endif
# if AP_BARO_DPS280_ENABLED
if ( probe & PROBE_DPS280 ) {
FOREACH_I2C_MASK ( i , mask ) {
ADD_BACKEND ( AP_Baro_DPS280 : : probe ( * this ,
@ -799,19 +833,25 @@ void AP_Baro::_probe_i2c_barometers(void)
@@ -799,19 +833,25 @@ void AP_Baro::_probe_i2c_barometers(void)
std : : move ( GET_I2C_DEVICE ( i , HAL_BARO_DPS280_I2C_ADDR2 ) ) ) ) ;
}
}
# endif
# if AP_BARO_LPS2XH_ENABLED
if ( probe & PROBE_LPS25H ) {
FOREACH_I2C_MASK ( i , mask ) {
ADD_BACKEND ( AP_Baro_LPS2XH : : probe ( * this ,
std : : move ( GET_I2C_DEVICE ( i , HAL_BARO_LPS25H_I2C_ADDR ) ) ) ) ;
}
}
# endif
# if APM_BUILD_TYPE(APM_BUILD_ArduSub)
# if AP_BARO_KELLERLD_ENABLED
if ( probe & PROBE_KELLER ) {
FOREACH_I2C_MASK ( i , mask ) {
ADD_BACKEND ( AP_Baro_KellerLD : : probe ( * this ,
std : : move ( GET_I2C_DEVICE ( i , HAL_BARO_KELLERLD_I2C_ADDR ) ) ) ) ;
}
}
# endif
# if AP_BARO_MS56XX_ENABLED
if ( probe & PROBE_MS5837 ) {
FOREACH_I2C_MASK ( i , mask ) {
ADD_BACKEND ( AP_Baro_MS56XX : : probe ( * this ,
@ -819,6 +859,7 @@ void AP_Baro::_probe_i2c_barometers(void)
@@ -819,6 +859,7 @@ void AP_Baro::_probe_i2c_barometers(void)
}
}
# endif
# endif
}
bool AP_Baro : : should_log ( ) const
@ -966,7 +1007,7 @@ void AP_Baro::set_pressure_correction(uint8_t instance, float p_correction)
@@ -966,7 +1007,7 @@ void AP_Baro::set_pressure_correction(uint8_t instance, float p_correction)
}
}
# if H AL_MS P_BARO_ENABLED
# if AP_BARO_MSP _ENABLED
/*
handle MSP barometer data
*/
@ -983,9 +1024,9 @@ void AP_Baro::handle_msp(const MSP::msp_baro_data_message_t &pkt)
@@ -983,9 +1024,9 @@ void AP_Baro::handle_msp(const MSP::msp_baro_data_message_t &pkt)
}
}
}
# endif
# endif
# if HAL_EXTERNAL_ AHRS_ENABLED
# if AP_BARO_EXTERNAL AHRS_ENABLED
/*
handle ExternalAHRS barometer data
*/
@ -995,7 +1036,7 @@ void AP_Baro::handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt)
@@ -995,7 +1036,7 @@ void AP_Baro::handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt)
drivers [ i ] - > handle_external ( pkt ) ;
}
}
# endif
# endif // AP_BARO_EXTERNALAHRS_ENABLED
namespace AP {