INS: switch to SREG = oldSREG pattern for interrupt mask/restore

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -243,6 +243,7 @@ bool AP_InertialSensor_MPU6000::update( void )
     while (_count == 0) /* nop */;
 
     // disable interrupts for mininum time
+    uint8_t oldSREG = SREG;
     cli();
     for (int i=0; i<7; i++) {
         sum[i] = _sum[i];
@@ -254,7 +255,7 @@ bool AP_InertialSensor_MPU6000::update( void )
     // record sample time
     _delta_time_micros = _last_sample_time_micros - _delta_time_start_micros;
     _delta_time_start_micros = _last_sample_time_micros;
-    sei();
+    SREG = oldSREG;
 
     count_scale = 1.0 / count;