Add MPU6050 code

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);

lib/comms/comms.cpp

@@ -432,12 +432,16 @@ String _wrap_str(String v)
 
 Message::~Message()
 {
-    if (type == SCAN_RESULT || type == SCAN_MAX_RESULT)
+    if (type == SCAN_RESULT || type == SCAN_MAX_RESULT || type == SCAN_HEADING_MAX)
     {
         if (payload.dump.sz > 0)
         {
             delete[] payload.dump.freqs_khz;
             delete[] payload.dump.rssis;
+            if (payload.dump.rssis2)
+            {
+                delete[] payload.dump.rssis2;
+            }
             payload.dump.sz = 0;
         }
 

lib/comms/comms.h

@@ -20,6 +20,7 @@ enum MessageType
     SCAN,
     SCAN_RESULT,
     SCAN_MAX_RESULT,
+    SCAN_HEADING_MAX,
     CONFIG_TASK,
     HEADING,
     _MAX_MESSAGE_TYPE = HEADING
@@ -53,6 +54,8 @@ struct ScanTaskResult
     int16_t *rssis;
     int16_t *rssis2;
     int16_t prssi;
+    int16_t heading_min;
+    int16_t heading_max;
 };
 
 struct ConfigTask

lib/comms/radio_comms.cpp

@@ -144,6 +144,32 @@ uint8_t *_serialize_scan_max_result(Message &m, size_t &p, uint8_t *msg)
     return msg;
 }
 
+uint8_t *_serialize_scan_heading_max(Message &m, size_t &p, uint8_t *msg)
+{
+    if (m.type != SCAN_HEADING_MAX)
+    {
+        return NULL;
+    }
+
+    size_t written = 0;
+    written = _write(msg, p, 0, (uint8_t *)&m.payload.dump.heading_min, 2);
+    written = _write(msg, p, written, (uint8_t *)&m.payload.dump.heading_max, 2);
+
+    uint8_t *sub_msg = msg + written;
+    p -= written;
+    m.type = SCAN_MAX_RESULT;
+    sub_msg = _serialize_scan_max_result(m, p, sub_msg);
+    p += written;
+    m.type = SCAN_HEADING_MAX;
+
+    if (sub_msg == NULL)
+    {
+        msg = NULL;
+    }
+
+    return msg;
+}
+
 uint8_t *_serialize_config_task(Message &m, size_t &p, uint8_t *msg)
 {
     if (m.type != CONFIG_TASK)
@@ -201,6 +227,10 @@ int16_t RadioComms::send(Message &m)
     {
         msg = _serialize_scan_max_result(m, p, msg_buf);
     }
+    else if (m.type == MessageType::SCAN_HEADING_MAX)
+    {
+        msg = _serialize_scan_heading_max(m, p, msg_buf);
+    }
     else if (m.type == MessageType::CONFIG_TASK)
     {
         msg = _serialize_config_task(m, p, msg_buf);
@@ -301,6 +331,7 @@ Message *_deserialize_scan_max_result(size_t len, size_t &p, uint8_t *packet)
     message->payload.dump.sz = dump_sz;
     message->payload.dump.freqs_khz = freqs;
     message->payload.dump.rssis = rssis;
+    message->payload.dump.rssis2 = NULL;
 
     uint32_t scale = 0;
     p = _read(packet, len, p, (uint8_t *)&scale);
@@ -318,6 +349,24 @@ Message *_deserialize_scan_max_result(size_t len, size_t &p, uint8_t *packet)
     return message;
 }
 
+Message *_deserialize_scan_heading_max(size_t len, size_t &p, uint8_t *packet)
+{
+    int16_t heading_min = -999;
+    int16_t heading_max = 0;
+    p = _read(packet, len, p, (uint8_t *)&heading_min, 2);
+    p = _read(packet, len, p, (uint8_t *)&heading_max, 2);
+
+    Message *message = _deserialize_scan_max_result(len, p, packet);
+
+    if (message != NULL)
+    {
+        message->type = SCAN_HEADING_MAX;
+        message->payload.dump.heading_min = heading_min;
+        message->payload.dump.heading_max = heading_max;
+    }
+    return message;
+}
+
 Message *_deserialize_config_task(size_t len, size_t &p, uint8_t *packet)
 {
     Message *message = new Message();
@@ -439,6 +488,10 @@ Message *RadioComms::receive(uint16_t timeout_ms)
     {
         message = _deserialize_scan_max_result(len, p, packet);
     }
+    else if (b == SCAN_HEADING_MAX)
+    {
+        message = _deserialize_scan_heading_max(len, p, packet);
+    }
     else if (b == CONFIG_TASK)
     {
         message = _deserialize_config_task(len, p, packet);