AP_InertialSensor: Backend: add hook for new gyro sample arrival

That hook will do common tasks for when new gyro raw sample is available.

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -53,6 +53,11 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &
     _imu._gyro_healthy[instance] = true;
 }
 
+void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
+                                                            const Vector3f &gyro)
+{
+}
+
 /*
   rotate accel vector, scale and add the accel offset
  */

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -85,6 +85,12 @@ protected:
     // rotate gyro vector, offset and publish
     void _publish_gyro(uint8_t instance, const Vector3f &gyro);
 
+    // this should be called every time a new gyro raw sample is available -
+    // be it published or not
+    // the sample is raw in the sense that it's not filtered yet, but it must
+    // be rotated and corrected (_rotate_and_correct_gyro)
+    void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel);
+
     // rotate accel vector, scale, offset and publish
     void _publish_accel(uint8_t instance, const Vector3f &accel);
 

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp

@@ -245,6 +245,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
         // Adjust for chip scaling to get radians/sec
         gyro *= FLYMAPLE_GYRO_SCALE_R_S;
         _rotate_and_correct_gyro(_gyro_instance, gyro);
+        _notify_new_gyro_raw_sample(_gyro_instance, gyro);
         _gyro_filtered = _gyro_filter.apply(gyro);
         _have_gyro_sample = true;
         _last_gyro_timestamp = now;

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp

@@ -321,6 +321,7 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
                 // Adjust for chip scaling to get radians/sec
                 gyro *= L3G4200D_GYRO_SCALE_R_S;
                 _rotate_and_correct_gyro(_gyro_instance, gyro);
+                _notify_new_gyro_raw_sample(_gyro_instance, gyro);
                 _data[_data_idx].gyro_filtered = _gyro_filter.apply(gyro);
                 _have_gyro_sample = true;
             }

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp

@@ -757,6 +757,7 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
     Vector3f gyro_data(raw_data.x, -raw_data.y, -raw_data.z);
     gyro_data *= _gyro_scale;
     _rotate_and_correct_gyro(_gyro_instance, gyro_data);
+    _notify_new_gyro_raw_sample(_gyro_instance, gyro_data);
     _gyro_filtered = _gyro_filter.apply(gyro_data);
     _gyro_sample_available = true;
 }

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -721,6 +721,7 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
         _rotate_and_correct_gyro(_gyro_instance, gyro);
 
         _notify_new_accel_raw_sample(_accel_instance, accel);
+        _notify_new_gyro_raw_sample(_gyro_instance, gyro);
 
         _accel_filtered = _accel_filter.apply(accel);
         _gyro_filtered = _gyro_filter.apply(gyro);

libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp

@@ -1098,6 +1098,7 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
         gyro = Vector3f(gyro_x, gyro_y, gyro_z);
         gyro *= MPU9150_GYRO_SCALE_2000;
         _rotate_and_correct_gyro(_gyro_instance, gyro);
+        _notify_new_gyro_raw_sample(_gyro_instance, gyro);
         _gyro_filtered = _gyro_filter.apply(gyro);
 
         _have_sample_available = true;

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

@@ -404,6 +404,7 @@ void AP_InertialSensor_MPU9250::_read_data_transaction()
     gyro *= GYRO_SCALE;
     gyro.rotate(_default_rotation);
     _rotate_and_correct_gyro(_gyro_instance, gyro);
+    _notify_new_gyro_raw_sample(_gyro_instance, gyro);
 
 
     // update the shared buffer

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -296,6 +296,7 @@ void AP_InertialSensor_PX4::_new_gyro_sample(uint8_t i, gyro_report &gyro_report
 
     // apply corrections
     _rotate_and_correct_gyro(frontend_instance, gyro);
+    _notify_new_gyro_raw_sample(frontend_instance, gyro);
 
     // apply filter for control path
     _gyro_in[i] = _gyro_filter[i].apply(gyro);