AP_InertialSensor: allow backends to publish delta velocities and angles

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -248,6 +248,8 @@ AP_InertialSensor::AP_InertialSensor() :
         _accel_error_count[i] = 0;
         _gyro_error_count[i] = 0;
     }
+    memset(_delta_velocity_valid,0,sizeof(_delta_velocity_valid));
+    memset(_delta_angle_valid,0,sizeof(_delta_angle_valid));
 }
 
 
@@ -1036,6 +1038,8 @@ void AP_InertialSensor::update(void)
             // _publish_accel()
             _gyro_healthy[i] = false;
             _accel_healthy[i] = false;
+            _delta_velocity_valid[i] = false;
+            _delta_angle_valid[i] = false;
         }
         for (uint8_t i=0; i<_backend_count; i++) {
             _backends[i]->update();

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -119,6 +119,23 @@ public:
     const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; }
     const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); }
 
+    //get delta angle if available
+    bool get_delta_angle(uint8_t i, Vector3f &delta_angle) const {
+        if(_delta_angle_valid[i]) delta_angle = _delta_angle[i];
+        return _delta_angle_valid[i];
+    }
+
+    bool get_delta_angle(Vector3f &delta_angle) const { return get_delta_angle(_primary_gyro, delta_angle); }
+
+
+    //get delta velocity if available
+    bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const {
+        if(_delta_velocity_valid[i]) delta_velocity = _delta_velocity[i];
+        return _delta_velocity_valid[i];
+    }
+
+    bool get_delta_velocity(Vector3f &delta_velocity) const { return get_delta_velocity(_primary_accel, delta_velocity); }
+
     /// Fetch the current accelerometer values
     ///
     /// @returns	vector of current accelerations in m/s/s
@@ -239,9 +256,13 @@ private:
     
     // Most recent accelerometer reading
     Vector3f _accel[INS_MAX_INSTANCES];
+    Vector3f _delta_velocity[INS_MAX_INSTANCES];
+    bool _delta_velocity_valid[INS_MAX_INSTANCES];
 
     // Most recent gyro reading
     Vector3f _gyro[INS_MAX_INSTANCES];
+    Vector3f _delta_angle[INS_MAX_INSTANCES];
+    bool _delta_angle_valid[INS_MAX_INSTANCES];
 
     // product id
     AP_Int16 _product_id;

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -9,31 +9,58 @@ AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
     _product_id(AP_PRODUCT_ID_NONE)
 {}
 
+void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vector3f &accel) {
+    const Vector3f &accel_scale = _imu._accel_scale[instance].get();
+    accel.rotate(_imu._board_orientation);
+    accel.x *= accel_scale.x;
+    accel.y *= accel_scale.y;
+    accel.z *= accel_scale.z;
+    accel -= _imu._accel_offset[instance];
+}
+
+void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) {
+    gyro.rotate(_imu._board_orientation);
+    gyro -= _imu._gyro_offset[instance];
+}
+
+void AP_InertialSensor_Backend::_publish_delta_angle(uint8_t instance, const Vector3f &delta_angle)
+{
+    // publish delta angle
+    _imu._delta_angle[instance] = delta_angle;
+    _imu._delta_angle_valid[instance] = true;
+}
+
 /*
   rotate gyro vector and add the gyro offset
  */
-void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro)
+void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro, bool rotate_and_correct)
 {
     _imu._gyro[instance] = gyro;
-    _imu._gyro[instance].rotate(_imu._board_orientation);
-    _imu._gyro[instance] -= _imu._gyro_offset[instance];
     _imu._gyro_healthy[instance] = true;
+
+    if (rotate_and_correct) {
+        _rotate_and_correct_gyro(instance, _imu._gyro[instance]);
+    }
+}
+
+void AP_InertialSensor_Backend::_publish_delta_velocity(uint8_t instance, const Vector3f &delta_velocity)
+{
+    // publish delta velocity
+    _imu._delta_velocity[instance] = delta_velocity;
+    _imu._delta_velocity_valid[instance] = true;
 }
 
 /*
   rotate accel vector, scale and add the accel offset
  */
-void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel)
+void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel, bool rotate_and_correct)
 {
     _imu._accel[instance] = accel;
-    _imu._accel[instance].rotate(_imu._board_orientation);
-
-    const Vector3f &accel_scale = _imu._accel_scale[instance].get();
-    _imu._accel[instance].x *= accel_scale.x;
-    _imu._accel[instance].y *= accel_scale.y;
-    _imu._accel[instance].z *= accel_scale.z;
-    _imu._accel[instance] -= _imu._accel_offset[instance];
     _imu._accel_healthy[instance] = true;
+
+    if (rotate_and_correct) {
+        _rotate_and_correct_accel(instance, _imu._accel[instance]);
+    }
 }
 
 // set accelerometer error_count

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -61,14 +61,17 @@ protected:
     // access to frontend
     AP_InertialSensor &_imu;
 
-    // rotate gyro vector and offset
+    void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel);
-    void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro);
+    void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro);
+
+    void _publish_delta_velocity(uint8_t instance, const Vector3f &delta_velocity);
+    void _publish_delta_angle(uint8_t instance, const Vector3f &delta_angle);
 
     // rotate gyro vector, offset and publish
-    void _publish_gyro(uint8_t instance, const Vector3f &gyro);
+    void _publish_gyro(uint8_t instance, const Vector3f &gyro, bool rotate_and_correct = true);
 
     // rotate accel vector, scale, offset and publish
-    void _publish_accel(uint8_t instance, const Vector3f &accel);
+    void _publish_accel(uint8_t instance, const Vector3f &accel, bool rotate_and_correct = true);
 
     // set accelerometer error_count
     void _set_accel_error_count(uint8_t instance, uint32_t error_count);