From 5329e6374262e5ea6839bf1774c881c8880ec62f Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Mon, 3 Aug 2015 16:22:41 -0300
Subject: [PATCH] AP_InertialSensor: allow publishing sample rate to frontend

That information will be used for a "centralized" vibration calculation.
---
 libraries/AP_InertialSensor/AP_InertialSensor.cpp         | 1 +
 libraries/AP_InertialSensor/AP_InertialSensor.h           | 3 +++
 libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp | 6 ++++++
 libraries/AP_InertialSensor/AP_InertialSensor_Backend.h   | 6 ++++++
 4 files changed, 16 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 2a1dbf394e..f847b1411b 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -344,6 +344,7 @@ AP_InertialSensor::AP_InertialSensor() :
 #endif
 
         _accel_max_abs_offsets[i] = 3.5f;
+        _accel_sample_rates[i] = 0;
     }
 #if INS_VIBRATION_CHECK
     for (uint8_t i=0; i<INS_VIBRATION_CHECK_INSTANCES; i++) {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 042e64601e..e15ded11f7 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -308,6 +308,9 @@ private:
     // accelerometer max absolute offsets to be used for calibration
     float _accel_max_abs_offsets[INS_MAX_INSTANCES];
 
+    // accelerometer sample rate in units of Hz
+    uint32_t _accel_sample_rates[INS_MAX_INSTANCES];
+
     // temperatures for an instance if available
     float _temperature[INS_MAX_INSTANCES];
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index 457c0dd396..702d9cd0b0 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -76,6 +76,12 @@ void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance,
     _imu._accel_max_abs_offsets[instance] = max_offset;
 }
 
+void AP_InertialSensor_Backend::_set_accel_sample_rate(uint8_t instance,
+                                                       uint32_t rate)
+{
+    _imu._accel_sample_rates[instance] = rate;
+}
+
 // set accelerometer error_count
 void AP_InertialSensor_Backend::_set_accel_error_count(uint8_t instance, uint32_t error_count)
 {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
index b71f840130..3219dfdf1c 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -91,6 +91,12 @@ protected:
     // set accelerometer max absolute offset for calibration
     void _set_accel_max_abs_offset(uint8_t instance, float offset);
 
+    // set accelerometer sample rate
+    void _set_accel_sample_rate(uint8_t instance, uint32_t rate);
+    uint32_t _accel_sample_rate(uint8_t instance) const {
+        return _imu._accel_sample_rates[instance];
+    }
+
     // publish a temperature value
     void _publish_temperature(uint8_t instance, float temperature);