From 7fef515555f2c1f565f6edd538ff4c92cb405d50 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Thu, 10 Sep 2015 09:34:01 -0300
Subject: [PATCH] AP_InertialSensor: unify delta angle calculation

This commit basically moves delta angle calculation that was previously done in
AP_InertialSensor_PX4 to a common place. Instances must publish their gyro raw
sample rate to enable delta angle calculation.
---
 .../AP_InertialSensor/AP_InertialSensor.cpp   |  5 +++
 .../AP_InertialSensor/AP_InertialSensor.h     |  3 ++
 .../AP_InertialSensor_Backend.cpp             | 38 +++++++++++++++++++
 3 files changed, 46 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 9f8c811bdf..8f59006a54 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -343,6 +343,10 @@ AP_InertialSensor::AP_InertialSensor() :
 
         _delta_velocity_acc[i].zero();
         _delta_velocity_acc_dt[i] = 0;
+
+        _delta_angle_acc[i].zero();
+        _last_delta_angle[i].zero();
+        _last_raw_gyro[i].zero();
     }
     for (uint8_t i=0; i<INS_VIBRATION_CHECK_INSTANCES; i++) {
         _accel_vibe_floor_filter[i].set_cutoff_frequency(AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ);
@@ -1272,6 +1276,7 @@ void AP_InertialSensor::update(void)
         for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
             _delta_velocity_acc[i].zero();
             _delta_velocity_acc_dt[i] = 0;
+            _delta_angle_acc[i].zero();
         }
 
         // adjust health status if a sensor has a non-zero error count
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 6ab13e6168..84019b9a49 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -285,6 +285,9 @@ private:
     Vector3f _gyro[INS_MAX_INSTANCES];
     Vector3f _delta_angle[INS_MAX_INSTANCES];
     bool _delta_angle_valid[INS_MAX_INSTANCES];
+    Vector3f _delta_angle_acc[INS_MAX_INSTANCES];
+    Vector3f _last_delta_angle[INS_MAX_INSTANCES];
+    Vector3f _last_raw_gyro[INS_MAX_INSTANCES];
 
     // product id
     AP_Int16 _product_id;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index 650d55a853..60c53a29b5 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -51,11 +51,49 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &
 {
     _imu._gyro[instance] = gyro;
     _imu._gyro_healthy[instance] = true;
+
+    if (_imu._gyro_sample_rates[instance] <= 0) {
+        return;
+    }
+
+    // publish delta angle
+    _imu._delta_angle[instance] = _imu._delta_angle_acc[instance];
+    _imu._delta_angle_valid[instance] = true;
 }
 
 void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
                                                             const Vector3f &gyro)
 {
+    float dt;
+
+    if (_imu._gyro_sample_rates[instance] <= 0) {
+        return;
+    }
+
+    dt = 1.0f / _imu._gyro_sample_rates[instance];
+
+    // compute delta angle
+    Vector3f delta_angle = (gyro + _imu._last_raw_gyro[instance]) * 0.5f * dt;
+
+    // compute coning correction
+    // see page 26 of:
+    // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
+    // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
+    // see also examples/coning.py
+    Vector3f delta_coning = (_imu._delta_angle_acc[instance] +
+                             _imu._last_delta_angle[instance] * (1.0f / 6.0f));
+    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;
+
+    // save previous delta angle for coning correction
+    _imu._last_delta_angle[instance] = delta_angle;
+    _imu._last_raw_gyro[instance] = gyro;
 }
 
 /*