From 60c29417f1a10bf0196db979b8aaa67de2d23591 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 30 Nov 2016 17:22:33 +1100
Subject: [PATCH] AP_HAL: added uint16 access functions

---
 libraries/AP_HAL/Device.cpp | 26 ++++++++++++++++++++++++++
 libraries/AP_HAL/Device.h   | 14 ++++++++++++++
 2 files changed, 40 insertions(+)

diff --git a/libraries/AP_HAL/Device.cpp b/libraries/AP_HAL/Device.cpp
index 75cc6413eb..d8fc3b497b 100644
--- a/libraries/AP_HAL/Device.cpp
+++ b/libraries/AP_HAL/Device.cpp
@@ -107,3 +107,29 @@ bool AP_HAL::Device::check_next_register(void)
     _checked.next = (_checked.next+1) % _checked.n_set;
     return true;
 }
+
+/**
+ * read 16 bit unsigned integer, little endian
+ *
+ * Return: true on a successful transfer, false on failure.
+ */
+bool AP_HAL::Device::read_uint16_le(uint8_t first_reg, uint16_t &value)
+{
+    // assume we are on a LE platform
+    return read_registers(first_reg, (uint8_t *)&value, 2);
+}
+
+/**
+ * read 16 bit unsigned integer, big endian
+ *
+ * Return: true on a successful transfer, false on failure.
+ */
+bool AP_HAL::Device::read_uint16_be(uint8_t first_reg, uint16_t &value)
+{
+    if (!read_uint16_le(first_reg, value)) {
+        return false;
+    }
+    uint8_t *b = (uint8_t *)&value;
+    value = (((uint16_t)b[0])<<8) | b[1];
+    return true;
+}
diff --git a/libraries/AP_HAL/Device.h b/libraries/AP_HAL/Device.h
index 358e107238..15f5ad110e 100644
--- a/libraries/AP_HAL/Device.h
+++ b/libraries/AP_HAL/Device.h
@@ -105,6 +105,20 @@ public:
         return transfer(&first_reg, 1, recv, recv_len);
     }
 
+    /**
+     * read 16 bit unsigned integer, little endian
+     *
+     * Return: true on a successful transfer, false on failure.
+     */
+    bool read_uint16_le(uint8_t first_reg, uint16_t &value);
+
+    /**
+     * read 16 bit unsigned integer, big endian
+     *
+     * Return: true on a successful transfer, false on failure.
+     */
+    bool read_uint16_be(uint8_t first_reg, uint16_t &value);
+    
     /**
      * Wrapper function over #transfer() to write a byte to the register reg.
      * The transfer is done by sending reg and val in that order.