AP_InertialSensor: MPU6000: change accel scale to 16G

Also change the ID of MPU6000 so previous calibration values are not
considered valid.

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -7,9 +7,6 @@
 
 extern const AP_HAL::HAL& hal;
 
-// MPU6000 accelerometer scaling
-#define MPU6000_ACCEL_SCALE_1G    (GRAVITY_MSS / 4096.0f)
-
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 #include <AP_HAL_Linux/GPIO.h>
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
@@ -213,7 +210,7 @@ extern const AP_HAL::HAL& hal;
 #endif
 #define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE)
 
-#define MPU6000_DRIVER_VERSION_ACCEL 0U
+#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]))
@@ -356,8 +353,6 @@ void AP_InertialSensor_MPU6000::start()
     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)   ||
@@ -365,9 +360,11 @@ void AP_InertialSensor_MPU6000::start()
         // Accel scale 8g (4096 LSB/g)
         // Rev C has different scaling than rev D
         _register_write(MPUREG_ACCEL_CONFIG,1<<3);
+        _accel_scale = GRAVITY_MSS / 4096.f;
     } else {
-        // Accel scale 8g (4096 LSB/g)
-        _register_write(MPUREG_ACCEL_CONFIG,2<<3);
+        // Accel scale 16g (2048 LSB/g)
+        _register_write(MPUREG_ACCEL_CONFIG,3<<3);
+        _accel_scale = GRAVITY_MSS / 2048.f;
     }
     hal.scheduler->delay(1);
 
@@ -482,7 +479,7 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
         accel = Vector3f(int16_val(data, 1),
                          int16_val(data, 0),
                          -int16_val(data, 2));
-        accel *= MPU6000_ACCEL_SCALE_1G;
+        accel *= _accel_scale;
 
         gyro = Vector3f(int16_val(data, 5),
                         int16_val(data, 4),

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -100,6 +100,7 @@ private:
     uint16_t _error_count;
 
     float _temp_filtered;
+    float _accel_scale;
     LowPassFilter2pFloat _temp_filter;
 
     enum Rotation _rotation;