Merge pull request #99 from rtlopez/gy87-mag

gy-87 compas fix

lib/Espfc/src/Blackbox.h

@@ -368,7 +368,7 @@ class Blackbox
           acc.accADC[i] = _model.state.accel[i] * ACCEL_G_INV * acc.dev.acc_1G;
         }
         if(_model.magActive()) {
-          mag.magADC[i] = _model.state.mag[i];
+          mag.magADC[i] = _model.state.mag[i] * 1090;
         }
         if(_model.baroActive()) {
           baro.altitude = lrintf(_model.state.baroAltitude * 100.f); // cm

lib/Espfc/src/Cli.h

@@ -1200,14 +1200,29 @@ class Cli
         s.println();
         printStats(s);
         s.println();
-        for(size_t i = 0; i < COUNTER_COUNT; ++i)
+        for(int i = 0; i < COUNTER_COUNT; ++i)
         {
-          s.print(FPSTR(_model.state.stats.getName((StatCounter)i)));
+          StatCounter c = (StatCounter)i;
+          int time = lrintf(_model.state.stats.getTime(c));
+          float load = _model.state.stats.getLoad(c);
+          int freq = lrintf(_model.state.stats.getFreq(c));
+
+          s.print(FPSTR(_model.state.stats.getName(c)));
           s.print(": ");
-          s.print((int)(_model.state.stats.getTime((StatCounter)i)), 1);
-          s.print("us, ");
-          s.print(_model.state.stats.getLoad((StatCounter)i), 1);
-          s.print("%");
+          if(time < 100) s.print(' ');
+          if(time < 10) s.print(' ');
+          s.print(time);
+          s.print("us,  ");
+
+          if(load < 10) s.print(' ');
+          s.print(load, 1);
+          s.print("%,  ");
+
+          if(freq < 1000) s.print(' ');
+          if(freq < 100) s.print(' ');
+          if(freq < 10) s.print(' ');
+          s.print(freq);
+          s.print(" Hz");
           s.println();
         }
         s.print(F("  TOTAL: "));

lib/Espfc/src/Device/BusAwareDevice.h

@@ -10,22 +10,25 @@ namespace Device {
 class BusAwareDevice
 {
   public:
-    void setBus(BusDevice * bus, uint8_t addr, uint8_t masterAddr = 0)
+    void setBus(BusDevice * bus, uint8_t addr)
     {
       _bus = bus;
       _addr = addr;
-      _masterAddr = masterAddr;
     }
 
     const BusDevice * getBus() const
     {
       return _bus;
     }
 
+    uint8_t getAddress() const
+    {
+      return _addr;
+    }
+
   protected:
     BusDevice * _bus;
     uint8_t _addr;
-    uint8_t _masterAddr;
 };
 
 }

lib/Espfc/src/Device/BusDevice.h

@@ -3,6 +3,7 @@
 
 #include <functional>
 #include <Arduino.h>
+#include "Math/Bits.h"
 
 #define ESPFC_BUS_TIMEOUT 100
 
@@ -13,6 +14,7 @@ enum BusType {
   BUS_AUTO,
   BUS_I2C,
   BUS_SPI,
+  BUS_SLV,
   BUS_MAX
 };
 
@@ -55,7 +57,7 @@ class BusDevice
     {
       uint8_t b;
       uint8_t count = readByte(devAddr, regAddr, &b);
-      *data = b & (1 << bitNum);
+      *data = Math::getBit(b, bitNum);
       return count;
     }
 
@@ -64,23 +66,17 @@ class BusDevice
       uint8_t count, b;
       if ((count = readByte(devAddr, regAddr, &b)) != 0)
       {
-        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
-        b &= mask;
-        b >>= (bitStart - length + 1);
-        *data = b;
+        *data = Math::getBitsMsb(b, bitStart, length);
       }
       return count;
     }
 
-    int8_t readBitsBMI160(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data)
+    int8_t readBitsLsb(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data)
     {
       uint8_t count, b;
       if ((count = readByte(devAddr, regAddr, &b)) != 0)
       {
-        uint8_t mask = (1 << length) - 1;
-        b >>= bitStart;
-        b &= mask;
-        *data = b;
+        *data = Math::getBitsLsb(b, bitStart, length);
       }
       return count;
     }
