AP_InertialSensor: update SITL to have device IDs

This commit is contained in:
Andrew Tridgell 2018-08-01 19:18:09 +10:00 committed by Randy Mackay
parent 8bac95209b
commit b378446472
3 changed files with 6 additions and 3 deletions

View File

@ -105,6 +105,7 @@ public:
DEVTYPE_INS_ICM20789 = 0x27, DEVTYPE_INS_ICM20789 = 0x27,
DEVTYPE_INS_ICM20689 = 0x28, DEVTYPE_INS_ICM20689 = 0x28,
DEVTYPE_INS_BMI055 = 0x29, DEVTYPE_INS_BMI055 = 0x29,
DEVTYPE_SITL = 0x2A,
}; };
protected: protected:

View File

@ -37,8 +37,11 @@ bool AP_InertialSensor_SITL::init_sensor(void)
// grab the used instances // grab the used instances
for (uint8_t i=0; i<INS_SITL_INSTANCES; i++) { for (uint8_t i=0; i<INS_SITL_INSTANCES; i++) {
gyro_instance[i] = _imu.register_gyro(gyro_sample_hz[i], i);
accel_instance[i] = _imu.register_accel(accel_sample_hz[i], i); gyro_instance[i] = _imu.register_gyro(gyro_sample_hz[i],
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 1, DEVTYPE_SITL));
accel_instance[i] = _imu.register_accel(accel_sample_hz[i],
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 2, DEVTYPE_SITL));
} }
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void)); hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void));

View File

@ -7,7 +7,6 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <AP_ADC/AP_ADC.h>
#include <AP_InertialSensor/AP_InertialSensor.h> #include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_Notify/AP_Notify.h> #include <AP_Notify/AP_Notify.h>
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>