Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added INS_ENABLE_MASK 

this allows for only a specified subset of IMUs to be probed, so you
can disable IMUs that aren't needed.

The back corresponds to bits in the order the IMUs are normally probed
on the board
This commit is contained in:
Andrew Tridgell 2018-01-17 18:16:48 +11:00
parent 9416165fe3
commit c0c5f23aa5
2 changed files with 81 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

156
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -423,6 +423,8 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// @DisplayName: Fast sampling mask
// @Description: Mask of IMUs to enable fast sampling on, if available
// @User: Advanced
// @Values: 1:FirstIMUOnly,3:FirstAndSecondIMU
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU
AP_GROUPINFO("FAST_SAMPLE", 36, AP_InertialSensor, _fast_sampling_mask, 0),
// @Group: NOTCH_
@ -433,6 +435,14 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// @Path: ../AP_InertialSensor/BatchSampler.cpp
AP_SUBGROUPINFO(batchsampler, "LOG_", 39, AP_InertialSensor, AP_InertialSensor::BatchSampler),
// @Group: ENABLE_MASK
// @DisplayName: IMU enable mask
// @Description: This is a bitmask of IMUs to enable. It can be used to prevent startup of specific detected IMUs
// @User: Advanced
// @Values: 1:FirstIMUOnly,3:FirstAndSecondIMU,7:FirstSecondAndThirdIMU,127:AllIMUs
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU
AP_GROUPINFO("ENABLE_MASK", 40, AP_InertialSensor, _enable_mask, 0x7F),
/*
NOTE: parameter indexes have gaps above. When adding new
parameters check for conflicts carefully
@ -596,6 +606,14 @@ void AP_InertialSensor::_start_backends()
if (_gyro_count == 0 || _accel_count == 0) {
AP_HAL::panic("INS needs at least 1 gyro and 1 accel");
}
// clear IDs for unused sensor instances
for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
_accel_id[i].set(0);
}
for (uint8_t i=get_gyro_count(); i<INS_MAX_INSTANCES; i++) {
_gyro_id[i].set(0);
}
}
/* Find the N instance of the backend that has already been successfully detected */
@ -688,36 +706,50 @@ AP_InertialSensor::detect_backends(void)
_backends_detected = true;
uint8_t probe_count = 0;
uint8_t enable_mask = uint8_t(_enable_mask.get());
uint8_t found_mask = 0;
/*
use ADD_BACKEND() macro to allow for INS_ENABLE_MASK for enabling/disabling INS backends
*/
#define ADD_BACKEND(x) do { \
if (((1U<<probe_count)&enable_mask) && _add_backend(x)) { \
found_mask |= (1U<<probe_count); \
} \
probe_count++; \
} while (0)
if (_hil_mode) {
_add_backend(AP_InertialSensor_HIL::detect(*this));
ADD_BACKEND(AP_InertialSensor_HIL::detect(*this));
return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
_add_backend(AP_InertialSensor_SITL::detect(*this));
ADD_BACKEND(AP_InertialSensor_SITL::detect(*this));
#elif HAL_INS_DEFAULT == HAL_INS_HIL
_add_backend(AP_InertialSensor_HIL::detect(*this));
ADD_BACKEND(AP_InertialSensor_HIL::detect(*this));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI && defined(HAL_INS_DEFAULT_ROTATION)
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME),
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME),
HAL_INS_DEFAULT_ROTATION));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && defined(HAL_INS_DEFAULT_ROTATION)
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR),
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR),
HAL_INS_DEFAULT_ROTATION));
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
#elif HAL_INS_DEFAULT == HAL_INS_BH
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
#elif AP_FEATURE_BOARD_DETECT
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PX4V1:
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
ROTATION_ROLL_180,
@ -728,56 +760,56 @@ AP_InertialSensor::detect_backends(void)
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
// older Pixhawk2 boards have the MPU6000 instead of MPU9250
_fast_sampling_mask.set_default(1);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180));
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180));
ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME),
ROTATION_ROLL_180_YAW_270,
ROTATION_ROLL_180_YAW_90,
ROTATION_ROLL_180_YAW_90));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
break;
case AP_BoardConfig::PX4_BOARD_PIXRACER:
// only do fast samplng on ICM-20608. The MPU9250 doesn't handle high rate well when it has a mag enabled
_fast_sampling_mask.set_default(1);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
_fast_sampling_mask.set_default(3);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
break;
case AP_BoardConfig::PX4_BOARD_PHMINI:
// PHMINI uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
_fast_sampling_mask.set_default(3);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
break;
case AP_BoardConfig::PX4_BOARD_AUAV21:
// AUAV2.1 uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
_fast_sampling_mask.set_default(3);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180_YAW_90));
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
break;
case AP_BoardConfig::PX4_BOARD_PH2SLIM:
_fast_sampling_mask.set_default(1);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
break;
case AP_BoardConfig::PX4_BOARD_AEROFC:
_fast_sampling_mask.set_default(1);
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU6500_NAME), ROTATION_YAW_270));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU6500_NAME), ROTATION_YAW_270));
break;
case AP_BoardConfig::PX4_BOARD_MINDPXV2:
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU6500_NAME), ROTATION_NONE));
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU6500_NAME), ROTATION_NONE));
ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
ROTATION_YAW_90,
@ -790,79 +822,43 @@ AP_InertialSensor::detect_backends(void)
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
// also add any PX4 backends (eg. canbus sensors)
_add_backend(AP_InertialSensor_PX4::detect(*this));
ADD_BACKEND(AP_InertialSensor_PX4::detect(*this));
#endif
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI && defined(HAL_INS_DEFAULT_ROTATION)
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), HAL_INS_DEFAULT_ROTATION));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), HAL_INS_DEFAULT_ROTATION));
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
#elif HAL_INS_DEFAULT == HAL_INS_EDGE
AP_InertialSensor_Backend *backend = AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_90);
if (backend) {
_add_backend(backend);
hal.console->printf("First IMU detected\n");
} else {
hal.console->printf("First IMU not detected\n");
}
backend = AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME_EXT), ROTATION_YAW_90);
if (backend) {
_add_backend(backend);
hal.console->printf("Second IMU detected\n");
} else {
hal.console->printf("Second IMU not detected\n");
}
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME_EXT), ROTATION_YAW_90));
#elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)));
#elif HAL_INS_DEFAULT == HAL_INS_L3G4200D
_add_backend(AP_InertialSensor_L3G4200D::probe(*this, hal.i2c_mgr->get_device(HAL_INS_L3G4200D_I2C_BUS, HAL_INS_L3G4200D_I2C_ADDR)));
ADD_BACKEND(AP_InertialSensor_L3G4200D::probe(*this, hal.i2c_mgr->get_device(HAL_INS_L3G4200D_I2C_BUS, HAL_INS_L3G4200D_I2C_ADDR)));
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR)));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR)));
#elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT
_add_backend(AP_InertialSensor_QFLIGHT::detect(*this));
ADD_BACKEND(AP_InertialSensor_QFLIGHT::detect(*this));
#elif HAL_INS_DEFAULT == HAL_INS_QURT
_add_backend(AP_InertialSensor_QURT::detect(*this));
ADD_BACKEND(AP_InertialSensor_QURT::detect(*this));
#elif HAL_INS_DEFAULT == HAL_INS_BBBMINI
AP_InertialSensor_Backend *backend = AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME));
if (backend) {
_add_backend(backend);
hal.console->printf("MPU9250: Onboard IMU detected\n");
} else {
hal.console->printf("MPU9250: Onboard IMU not detected\n");
}
backend = AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME_EXT));
if (backend) {
_add_backend(backend);
hal.console->printf("MPU9250: External IMU detected\n");
} else {
hal.console->printf("MPU9250: External IMU not detected\n");
}
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME_EXT)));
#elif HAL_INS_DEFAULT == HAL_INS_AERO
auto *backend = AP_InertialSensor_BMI160::probe(*this,
hal.spi->get_device("bmi160"));
if (backend) {
_add_backend(backend);
} else {
hal.console->printf("aero: onboard IMU not detected\n");
}
ADD_BACKEND(AP_InertialSensor_BMI160::probe(*this, hal.spi->get_device("bmi160")));
#elif HAL_INS_DEFAULT == HAL_INS_RST
AP_InertialSensor_Backend *backend = AP_InertialSensor_RST::probe(*this, hal.spi->get_device(HAL_INS_RST_G_NAME),
ADD_BACKEND(AP_InertialSensor_RST::probe(*this, hal.spi->get_device(HAL_INS_RST_G_NAME),
hal.spi->get_device(HAL_INS_RST_A_NAME),
HAL_INS_DEFAULT_G_ROTATION,
HAL_INS_DEFAULT_A_ROTATION);
if (backend) {
_add_backend(backend);
hal.console->printf("RST: IMU detected\n");
} else {
hal.console->printf("RST: IMU not detected\n");
}
HAL_INS_DEFAULT_A_ROTATION));
#else
#error Unrecognised HAL_INS_TYPE setting
#endif
_enable_mask.set(found_mask);
if (_backend_count == 0) {
AP_HAL::panic("No INS backends available");
}

3
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -460,6 +460,9 @@ private:
// control enable of fast sampling
AP_Int8 _fast_sampling_mask;
// control enable of detected sensors
AP_Int8 _enable_mask;
// board orientation from AHRS
enum Rotation _board_orientation;