Add MPU6050

lib/comms/bus.cpp

@@ -1,5 +1,4 @@
 #include "bus.h"
-#include <Wire.h>
 
 bool initUARTs(Config &config)
 {
@@ -97,3 +96,139 @@ uint8_t _scanSupportedDevicesOnWire(TwoWire &w, int bus_num)
 
     return res;
 }
+
+uint8_t _write_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t value,
+                        bool skipValue)
+{
+    wire.beginTransmission(addr);
+    size_t s = wire.write(reg);
+    if (s == 1 && !skipValue)
+    {
+        s = wire.write(value);
+    }
+
+    size_t s1 = wire.endTransmission();
+    if (s != 1 && s1 == 0)
+    {
+        return 1; // "data too long to fit in transmit buffer"
+    }
+
+    return s1;
+}
+
+uint8_t _write_registers(TwoWire &wire, uint8_t addr, uint8_t reg, size_t sz,
+                         const uint8_t *value)
+{
+    wire.beginTransmission(addr);
+    size_t s = wire.write(reg);
+    if (s == 1 && sz > 0)
+    {
+        s = wire.write(value, sz);
+    }
+
+    size_t s1 = wire.endTransmission();
+    if (s != sz && s1 == 0)
+    {
+        return 1; // "data too long to fit in transmit buffer"
+    }
+
+    return s1;
+}
+
+int8_t I2Cdev::writeBytes(TwoWire &w, uint8_t addr, uint8_t reg, size_t sz,
+                          const uint8_t *buf)
+{
+    return _write_registers(w, addr, reg, sz, buf);
+}
+
+int8_t I2Cdev::writeWords(TwoWire &w, uint8_t addr, uint8_t reg, size_t sz,
+                          const uint16_t *buf16)
+{
+    uint8_t *buf = (uint8_t *)malloc(sz * 2);
+    uint8_t *p = buf;
+    for (int i = 0; i < sz; i++)
+    {
+        *(p++) = buf16[i] >> 8;
+        *(p++) = (uint8_t)buf16[i];
+    }
+    uint8_t r = writeBytes(w, addr, reg, sz * 2, buf);
+    free(buf);
+    return r;
+}
+
+int8_t I2Cdev::writeBit(TwoWire &wireObj, uint8_t addr, uint8_t reg, size_t bitNum,
+                        uint8_t data)
+{
+    uint8_t b;
+    int8_t r = readBytes(wireObj, addr, reg, 1, &b, I2Cdev::readTimeout);
+    if (r != 1)
+    {
+        return r;
+    }
+
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return writeBytes(wireObj, addr, reg, 1, &b);
+}
+
+int8_t I2Cdev::writeBits(TwoWire &w, uint8_t addr, uint8_t reg, size_t bitn, size_t sz,
+                         uint8_t v)
+{
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitn=4, sz=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b;
+    int8_t r = readBytes(w, addr, reg, 1, &b, I2Cdev::readTimeout);
+    if (r != 1)
+    {
+        return r;
+    }
+
+    uint8_t mask = ((1 << sz) - 1) << (bitn - sz + 1);
+    v <<= (bitn - sz + 1); // shift data into correct position
+    v &= mask;             // zero all non-important bits in data
+    b &= ~(mask);          // zero all important bits in existing byte
+    b |= v;                // combine data with existing byte
+    return writeBytes(w, addr, reg, 1, &b);
+}
+
+int8_t _read_registers(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t *v, size_t sz,
+                       bool skipRegister)
+{
+    if (!skipRegister)
+    {
+        uint8_t s = _write_registers(wire, addr, reg, 0, NULL);
+        if (s != 0)
+        {
+            return s;
+        }
+    }
+
+    uint8_t r = wire.requestFrom(addr, sz);
+    r = wire.readBytes(v, r);
+
+    return r - sz;
+}
+
+uint8_t _read_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t &v,
+                       bool skipRegister)
+{
+    uint8_t r = _read_registers(wire, addr, reg, &v, 1, skipRegister);
+    if (r != 0)
+    {
+        return 1;
+    }
+
+    return 0;
+}
+
+int8_t I2Cdev::readBytes(TwoWire &wireObj, uint8_t addr, uint8_t reg, size_t length,
+                         uint8_t *data, uint16_t timeout)
+{ // timeout is unused right now
+    return _read_registers(wireObj, addr, reg, data, length);
+}
+
+uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;

lib/comms/bus.h

@@ -1,17 +1,20 @@
 #pragma once
+#include <Wire.h>
 #include <config.h>
 
 struct
 {
     String name;
     uint8_t address;
-} known_i2c_devices[] = {{"HMC5883L", 0x1e}, {"QMC5883L", 0x0d}, {" last record ", 0}};
+} known_i2c_devices[] = {
+    {"HMC5883L", 0x1e}, {"QMC5883L", 0x0d}, {"MPU6050", 0x68}, {" last record ", 0}};
 
 enum I2CDevices
 {
     // powers of 2
     HMC5883L = 1,
-    QMC5883L = 2
+    QMC5883L = 2,
+    MPU6050 = 4
 };
 
 extern uint8_t wireDevices;
@@ -39,3 +42,30 @@ bool initSPIs(Config &config);
 bool initUARTs(Config &config);
 
 bool initWires(Config &config);
+
+#define I2CDEV_DEFAULT_READ_TIMEOUT 0
+
+struct I2Cdev
+{
+    static uint16_t readTimeout;
+
+    static int8_t writeBit(TwoWire &w, uint8_t addr, uint8_t reg, size_t bitn, uint8_t v);
+    static int8_t writeBits(TwoWire &w, uint8_t addr, uint8_t reg, size_t bitn, size_t sz,
+                            uint8_t v);
+    static int8_t writeBytes(TwoWire &w, uint8_t addr, uint8_t reg, size_t sz,
+                             const uint8_t *buf);
+    static int8_t writeWords(TwoWire &w, uint8_t addr, uint8_t reg, size_t sz,
+                             const uint16_t *buf);
+
+    static int8_t readBytes(TwoWire &wireObj, uint8_t addr, uint8_t reg, size_t length,
+                            uint8_t *data, uint16_t timeout = I2Cdev::readTimeout);
+};
+
+uint8_t _read_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t &v,
+                       bool skipRegister = false);
+int8_t _read_registers(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t *v, size_t sz,
+                       bool skipRegister = false);
+uint8_t _write_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t value,
+                        bool skipValue = false);
+uint8_t _write_registers(TwoWire &wire, uint8_t addr, uint8_t reg, size_t sz,
+                         const uint8_t *value);