aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: make INS sensor IDs match for PX4

Browse Source 
in-tree driver IDs now match PX4Firmware driver IDs on Pixhawk2
This commit is contained in:
Andrew Tridgell 2016-11-05 11:11:00 +11:00
parent 76b2759317
commit aa4025bdf9
6 changed files with 30 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
View File

@ -177,10 +177,8 @@ void AP_InertialSensor_BMI160::start()
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
_accel_instance = _imu.register_accel(BMI160_ODR_TO_HZ(BMI160_ODR), _accel_instance = _imu.register_accel(BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160));
_dev->get_bus_id()); _gyro_instance = _imu.register_gyro(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());
/* Call _poll_data() at 1kHz */ /* Call _poll_data() at 1kHz */
_dev->register_periodic_callback(1000, _dev->register_periodic_callback(1000,

20
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -70,6 +70,26 @@ public:
*/ */
int16_t get_id() const { return _id; } 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: protected:
// access to frontend // access to frontend
AP_InertialSensor &_imu; AP_InertialSensor &_imu;

4
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -204,8 +204,8 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
*/ */
void AP_InertialSensor_L3G4200D::start(void) void AP_InertialSensor_L3G4200D::start(void)
{ {
_gyro_instance = _imu.register_gyro(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()); _accel_instance = _imu.register_accel(800, _dev->get_bus_id_devtype(DEVTYPE_L3G4200D));
// start the timer process to read samples // start the timer process to read samples
_dev->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, bool)); _dev->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, bool));

4
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -505,8 +505,8 @@ fail_whoami:
*/ */
void AP_InertialSensor_LSM9DS0::start(void) void AP_InertialSensor_LSM9DS0::start(void)
{ {
_gyro_instance = _imu.register_gyro(760, _dev_gyro->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()); _accel_instance = _imu.register_accel(800, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D));
set_gyro_orientation(_gyro_instance, _rotation); set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation); set_accel_orientation(_accel_instance, _rotation);

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -210,9 +210,6 @@ extern const AP_HAL::HAL& hal;
#endif #endif
#define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE) #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 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]) #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(); _dev->get_semaphore()->give();
// grab the used instances // grab the used instances
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(MPU6000_DRIVER_VERSION_GYRO)); _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(MPU6000_DRIVER_VERSION_ACCEL)); _accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU6000));
// setup sensor rotations from probe() // setup sensor rotations from probe()
set_gyro_orientation(_gyro_instance, _rotation); set_gyro_orientation(_gyro_instance, _rotation);

4
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -320,8 +320,8 @@ void AP_InertialSensor_MPU9250::start()
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
// grab the used instances // grab the used instances
_gyro_instance = _imu.register_gyro(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()); _accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU9250));
// start the timer process to read samples // start the timer process to read samples
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_read_sample, bool)); _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_read_sample, bool));