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.