AP_InertialSensor: take semaphore for update of accumulators

this fixes a race in update of delta angle and delta velocity between
backend and frontend

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -93,18 +93,18 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
     delta_coning = delta_coning % delta_angle;
     delta_coning *= 0.5f;
 
-    // integrate delta angle accumulator
-    // the angles and coning corrections are accumulated separately in the
-    // referenced paper, but in simulation little difference was found between
-    // integrating together and integrating separately (see examples/coning.py)
-    _imu._delta_angle_acc[instance] += delta_angle + delta_coning;
-    _imu._delta_angle_acc_dt[instance] += dt;
+    if (_sem->take(0)) {
+        // integrate delta angle accumulator
+        // the angles and coning corrections are accumulated separately in the
+        // referenced paper, but in simulation little difference was found between
+        // integrating together and integrating separately (see examples/coning.py)
+        _imu._delta_angle_acc[instance] += delta_angle + delta_coning;
+        _imu._delta_angle_acc_dt[instance] += dt;
 
-    // save previous delta angle for coning correction
-    _imu._last_delta_angle[instance] = delta_angle;
-    _imu._last_raw_gyro[instance] = gyro;
+        // save previous delta angle for coning correction
+        _imu._last_delta_angle[instance] = delta_angle;
+        _imu._last_raw_gyro[instance] = gyro;
 
-    if (_sem->take(0)) {
         _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
         if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) {
             _imu._gyro_filter[instance].reset();
@@ -183,11 +183,11 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
     
     _imu.calc_vibration_and_clipping(instance, accel, dt);
 
-    // delta velocity
-    _imu._delta_velocity_acc[instance] += accel * dt;
-    _imu._delta_velocity_acc_dt[instance] += dt;
-
     if (_sem->take(0)) {
+        // delta velocity
+        _imu._delta_velocity_acc[instance] += accel * dt;
+        _imu._delta_velocity_acc_dt[instance] += dt;
+
         _imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
         if (_imu._accel_filtered[instance].is_nan() || _imu._accel_filtered[instance].is_inf()) {
             _imu._accel_filter[instance].reset();