From 3415dfb46c38974aa17d7763111b4bf556e0d2b7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 18 Apr 2017 12:28:47 +1000
Subject: [PATCH] HAL_PX4: implement split transfers for I2C

---
 libraries/AP_HAL_PX4/I2CDevice.cpp | 34 ++++++++++++++++++------------
 libraries/AP_HAL_PX4/I2CDevice.h   |  5 +++++
 libraries/AP_HAL_PX4/I2CWrapper.h  |  2 +-
 3 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/libraries/AP_HAL_PX4/I2CDevice.cpp b/libraries/AP_HAL_PX4/I2CDevice.cpp
index 5dd8375aa4..5b28ab9bd9 100644
--- a/libraries/AP_HAL_PX4/I2CDevice.cpp
+++ b/libraries/AP_HAL_PX4/I2CDevice.cpp
@@ -62,7 +62,7 @@ uint8_t PX4_I2C::map_bus_number(uint8_t bus) const
 /*
   implement wrapper for PX4 I2C driver
  */
-bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)
+bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len, bool split_transfers)
 {
     set_address(address);
     if (!init_done) {
@@ -78,18 +78,26 @@ bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_le
     if (!init_ok) {
         return false;
     }
-    /*
-      splitting the transfer() into two pieces avoids a stop condition
-      with SCL low which is not supported on some devices (such as
-      LidarLite blue label)
-     */
-    if (send && send_len) {
-        if (transfer(send, send_len, nullptr, 0) != OK) {
-            return false;
+
+    if (split_transfers) {
+        /*
+          splitting the transfer() into two pieces avoids a stop condition
+          with SCL low which is not supported on some devices (such as
+          LidarLite blue label)
+        */
+        if (send && send_len) {
+            if (transfer(send, send_len, nullptr, 0) != OK) {
+                return false;
+            }
         }
-    }
-    if (recv && recv_len) {
-        if (transfer(nullptr, 0, recv, recv_len) != OK) {
+        if (recv && recv_len) {
+            if (transfer(nullptr, 0, recv, recv_len) != OK) {
+                return false;
+            }
+        }
+    } else {
+        // combined transfer
+        if (transfer(send, send_len, recv, recv_len) != OK) {
             return false;
         }
     }
@@ -120,7 +128,7 @@ bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
                          uint8_t *recv, uint32_t recv_len)
 {
     perf_begin(perf);
-    bool ret = _px4dev.do_transfer(_address, send, send_len, recv, recv_len);
+    bool ret = _px4dev.do_transfer(_address, send, send_len, recv, recv_len, _split_transfers);
     perf_end(perf);
     return ret;
 }
diff --git a/libraries/AP_HAL_PX4/I2CDevice.h b/libraries/AP_HAL_PX4/I2CDevice.h
index 11f9ecf466..0246dd1ea9 100644
--- a/libraries/AP_HAL_PX4/I2CDevice.h
+++ b/libraries/AP_HAL_PX4/I2CDevice.h
@@ -65,6 +65,10 @@ public:
         return &businfo[_busnum<num_buses?_busnum:0].semaphore;
     }
 
+    void set_split_transfers(bool set) override {
+        _split_transfers = set;
+    }
+    
 private:
     static const uint8_t num_buses = 2;
     static DeviceBus businfo[num_buses];
@@ -74,6 +78,7 @@ private:
     uint8_t _address;
     perf_counter_t perf;
     char *pname;
+    bool _split_transfers;
 };
 
 class I2CDeviceManager : public AP_HAL::I2CDeviceManager {
diff --git a/libraries/AP_HAL_PX4/I2CWrapper.h b/libraries/AP_HAL_PX4/I2CWrapper.h
index ab569626cc..e0f2d9e444 100644
--- a/libraries/AP_HAL_PX4/I2CWrapper.h
+++ b/libraries/AP_HAL_PX4/I2CWrapper.h
@@ -14,7 +14,7 @@ extern const AP_HAL::HAL& hal;
 class PX4::PX4_I2C : public device::I2C {
 public:
     PX4_I2C(uint8_t bus);
-    bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len);
+    bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len, bool split_transfers);
 
     void set_retries(uint8_t retries) {
         _retries = retries;