diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index cd0edb6102..b9aac1f684 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -689,16 +689,20 @@ AP_InertialSensor::detect_backends(void) _add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME))); #elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN - if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PX4V1) { + 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))); + break; - } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK) { + 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, hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME), hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_ROLL_180, ROTATION_ROLL_180_YAW_270)); - } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) { + break; + + 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)); @@ -707,20 +711,28 @@ AP_InertialSensor::detect_backends(void) hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_ROLL_180_YAW_270, ROTATION_ROLL_180_YAW_90)); _add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270)); - } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXRACER) { + break; + + case AP_BoardConfig::PX4_BOARD_PIXRACER: _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)); + break; - } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PHMINI) { + 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)); + break; - } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PH2SLIM) { + 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)); + break; + + default: + break; } // also add any PX4 backends (eg. canbus sensors) _add_backend(AP_InertialSensor_PX4::detect(*this));