diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 129d2cda40..77f83100e2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -326,6 +326,9 @@ AP_InertialSensor::_detect_backends(Sample_rate sample_rate) _accel_count == 0) { hal.scheduler->panic(PSTR("No INS backends available")); } + + // set the product ID to the ID of the first backend + _product_id.set(_backends[0]->product_id()); } void diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 0fde6615f3..ade75ee243 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -5,7 +5,8 @@ #include "AP_InertialSensor_Backend.h" AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) : - _imu(imu) + _imu(imu), + _product_id(AP_PRODUCT_ID_NONE) {} /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 44da6dc502..524e390468 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -48,6 +48,11 @@ public: */ virtual bool gyro_sample_available() = 0; + /* + return the product ID + */ + int16_t product_id(void) const { return _product_id; } + protected: // access to frontend AP_InertialSensor &_imu; @@ -58,6 +63,9 @@ protected: // rotate accel vector, scale and offset void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now); + // backend should fill in its product ID from AP_PRODUCT_ID_* + int16_t _product_id; + // note that each backend is also expected to have a static detect() // function which instantiates an instance of the backend sensor // driver if the sensor is available diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp index cc0d5a097e..2a5e660fb4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp @@ -172,6 +172,8 @@ bool AP_InertialSensor_Flymaple::_init_sensor(AP_InertialSensor::Sample_rate sam _gyro_instance = _imu.register_gyro(); _accel_instance = _imu.register_accel(); + _product_id = AP_PRODUCT_ID_FLYMAPLE; + return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index e779835d48..5e805e2b7f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp @@ -51,6 +51,8 @@ bool AP_InertialSensor_HIL::_init_sensor(AP_InertialSensor::Sample_rate sample_r _imu.register_gyro(); _imu.register_accel(); + _product_id = AP_PRODUCT_ID_NONE; + return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index cc69d3dbdc..0f6c7c9815 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -238,6 +238,8 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(AP_InertialSensor::Sample_rate sam _gyro_instance = _imu.register_gyro(); _accel_instance = _imu.register_accel(); + _product_id = AP_PRODUCT_ID_L3G4200D; + return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h index 930e9b25d9..fc26dc97cb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h @@ -26,6 +26,9 @@ public: static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, AP_InertialSensor::Sample_rate sample_rate); + // return product ID + int16_t product_id(void) const { return AP_PRODUCT_ID_L3G4200D; } + private: bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate); void _accumulate(void); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index f9687c66a7..d6ffd2c009 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -525,13 +525,13 @@ bool AP_InertialSensor_MPU6000::_hardware_init(AP_InertialSensor::Sample_rate sa hal.scheduler->delay(1); // read the product ID rev c has 1/2 the sensitivity of rev d - uint8_t product_id = _register_read(MPUREG_PRODUCT_ID); + _product_id = _register_read(MPUREG_PRODUCT_ID); //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); - if ((product_id == MPU6000ES_REV_C4) || - (product_id == MPU6000ES_REV_C5) || - (product_id == MPU6000_REV_C4) || - (product_id == MPU6000_REV_C5)) { + if ((_product_id == MPU6000ES_REV_C4) || + (_product_id == MPU6000ES_REV_C5) || + (_product_id == MPU6000_REV_C4) || + (_product_id == MPU6000_REV_C5)) { // Accel scale 8g (4096 LSB/g) // Rev C has different scaling than rev D _register_write(MPUREG_ACCEL_CONFIG,1<<3); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 3e349784d5..fe46d82da0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -250,6 +250,8 @@ bool AP_InertialSensor_MPU9250::_init_sensor(AP_InertialSensor::Sample_rate samp _gyro_instance = _imu.register_gyro(); _accel_instance = _imu.register_accel(); + _product_id = AP_PRODUCT_ID_MPU9250; + // start the timer process to read samples hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data)); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index 3a5af54921..b65b11ae15 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -86,6 +86,8 @@ bool AP_InertialSensor_Oilpan::_init_sensor(AP_InertialSensor::Sample_rate sampl _gyro_instance = _imu.register_gyro(); _accel_instance = _imu.register_accel(); + _product_id = AP_PRODUCT_ID_APM1_2560; + return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index aa96796386..f37b86c448 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -94,6 +94,12 @@ bool AP_InertialSensor_PX4::_init_sensor(AP_InertialSensor::Sample_rate sample_r _set_filter_frequency(_imu.get_filter()); +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + _product_id = AP_PRODUCT_ID_PX4_V2; +#else + _product_id = AP_PRODUCT_ID_PX4; +#endif + return true; }