AP_InertialSensor: properly reset MPU6000 signal path at startup

failure to reset the signal path as per the register spec leads to
very poor temperature response early on in startup.

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp

@@ -973,6 +973,13 @@ bool AP_InertialSensor_Invensense::_hardware_init(void)
 
         /* bus-dependent initialization */
         if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
+            /* reset signal path as recommended in the datasheet */
+            if (_mpu_type == Invensense_MPU6000 || _mpu_type == Invensense_MPU6500) {
+                _register_write(MPUREG_SIGNAL_PATH_RESET,
+                    BIT_SIGNAL_PATH_RESET_TEMP_RESET|BIT_SIGNAL_PATH_RESET_ACCEL_RESET|BIT_SIGNAL_PATH_RESET_GYRO_RESET);
+                hal.scheduler->delay(100);
+            }
+
             /* Disable I2C bus if SPI selected (Recommended in Datasheet to be
              * done just after the device is reset) */
             _last_stat_user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;

libraries/AP_InertialSensor/AP_InertialSensor_Invensense_registers.h

@@ -113,6 +113,11 @@
 #       define BIT_I2C_SLV1_DLY_EN              0x02
 #       define BIT_I2C_SLV2_DLY_EN              0x04
 #       define BIT_I2C_SLV3_DLY_EN              0x08
+#define MPUREG_SIGNAL_PATH_RESET                0x68
+// bit definitions for MPUREG_SIGNAL_PATH_RESET
+#       define BIT_SIGNAL_PATH_RESET_TEMP_RESET     0x01 // resets temp signal path
+#       define BIT_SIGNAL_PATH_RESET_ACCEL_RESET    0x02 // resets accel signal path
+#       define BIT_SIGNAL_PATH_RESET_GYRO_RESET     0x04 // resets gyro signal path
 #define MPUREG_USER_CTRL                        0x6A
 // bit definitions for MPUREG_USER_CTRL
 #       define BIT_USER_CTRL_SIG_COND_RESET         0x01 // resets signal paths and results registers for all sensors (gyros, accel, temp)