@@ -89,7 +85,7 @@ class BusDevice
     {
       uint8_t b;
       readByte(devAddr, regAddr, &b);
-      b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+      b = Math::setBit(b, bitNum, data);
       return writeByte(devAddr, regAddr, b);
     }
 
@@ -98,27 +94,19 @@ class BusDevice
       uint8_t b = 0;
       if (readByte(devAddr, regAddr, &b) != 0)
       {
-        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
-        data <<= (bitStart - length + 1); // shift data into correct position
-        data &= mask; // zero all non-important bits in data
-        b &= ~(mask); // zero all important bits in existing byte
-        b |= data; // combine data with existing byte
+        b = Math::setBitsMsb(b, bitStart, length, data);
         return writeByte(devAddr, regAddr, b);
       } else {
         return false;
       }
     }
 
-    bool writeBitsBMI160(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data)
+    bool writeBitsLsb(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data)
     {
       uint8_t b = 0;
       if (readByte(devAddr, regAddr, &b) != 0)
       {
-        uint8_t mask = ((1 << length) - 1) << bitStart;
-        data <<= bitStart; // shift data into correct position
-        data &= mask; // zero all non-important bits in data
-        b &= ~(mask); // zero all important bits in existing byte
-        b |= data; // combine data with existing byte
+        b = Math::setBitsLsb(b, bitStart, length, data);
         return writeByte(devAddr, regAddr, b);
       } else {
         return false;
@@ -130,9 +118,7 @@ class BusDevice
       uint8_t b = 0;
       if (readByte(devAddr, regAddr, &b) != 0)
       {
-        data &= mask; // zero all non-important bits in data
-        b &= ~(mask); // zero all important bits in existing byte
-        b |= data; // combine data with existing byte
+        b = Math::setMasked(b, mask, data);
         return writeByte(devAddr, regAddr, b);
       } else {
         return false;
@@ -141,7 +127,7 @@ class BusDevice
 
     static const char ** getNames()
     {
-      static const char* busDevChoices[] = { PSTR("NONE"), PSTR("AUTO"), PSTR("I2C"), PSTR("SPI"), NULL };
+      static const char* busDevChoices[] = { PSTR("NONE"), PSTR("AUTO"), PSTR("I2C"), PSTR("SPI"), PSTR("SLV"), NULL };
       return busDevChoices;
     }
 

lib/Espfc/src/Device/BusSlave.h

@@ -0,0 +1,96 @@
+#pragma once
+
+#define MPU6050_I2C_SLV0_ADDR     0x25
+#define MPU6050_I2C_SLV0_REG      0x26
+#define MPU6050_I2C_SLV0_DO       0x63
+#define MPU6050_I2C_SLV0_CTRL     0x27
+#define MPU6050_I2C_SLV0_EN       0x80
+#define MPU6050_EXT_SENS_DATA_00  0x49
+
+namespace Espfc {
+
+namespace Device {
+
+class BusSlave: public BusDevice, public BusAwareDevice
+{
+  public:
+    BusSlave() {}
+
+    int begin(BusDevice * dev, int addr)
+    {
+      setBus(dev, addr);
+
+      return 1;
+    }
+
+    BusType getType() const override { return BUS_SLV; }
+
+    virtual int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override
+    {
+      // set slave 0 to the AK8963 and set for read
+      if(!writeMaster(MPU6050_I2C_SLV0_ADDR, devAddr | 0x80)) {
+        return 0;
+      }
+      // set the register to the desired AK8963 sub address
+      if(!writeMaster(MPU6050_I2C_SLV0_REG, regAddr)) {
+        return 0;
+      }
+      // enable I2C and request the bytes
+      if(!writeMaster(MPU6050_I2C_SLV0_CTRL, MPU6050_I2C_SLV0_EN | length)) {
+        return 0;
+      }
+
+      // takes some time for these registers to fill
+      delay(1);
+
+      // read the bytes off the EXT_SENS_DATA registers
+      int8_t res = readMaster(MPU6050_EXT_SENS_DATA_00, length, data);
+
+      return res;
+    }
+
+    // readFast() ignores devAddr and regAddr args and read ext sensor data reg from master
+    virtual int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override
+    {
+      return _bus->readFast(_addr, MPU6050_EXT_SENS_DATA_00, length, data);
+    }
+
+    // writes only one byte, length is ignored
+    virtual bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override
+    {
+      // set slave 0 to the AK8963 and set for write
+      if(!writeMaster(MPU6050_I2C_SLV0_ADDR, devAddr)) {
+        return false;
+      }
+      // set the register to the desired AK8963 sub address 
+      if(!writeMaster(MPU6050_I2C_SLV0_REG, regAddr)) {
+        return false;
+      }
+      // store the data for write
+      if(!writeMaster(MPU6050_I2C_SLV0_DO, *data)) {
+        return false;
+      }
+      // enable I2C and send 1 byte
+      if(!writeMaster(MPU6050_I2C_SLV0_CTRL, MPU6050_I2C_SLV0_EN | 0x01)) {
+        return false;
+      }
+
+      return true;
+    }
+
+    int8_t writeMaster(uint8_t regAddr, uint8_t data)
+    {
+      int8_t res = _bus->write(_addr, regAddr, 1, &data);
+      delay(10);
+      return res;
+    }
+
+    int8_t readMaster(uint8_t regAddr, uint8_t length, uint8_t *data)
+    {
+      return _bus->read(_addr, regAddr, length, data);
+    }
+};
+
+}
+
+}

lib/Espfc/src/Device/GyroBMI160.h

@@ -155,11 +155,11 @@ class GyroBMI160: public GyroDevice
       delay(1);
 
       // Enable accel offset
-      //_bus->writeBitsBMI160(_addr, BMI160_RA_OFFSET_6, BMI160_ACC_OFFSET_EN, BMI160_ACC_OFFSET_LEN, BMI160_RESULT_OK);
+      //_bus->writeBitsLsb(_addr, BMI160_RA_OFFSET_6, BMI160_ACC_OFFSET_EN, BMI160_ACC_OFFSET_LEN, BMI160_RESULT_OK);
       delay(1);
 
       // Enable gyro offset
-      //_bus->writeBitsBMI160(_addr, BMI160_RA_OFFSET_6, BMI160_GYR_OFFSET_EN, BMI160_GYR_OFFSET_LEN, BMI160_RESULT_OK);
+      //_bus->writeBitsLsb(_addr, BMI160_RA_OFFSET_6, BMI160_GYR_OFFSET_EN, BMI160_GYR_OFFSET_LEN, BMI160_RESULT_OK);
       delay(1);
 
       // Enable data ready interrupt
@@ -229,29 +229,13 @@ class GyroBMI160: public GyroDevice
     {
     }
 
-    void setFullScaleGyroRange(uint8_t range) override
-    {
-    }
-
-    void setFullScaleAccelRange(uint8_t range) override
-    {
-    }
-
     bool testConnection() override
     {
       uint8_t whoami = 0;
       _bus->readByte(_addr, BMI160_RA_CHIP_ID, &whoami);
       //D("bmi160:whoami", _addr, whoami);
       return whoami == BMI160_CHIP_ID_DEFAULT_VALUE;
     }
-
-    void setSleepEnabled(bool enabled)
-    {
-    }
-
-    void setClockSource(uint8_t source)
-    {
-    }
 };
 
 }

lib/Espfc/src/Device/GyroDevice.h

@@ -38,8 +38,6 @@ class GyroDevice: public BusAwareDevice
     virtual void setDLPFMode(uint8_t mode) = 0;
     virtual int getRate() const = 0;
     virtual void setRate(int rate) = 0;
-    virtual void setFullScaleGyroRange(uint8_t range) = 0;
-    virtual void setFullScaleAccelRange(uint8_t range) = 0;
 
     virtual bool testConnection() = 0;