_calibrate_at_rest() never set gyroscope biases correctly

[Ic-arbon]

I always get [0.0, 0.0, 0.0] when i try to get gyro biases. I find that init_mpu() will be executed after set_unscaled_gyro_biases in _calibrate_at_rest().

        self.set_unscaled_gyro_bias(false,
                                    [(gyro_biases[0] / -4) as i16,
                                     (gyro_biases[1] / -4) as i16,
                                     (gyro_biases[2] / -4) as i16])?;

        // Compute accelerometer biases to be returned
        let resolution = self.accel_scale.resolution();
        let scale = G * resolution;

        // Set original values back and re-init device
        self.gyro_scale = orig_gyro_scale;
        self.accel_scale = orig_accel_scale;
        self.gyro_temp_data_rate = orig_gyro_temp_data_rate;
        self.accel_data_rate = orig_accel_data_rate;
        self.sample_rate_divisor = orig_sample_rate_divisor;
        self.init_mpu(delay)?;

XG_OFFSET, YG_OFFSET, ZG_OFFSET will be reset in init_mpu(), when PWR_MGMT_1 changes

        // wake up device
        self.dev.write(Register::PWR_MGMT_1, 0x80)?;
        delay.delay_ms(100); // Wait for all registers to reset

I simply move set biases below register rest operation, and it just works.

        self.init_mpu(delay)?;

        self.set_unscaled_gyro_bias(false,
                                    [(gyro_biases[0] / -4) as i16,
                                     (gyro_biases[1] / -4) as i16,
                                     (gyro_biases[2] / -4) as i16])?;

        Ok([accel_biases[0] as f32 * scale,
            accel_biases[1] as f32 * scale,
            accel_biases[2] as f32 * scale])