From 5d0eb4911449497cf9b864750ff5653651a22eca Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 12 Mar 2015 10:02:36 +1100
Subject: [PATCH] AP_InertialSensor: calculate queue depth based on requested
 sample rate

this auto-scales the queue depth for plane, rover and copter
---
 .../AP_InertialSensor/AP_InertialSensor.h     |  8 +++----
 .../AP_InertialSensor_Backend.cpp             |  7 ++++++
 .../AP_InertialSensor_Backend.h               |  3 +++
 .../AP_InertialSensor_PX4.cpp                 | 23 +++++++++++++++----
 .../AP_InertialSensor/AP_InertialSensor_PX4.h |  3 +++
 5 files changed, 35 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index d1f3a49ba0..af0b25ab84 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -48,10 +48,10 @@ public:
 
     // the rate that updates will be available to the application
     enum Sample_rate {
-        RATE_50HZ,
-        RATE_100HZ,
-        RATE_200HZ,
-        RATE_400HZ
+        RATE_50HZ  = 50,
+        RATE_100HZ = 100,
+        RATE_200HZ = 200,
+        RATE_400HZ = 400
     };
 
     /// Perform startup initialisation.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index c577061b68..a87efff5b2 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -116,3 +116,10 @@ uint8_t AP_InertialSensor_Backend::_default_filter(void) const
     }
     return 30;
 }
+
+// return the requested sample rate in Hz
+uint16_t AP_InertialSensor_Backend::get_sample_rate_hz(void) const
+{
+    // enum can be directly cast to Hz
+    return (uint16_t)_imu._sample_rate;
+}
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
index 43600c8869..1e88c6f0c2 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -85,6 +85,9 @@ protected:
     // return the default filter frequency in Hz for the sample rate
     uint8_t _default_filter(void) const;
 
+    // return the requested sample rate in Hz
+    uint16_t get_sample_rate_hz(void) const;
+
     // note that each backend is also expected to have a static detect()
     // function which instantiates an instance of the backend sensor
     // driver if the sensor is available
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
index 89c859f067..be9d9a8169 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
@@ -44,6 +44,19 @@ AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu
     return sensor;
 }
 
+/*
+  calculate the right queue depth for a device with the given sensor
+  sample rate
+ */
+uint8_t AP_InertialSensor_PX4::_queue_depth(uint16_t sensor_sample_rate) const
+{
+    uint16_t requested_sample_rate = get_sample_rate_hz();
+    uint8_t min_depth = (sensor_sample_rate+requested_sample_rate-1)/requested_sample_rate;
+    // add 5ms more worth of queue to account for possible timing jitter
+    uint8_t ret = min_depth + (5 * sensor_sample_rate) / 1000;
+    return ret;
+}
+
 bool AP_InertialSensor_PX4::_init_sensor(void) 
 {
     // assumes max 3 instances
@@ -88,8 +101,8 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
                 ioctl(fd, GYROIOCSHWLOWPASS, 256);
                 // khz sampling
                 ioctl(fd, GYROIOCSSAMPLERATE, 1000);
-                // 10ms queue depth
-                ioctl(fd, SENSORIOCSQUEUEDEPTH, 10);
+                // set queue depth
+                ioctl(fd, SENSORIOCSQUEUEDEPTH, _queue_depth(1000));
                 break;
             case DRV_GYR_DEVTYPE_L3GD20:
                 // hardware LPF as high as possible
@@ -97,7 +110,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
                 // ~khz sampling
                 ioctl(fd, GYROIOCSSAMPLERATE, 800);
                 // 10ms queue depth
-                ioctl(fd, SENSORIOCSQUEUEDEPTH, 8);
+                ioctl(fd, SENSORIOCSQUEUEDEPTH, _queue_depth(800));
                 break;
             default:
                 break;
@@ -120,7 +133,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
                 // khz sampling
                 ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
                 // 10ms queue depth
-                ioctl(fd, SENSORIOCSQUEUEDEPTH, 10);
+                ioctl(fd, SENSORIOCSQUEUEDEPTH, _queue_depth(1000));
                 break;
             case DRV_ACC_DEVTYPE_LSM303D:
                 // hardware LPF to ~1/10th sample rate for antialiasing
@@ -129,7 +142,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
                 ioctl(fd, ACCELIOCSSAMPLERATE, 1600);
                 ioctl(fd,SENSORIOCSPOLLRATE, 1600);
                 // 10ms queue depth
-                ioctl(fd, SENSORIOCSQUEUEDEPTH, 16);
+                ioctl(fd, SENSORIOCSQUEUEDEPTH, _queue_depth(1600));
                 break;
             default:
                 break;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
index 3dac2c1dc8..15bc153411 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
@@ -50,6 +50,9 @@ private:
     bool _get_gyro_sample(uint8_t i, struct gyro_report &gyro_report);
     bool _get_accel_sample(uint8_t i, struct accel_report &accel_report);
 
+    // calculate right queue depth for a sensor
+    uint8_t _queue_depth(uint16_t sensor_sample_rate) const;
+
     // support for updating filter at runtime
     uint8_t _last_filter_hz;