From ace61087a18291c1d052e774ad52317b22f924ba Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Sat, 3 Sep 2016 12:37:47 -0300 Subject: [PATCH] AP_InertialSensor: remove product_id We only leave the parameter there for backward-compatibility. However product id on the inertial sensor is not much useful since it's only kept for the first instance. A better implementation per-gyro and per-accel is needed in order to avoid problems with sensors taking the offsets configured for another sensor. --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 9 ++------- libraries/AP_InertialSensor/AP_InertialSensor.h | 2 +- .../AP_InertialSensor/AP_InertialSensor_Backend.cpp | 3 +-- .../AP_InertialSensor/AP_InertialSensor_Backend.h | 8 -------- libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp | 1 - .../AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp | 1 - .../AP_InertialSensor/AP_InertialSensor_MPU6000.cpp | 10 +++++----- .../AP_InertialSensor/AP_InertialSensor_MPU9250.cpp | 2 -- libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp | 11 ----------- .../AP_InertialSensor/AP_InertialSensor_QURT.cpp | 2 -- .../AP_InertialSensor/AP_InertialSensor_SITL.cpp | 2 -- .../AP_InertialSensor/AP_InertialSensor_qflight.cpp | 1 - 12 files changed, 9 insertions(+), 43 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 8bed92b21d..0f2bfe28ac 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -55,10 +55,9 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // @Param: PRODUCT_ID // @DisplayName: IMU Product ID - // @Description: Which type of IMU is installed (read-only). + // @Description: unused // @User: Advanced - // @Values: 0:Unknown,1:unused,2:unused,88:unused,3:SITL,4:PX4v1,5:PX4v2,256:unused,257:Linux - AP_GROUPINFO("PRODUCT_ID", 0, AP_InertialSensor, _product_id, 0), + AP_GROUPINFO("PRODUCT_ID", 0, AP_InertialSensor, _old_product_id, 0), /* The following parameter indexes and reserved from previous use @@ -656,9 +655,6 @@ AP_InertialSensor::detect_backends(void) if (_backend_count == 0) { AP_HAL::panic("No INS backends available"); } - - // set the product ID to the ID of the first backend - _product_id.set(_backends[0]->product_id()); } /* @@ -992,7 +988,6 @@ AP_InertialSensor::_init_gyro() // save parameters to eeprom void AP_InertialSensor::_save_parameters() { - _product_id.save(); for (uint8_t i=0; inew_semaphore(); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index a82680a4a0..3c6f84cb6e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -64,11 +64,6 @@ public: */ virtual AuxiliaryBus *get_auxiliary_bus() { return nullptr; } - /* - return the product ID - */ - int16_t product_id(void) const { return _product_id; } - int16_t get_id() const { return _id; } protected: @@ -121,9 +116,6 @@ protected: // set gyro error_count void _set_gyro_error_count(uint8_t instance, uint32_t error_count); - // backend should fill in its product ID from AP_PRODUCT_ID_* - int16_t _product_id; - // backend unique identifier or -1 if backend doesn't identify itself int16_t _id = -1; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index 3f592f345e..f178fddaa2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp @@ -30,7 +30,6 @@ bool AP_InertialSensor_HIL::_init_sensor(void) _imu.register_gyro(1200); _imu.register_accel(1200); - _product_id = AP_PRODUCT_ID_NONE; _imu.set_hil_mode(); return true; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index d6a5839e60..347e2f4744 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -390,7 +390,6 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu, , _drdy_pin_num_g(drdy_pin_num_g) , _rotation(rotation) { - _product_id = AP_PRODUCT_ID_NONE; } AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::probe(AP_InertialSensor &_imu, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index d9efdbbb71..94a39df5cb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -350,15 +350,15 @@ void AP_InertialSensor_MPU6000::start() hal.scheduler->delay(1); // read the product ID rev c has 1/2 the sensitivity of rev d - _product_id = _register_read(MPUREG_PRODUCT_ID); + uint8_t product_id = _register_read(MPUREG_PRODUCT_ID); //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); // TODO: should be changed to 16G once we have a way to override the // previous offsets - 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 d7ab904734..927424fd84 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -302,8 +302,6 @@ void AP_InertialSensor_MPU9250::start() _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); hal.scheduler->delay(1); - _product_id = AP_PRODUCT_ID_MPU9250; - // RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 16g _register_write(MPUREG_ACCEL_CONFIG,3<<3); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index a5f4e4d424..61b886a72e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -156,17 +156,6 @@ bool AP_InertialSensor_PX4::_init_sensor(void) _accel_sample_time[i] = 1.0f / samplerate; } -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - _product_id = AP_PRODUCT_ID_VRBRAIN; -#else -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - _product_id = AP_PRODUCT_ID_PX4_V2; -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4) - _product_id = AP_PRODUCT_ID_PX4_V4; -#else - _product_id = AP_PRODUCT_ID_PX4; -#endif -#endif return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp index 54bf2af4d3..f528c131b2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp @@ -36,8 +36,6 @@ bool AP_InertialSensor_QURT::init_sensor(void) mpu9250_mag_buffer = new ObjectBuffer(20); init_mpu9250(); - _product_id = AP_PRODUCT_ID_MPU9250; - return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index b3032509fa..f4c12610a1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -42,8 +42,6 @@ bool AP_InertialSensor_SITL::init_sensor(void) hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void)); - _product_id = AP_PRODUCT_ID_NONE; - return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp index 19ebacab43..138586a184 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp @@ -35,7 +35,6 @@ bool AP_InertialSensor_QFLIGHT::init_sensor(void) accel_instance = _imu.register_accel(1000); hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_QFLIGHT::timer_update, void)); - _product_id = AP_PRODUCT_ID_MPU9250; return true; }