diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp index ee33a2e19e..851e8b3406 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp @@ -177,10 +177,8 @@ void AP_InertialSensor_BMI160::start() _dev->get_semaphore()->give(); - _accel_instance = _imu.register_accel(BMI160_ODR_TO_HZ(BMI160_ODR), - _dev->get_bus_id()); - _gyro_instance = _imu.register_gyro(BMI160_ODR_TO_HZ(BMI160_ODR), - _dev->get_bus_id()); + _accel_instance = _imu.register_accel(BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)); + _gyro_instance = _imu.register_gyro(BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)); /* Call _poll_data() at 1kHz */ _dev->register_periodic_callback(1000, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 44afdeb2f7..0bb6e1ecb4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -70,6 +70,26 @@ public: */ int16_t get_id() const { return _id; } + /* + device driver IDs. These are used to fill in the devtype field + of the device ID, which shows up as INS*ID* parameters to + users. The values are chosen for compatibility with existing PX4 + drivers. + If a change is made to a driver that would make existing + calibration values invalid then this number must be changed. + */ + enum DevTypes { + DEVTYPE_BMI160 = 0x09, + DEVTYPE_L3G4200D = 0x10, + DEVTYPE_ACC_LSM303D = 0x11, + DEVTYPE_ACC_BMA180 = 0x12, + DEVTYPE_ACC_MPU6000 = 0x13, + DEVTYPE_ACC_MPU9250 = 0x16, + DEVTYPE_GYR_MPU6000 = 0x21, + DEVTYPE_GYR_L3GD20 = 0x22, + DEVTYPE_GYR_MPU9250 = 0x24 + }; + protected: // access to frontend AP_InertialSensor &_imu; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index 4b76c83902..82948b7b5c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -204,8 +204,8 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void) */ void AP_InertialSensor_L3G4200D::start(void) { - _gyro_instance = _imu.register_gyro(800, _dev->get_bus_id()); - _accel_instance = _imu.register_accel(800, _dev->get_bus_id()); + _gyro_instance = _imu.register_gyro(800, _dev->get_bus_id_devtype(DEVTYPE_L3G4200D)); + _accel_instance = _imu.register_accel(800, _dev->get_bus_id_devtype(DEVTYPE_L3G4200D)); // start the timer process to read samples _dev->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, bool)); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index 3bd5302f0a..407f7b225d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -505,8 +505,8 @@ fail_whoami: */ void AP_InertialSensor_LSM9DS0::start(void) { - _gyro_instance = _imu.register_gyro(760, _dev_gyro->get_bus_id()); - _accel_instance = _imu.register_accel(800, _dev_accel->get_bus_id()); + _gyro_instance = _imu.register_gyro(760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20)); + _accel_instance = _imu.register_accel(800, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D)); set_gyro_orientation(_gyro_instance, _rotation); set_accel_orientation(_accel_instance, _rotation); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index d156bbb12a..22ae20d162 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -210,9 +210,6 @@ extern const AP_HAL::HAL& hal; #endif #define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE) -#define MPU6000_DRIVER_VERSION_ACCEL 1U -#define MPU6000_DRIVER_VERSION_GYRO 0U - #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) #define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]) @@ -382,8 +379,8 @@ void AP_InertialSensor_MPU6000::start() _dev->get_semaphore()->give(); // grab the used instances - _gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(MPU6000_DRIVER_VERSION_GYRO)); - _accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(MPU6000_DRIVER_VERSION_ACCEL)); + _gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU6000)); + _accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU6000)); // setup sensor rotations from probe() set_gyro_orientation(_gyro_instance, _rotation); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index f459b041ba..e08c7ffe7e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -320,8 +320,8 @@ void AP_InertialSensor_MPU9250::start() _dev->get_semaphore()->give(); // grab the used instances - _gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE, _dev->get_bus_id()); - _accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE, _dev->get_bus_id()); + _gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU9250)); + _accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU9250)); // start the timer process to read samples _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_read_sample, bool));