AP_InertialSensor: instantiate SIM_IMU_COUNT sensors in SITL

This commit is contained in:
Andrew Tridgell 2020-08-27 18:32:58 +10:00
parent c16467a47f
commit d5c0522651
1 changed files with 3 additions and 2 deletions

View File

@ -814,8 +814,9 @@ AP_InertialSensor::detect_backends(void)
// IMUs defined by IMU lines in hwdef.dat
HAL_INS_PROBE_LIST;
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
ADD_BACKEND(AP_InertialSensor_SITL::detect(*this, INS_SITL_SENSOR_A));
ADD_BACKEND(AP_InertialSensor_SITL::detect(*this, INS_SITL_SENSOR_B));
for (uint8_t i=0; i<AP::sitl()->imu_count; i++) {
ADD_BACKEND(AP_InertialSensor_SITL::detect(*this, i==1?INS_SITL_SENSOR_B:INS_SITL_SENSOR_A));
}
#elif HAL_INS_DEFAULT == HAL_INS_HIL
ADD_BACKEND(AP_InertialSensor_HIL::detect(*this));
#elif AP_FEATURE_BOARD_DETECT