From 0ef914adacda9effbb5f0e2dd89b11433787fc55 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Tue, 23 Jun 2015 11:33:32 -0300
Subject: [PATCH] AP_InertialSensor: don't take SPI semaphore if not necessary
 on LSM9DS0

If the data-ready polling is done entirely on GPIO pins, it isn't necessary to
hold the semaphore before we now we have data to consume. In that case, only
take the SPI semaphore if there's new data available.

On the other hand, if at least one SPI transaction is done in order to check
for new data, then it makes sense to take the semaphore beforehand.
---
 .../AP_InertialSensor_LSM9DS0.cpp             | 32 +++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
index 00f65e25a0..88352844f8 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
@@ -659,6 +659,37 @@ void AP_InertialSensor_LSM9DS0::_set_accel_scale(accel_scale scale)
 /**
  * Timer process to poll for new data from the LSM9DS0.
  */
+#if DRDY_GPIO_PIN_A != 0 && DRDY_GPIO_PIN_G != 0
+void AP_InertialSensor_LSM9DS0::_poll_data()
+{
+    bool gyro_ready = _gyro_data_ready();
+    bool accel_ready = _accel_data_ready();
+
+    if (!gyro_ready && !accel_ready) {
+        return;
+    }
+
+    if (!_spi_sem->take_nonblocking()) {
+        /*
+         *  the semaphore being busy is an expected condition when the
+         *  mainline code is calling wait_for_sample() which will
+         *  grab the semaphore. We return now and rely on the mainline
+         *  code grabbing the latest sample.
+         */
+        return;
+    }
+
+    if (gyro_ready) {
+        _read_data_transaction_g();
+    }
+
+    if (accel_ready) {
+        _read_data_transaction_a();
+    }
+
+    _spi_sem->give();
+}
+#else
 void AP_InertialSensor_LSM9DS0::_poll_data()
 {
     if (!_spi_sem->take_nonblocking()) {
@@ -681,6 +712,7 @@ void AP_InertialSensor_LSM9DS0::_poll_data()
 
     _spi_sem->give();
 }
+#endif /* DRDY_GPIO_PIN_A != 0 && DRDY_GPIO_PIN_G != 0 */
 
 #if DRDY_GPIO_PIN_A != 0
 bool AP_InertialSensor_LSM9DS0::_accel_data_ready()