mirror of https://github.com/ArduPilot/ardupilot
INS: switch to SREG = oldSREG pattern for interrupt mask/restore
This commit is contained in:
parent
e331634bc5
commit
f843705da3
|
@ -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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue