diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h
index 80bc3f23..553e9091 100644
--- a/lib/Espfc/src/Blackbox.h
+++ b/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
diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h
index 393f4014..2be914d7 100644
--- a/lib/Espfc/src/Cli.h
+++ b/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: "));
diff --git a/lib/Espfc/src/Device/BusAwareDevice.h b/lib/Espfc/src/Device/BusAwareDevice.h
index 1f86fda8..10d2fef4 100644
--- a/lib/Espfc/src/Device/BusAwareDevice.h
+++ b/lib/Espfc/src/Device/BusAwareDevice.h
@@ -10,11 +10,10 @@ 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
@@ -22,10 +21,14 @@ class BusAwareDevice
       return _bus;
     }
 
+    uint8_t getAddress() const
+    {
+      return _addr;
+    }
+
   protected:
     BusDevice * _bus;
     uint8_t _addr;
-    uint8_t _masterAddr;
 };
 
 }
diff --git a/lib/Espfc/src/Device/BusDevice.h b/lib/Espfc/src/Device/BusDevice.h
index 3738d263..79ead4ea 100644
--- a/lib/Espfc/src/Device/BusDevice.h
+++ b/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;
     }
 
diff --git a/lib/Espfc/src/Device/BusSlave.h b/lib/Espfc/src/Device/BusSlave.h
new file mode 100644
index 00000000..91c65842
--- /dev/null
+++ b/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);
+    }
+};
+
+}
+
+}
\ No newline at end of file
diff --git a/lib/Espfc/src/Device/GyroBMI160.h b/lib/Espfc/src/Device/GyroBMI160.h
index 49ac363e..200bec07 100644
--- a/lib/Espfc/src/Device/GyroBMI160.h
+++ b/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,14 +229,6 @@ class GyroBMI160: public GyroDevice
     {
     }
 
-    void setFullScaleGyroRange(uint8_t range) override
-    {
-    }
-
-    void setFullScaleAccelRange(uint8_t range) override
-    {
-    }
-
     bool testConnection() override
     {
       uint8_t whoami = 0;
@@ -244,14 +236,6 @@ class GyroBMI160: public GyroDevice
       //D("bmi160:whoami", _addr, whoami);
       return whoami == BMI160_CHIP_ID_DEFAULT_VALUE;
     }
-
-    void setSleepEnabled(bool enabled)
-    {
-    }
-
-    void setClockSource(uint8_t source)
-    {
-    }
 };
 
 }
diff --git a/lib/Espfc/src/Device/GyroDevice.h b/lib/Espfc/src/Device/GyroDevice.h
index 47a7993f..7967cd07 100644
--- a/lib/Espfc/src/Device/GyroDevice.h
+++ b/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;
 
diff --git a/lib/Espfc/src/Device/GyroICM20602.h b/lib/Espfc/src/Device/GyroICM20602.h
index 462e4017..508c1487 100644
--- a/lib/Espfc/src/Device/GyroICM20602.h
+++ b/lib/Espfc/src/Device/GyroICM20602.h
@@ -2,197 +2,38 @@
 #define _ESPFC_DEVICE_GYRO_ICM20602_H_
 
 #include "BusDevice.h"
-#include "GyroDevice.h"
+#include "GyroMPU6050.h"
 #include "helper_3dmath.h"
 #include "Debug_Espfc.h"
 
-#define ICM20602_ADDRESS_FIRST       0x68 // address pin low (GND), default for InvenSense evaluation board
-#define ICM20602_ADDRESS_SECOND      0x69 // address pin high (VCC)
-
-#define ICM20602_RA_SMPLRT_DIV       0x19
-#define ICM20602_RA_CONFIG           0x1A
-#define ICM20602_RA_GYRO_CONFIG      0x1B
-#define ICM20602_RA_ACCEL_CONFIG     0x1C
-#define ICM20602_RA_ACCEL_XOUT_H     0x3B
-#define ICM20602_RA_ACCEL_XOUT_L     0x3C
-#define ICM20602_RA_ACCEL_YOUT_H     0x3D
-#define ICM20602_RA_ACCEL_YOUT_L     0x3E
-#define ICM20602_RA_ACCEL_ZOUT_H     0x3F
-#define ICM20602_RA_ACCEL_ZOUT_L     0x40
-#define ICM20602_RA_TEMP_OUT_H       0x41
-#define ICM20602_RA_TEMP_OUT_L       0x42
-#define ICM20602_RA_GYRO_XOUT_H      0x43
-#define ICM20602_RA_GYRO_XOUT_L      0x44
-#define ICM20602_RA_GYRO_YOUT_H      0x45
-#define ICM20602_RA_GYRO_YOUT_L      0x46
-#define ICM20602_RA_GYRO_ZOUT_H      0x47
-#define ICM20602_RA_GYRO_ZOUT_L      0x48
-#define ICM20602_RA_PWR_MGMT_1       0x6B
-#define ICM20602_RA_PWR_MGMT_2       0x6C
-#define ICM20602_RA_FIFO_COUNTH      0x72
-#define ICM20602_RA_FIFO_COUNTL      0x73
-#define ICM20602_RA_FIFO_R_W         0x74
-#define ICM20602_RA_WHO_AM_I         0x75
-
-
-#define ICM20602_PWR1_CLKSEL_BIT         2
-#define ICM20602_PWR1_CLKSEL_LENGTH      3
-#define ICM20602_PWR1_SLEEP_BIT          6
-
-#define ICM20602_CLOCK_PLL_XGYRO         0x01
-
-#define ICM20602_CFG_DLPF_CFG_BIT    2
-#define ICM20602_CFG_DLPF_CFG_LENGTH 3
-
-#define ICM20602_DLPF_BW_256         0x00
-#define ICM20602_DLPF_BW_188         0x01
-#define ICM20602_DLPF_BW_98          0x02
-#define ICM20602_DLPF_BW_42          0x03
-#define ICM20602_DLPF_BW_20          0x04
-#define ICM20602_DLPF_BW_10          0x05
-#define ICM20602_DLPF_BW_5           0x06
-
-#define ICM20602_GCONFIG_FS_SEL_BIT      4
-#define ICM20602_GCONFIG_FS_SEL_LENGTH   2
-
-#define ICM20602_GYRO_FS_250         0x00
-#define ICM20602_GYRO_FS_500         0x01
-#define ICM20602_GYRO_FS_1000        0x02
-#define ICM20602_GYRO_FS_2000        0x03
-
-#define ICM20602_ACCEL_FS_2          0x00
-#define ICM20602_ACCEL_FS_4          0x01
-#define ICM20602_ACCEL_FS_8          0x02
-#define ICM20602_ACCEL_FS_16         0x03
-
-#define ICM20602_RESET               0x80
-
-#define ICM20602_ACONFIG_AFS_SEL_BIT         4
-#define ICM20602_ACONFIG_AFS_SEL_LENGTH      2
-
-#define ICM20602_WHO_AM_I_BIT        6
-#define ICM20602_WHO_AM_I_LENGTH     6
-
-#define ICM20602_USERCTRL_FIFO_EN_BIT            6
-#define ICM20602_USERCTRL_FIFO_RESET_BIT         2
-
+#define ICM20602_RA_ACCEL2_CONFIG     0x1D
+#define ICM20602_WHOAMI_DEFAULT_VALUE 0x12
 
 namespace Espfc {
 
 namespace Device {
 
-class GyroICM20602: public GyroDevice
+class GyroICM20602: public GyroMPU6050
 {
   public:
-    int begin(BusDevice * bus) override
-    {
-      return begin(bus, ICM20602_ADDRESS_FIRST) ? 1 : begin(bus, ICM20602_ADDRESS_SECOND) ? 1 : 0;
-    }
-
-    int begin(BusDevice * bus, uint8_t addr) override
-    {
-      setBus(bus, addr);
-
-      if(!testConnection()) return 0;
-
-      _bus->writeByte(_addr, ICM20602_RA_PWR_MGMT_1, ICM20602_RESET);
-      delay(100);
-
-      setClockSource(ICM20602_CLOCK_PLL_XGYRO);
-      delay(15);
-
-      setSleepEnabled(false);
-
-      return 1;
-    }
-
     GyroDeviceType getType() const override
     {
       return GYRO_ICM20602;
     }
 
-    int readGyro(VectorInt16& v) override
-    {
-      uint8_t buffer[6];
-
-      _bus->readFast(_addr, ICM20602_RA_GYRO_XOUT_H, 6, buffer);
-
-      v.x = (((int16_t)buffer[0]) << 8) | buffer[1];
-      v.y = (((int16_t)buffer[2]) << 8) | buffer[3];
-      v.z = (((int16_t)buffer[4]) << 8) | buffer[5];
-
-      return 1;
-    }
-
-    int readAccel(VectorInt16& v) override
-    {
-      uint8_t buffer[6];
-
-      _bus->readFast(_addr, ICM20602_RA_ACCEL_XOUT_H, 6, buffer);
-
-      v.x = (((int16_t)buffer[0]) << 8) | buffer[1];
-      v.y = (((int16_t)buffer[2]) << 8) | buffer[3];
-      v.z = (((int16_t)buffer[4]) << 8) | buffer[5];
-
-      return 1;
-    }
-
     void setDLPFMode(uint8_t mode) override
     {
-      _dlpf = mode;
-      _bus->writeBits(_addr, ICM20602_RA_CONFIG, ICM20602_CFG_DLPF_CFG_BIT, ICM20602_CFG_DLPF_CFG_LENGTH, mode);
-    }
-
-    int getRate() const override
-    {
-      switch(_dlpf)
-      {
-        case GYRO_DLPF_256:
-        case GYRO_DLPF_EX:
-          return 8000;
-      }
-      return 1000;
-    }
-
-    void setRate(int rate) override
-    {
-      // The Sample Rate is generated by dividing the gyroscope output rate by SMPLRT_DIV:
-      // Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
-      // where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7),
-      // and 1kHz when the DLPF is enabled (see Register 26).
-      const uint8_t divider = getRate() / (rate + 1);
-      _bus->writeByte(_addr, ICM20602_RA_SMPLRT_DIV, divider);
-    }
-
-    void setFullScaleGyroRange(uint8_t range) override
-    {
-      _bus->writeBits(_addr, ICM20602_RA_GYRO_CONFIG, ICM20602_GCONFIG_FS_SEL_BIT, ICM20602_GCONFIG_FS_SEL_LENGTH, range);
-    }
-
-    void setFullScaleAccelRange(uint8_t range) override
-    {
-      _bus->writeBits(_addr, ICM20602_RA_ACCEL_CONFIG, ICM20602_ACONFIG_AFS_SEL_BIT, ICM20602_ACONFIG_AFS_SEL_LENGTH, range);
+      GyroMPU6050::setDLPFMode(mode);
+      _bus->writeByte(_addr, ICM20602_RA_ACCEL2_CONFIG, mode);
     }
 
     bool testConnection() override
     {
       uint8_t whoami = 0;
-      _bus->readByte(_addr, ICM20602_RA_WHO_AM_I, &whoami);
-      //D("mpu6050:whoami", _addr, whoami);
-      return whoami == 0x12;
+      _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
+      //D("icm20602:whoami", _addr, whoami);
+      return whoami == ICM20602_WHOAMI_DEFAULT_VALUE;
     }
-
-    void setSleepEnabled(bool enabled)
-    {
-      _bus->writeBit(_addr, ICM20602_RA_PWR_MGMT_1, ICM20602_PWR1_SLEEP_BIT, enabled);
-    }
-
-    void setClockSource(uint8_t source)
-    {
-      _bus->writeBits(_addr, ICM20602_RA_PWR_MGMT_1, ICM20602_PWR1_CLKSEL_BIT, ICM20602_PWR1_CLKSEL_LENGTH, source);
-    }
-
-    uint8_t _dlpf;
 };
 
 }
diff --git a/lib/Espfc/src/Device/GyroLSM6DSO.h b/lib/Espfc/src/Device/GyroLSM6DSO.h
index 66577607..06ac9e9e 100644
--- a/lib/Espfc/src/Device/GyroLSM6DSO.h
+++ b/lib/Espfc/src/Device/GyroLSM6DSO.h
@@ -150,14 +150,6 @@ class GyroLSM6DSO: public GyroDevice
     {
     }
 
-    void setFullScaleGyroRange(uint8_t range) override
-    {
-    }
-
-    void setFullScaleAccelRange(uint8_t range) override
-    {
-    }
-
     bool testConnection() override
     {
       uint8_t whoami = 0;
@@ -165,14 +157,6 @@ class GyroLSM6DSO: public GyroDevice
       //D("lsm6dso:whoami", _addr, whoami);
       return whoami == 0x6C || whoami == 0x69;
     }
-
-    void setSleepEnabled(bool enabled)
-    {
-    }
-
-    void setClockSource(uint8_t source)
-    {
-    }
 };
 
 }
diff --git a/lib/Espfc/src/Device/GyroMPU6050.h b/lib/Espfc/src/Device/GyroMPU6050.h
index 9d2615d7..92e3b9ba 100644
--- a/lib/Espfc/src/Device/GyroMPU6050.h
+++ b/lib/Espfc/src/Device/GyroMPU6050.h
@@ -1,6 +1,9 @@
 #ifndef _ESPFC_DEVICE_GYRO_MPU6050_H_
 #define _ESPFC_DEVICE_GYRO_MPU6050_H_
 
+// https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/MPU6050.cpp#L1501
+// https://github.com/guywithaview/Arduino-Test/blob/master/GY87/GY87.ino
+
 #include "BusDevice.h"
 #include "GyroDevice.h"
 #include "helper_3dmath.h"
@@ -66,6 +69,7 @@
 #define MPU6050_ACCEL_FS_16         0x03
 
 #define MPU6050_RESET               0x80
+#define MPU6050_SIG_COND_RESET      0x01
 
 #define MPU6050_ACONFIG_AFS_SEL_BIT         4
 #define MPU6050_ACONFIG_AFS_SEL_LENGTH      2
@@ -76,6 +80,23 @@
 #define MPU6050_USERCTRL_FIFO_EN_BIT            6
 #define MPU6050_USERCTRL_FIFO_RESET_BIT         2
 
+#define MPU6050_USER_CTRL         0x6A
+#define MPU6050_I2C_MST_EN        0x20
+#define MPU6050_I2C_IF_DIS        0x10
+#define MPU6050_I2C_MST_400       0x0D
+#define MPU6050_I2C_MST_500       0x09
+#define MPU6050_I2C_MST_CTRL      0x24
+#define MPU6050_I2C_MST_RESET     0x02
+
+#define MPU6050_I2C_MST_DELAY_CTRL 0x67
+#define MPU6050_I2C_SLV0_DLY_EN    0x01
+#define MPU6050_I2C_DELAY_ES_SHADOW 0x80
+
+#define MPU6050_I2C_SLV4_CTRL     0x34
+#define MPU6050_I2C_MST_DLY_15    0x0f
+
+#define MPU6050_INT_PIN_CFG       0x37
+#define MPU6050_I2C_BYPASS_EN     0x02
 
 namespace Espfc {
 
@@ -86,7 +107,7 @@ class GyroMPU6050: public GyroDevice
   public:
     int begin(BusDevice * bus) override
     {
-      return begin(bus, MPU6050_ADDRESS_FIRST) ? 1 : begin(bus, MPU6050_ADDRESS_SECOND) ? 1 : 0;
+      return begin(bus, MPU6050_ADDRESS_FIRST) ? 1 : begin(bus, MPU6050_ADDRESS_SECOND);
     }
 
     int begin(BusDevice * bus, uint8_t addr) override
@@ -95,13 +116,70 @@ class GyroMPU6050: public GyroDevice
 
       if(!testConnection()) return 0;
 
-      _bus->writeByte(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_RESET);
+      uint8_t res = 0;
+
+      res = _bus->writeByte(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_RESET);
+      //D("mpu6050:reset", res);
       delay(100);
 
-      setClockSource(MPU6050_CLOCK_PLL_XGYRO);
+      // disable sleep mode and set clock source
+      res = _bus->writeByte(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_CLOCK_PLL_XGYRO);
+      //D("mpu6050:sleep_pll", res);
       delay(15);
 
-      setSleepEnabled(false);
+      // reset I2C master and sensors signal path
+      res = _bus->writeByte(_addr, MPU6050_USER_CTRL, MPU6050_I2C_MST_RESET | MPU6050_SIG_COND_RESET);
+      //D("mpu6050:sig_reset", res);
+      delay(15);
+
+      // temporary force 1k gyro rate for mag initiation, will be overwritten in GyroSensor
+      //setDLPFMode(GYRO_DLPF_188);
+      //setRate(100);
+
+      setDLPFMode(GYRO_DLPF_256);
+      setRate(8000);
+      delay(10);
+
+      // gyro range to 2000 deg/s
+      _bus->writeByte(_addr, MPU6050_RA_GYRO_CONFIG, MPU6050_GYRO_FS_2000 << 3);
+
+      // accel range to 16G
+      _bus->writeByte(_addr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACCEL_FS_16 << 3);
+
+      bool isSpi = _bus->isSPI();
+
+      uint8_t userCtrl = MPU6050_I2C_MST_EN;
+      if(isSpi)
+      {
+        userCtrl |= MPU6050_I2C_IF_DIS;
+      }
+
+      if(true)
+      {
+        // reset I2C master
+        //_bus->writeByte(_addr, MPU6050_USER_CTRL, MPU6050_I2C_MST_RESET);
+
+        // enable I2C master mode, and disable I2C
+        res = _bus->writeByte(_addr, MPU6050_USER_CTRL, userCtrl);
+        //D("mpu6050:i2c_master_en", b, res);
+
+        // set the I2C bus speed to 400 kHz
+        res = _bus->writeByte(_addr, MPU6050_I2C_MST_CTRL, MPU6050_I2C_MST_400);
+        //D("mpu6050:i2c_master_speed", b, res);
+
+        // set i2c master delay and enable for slave 0
+        res = _bus->writeByte(_addr, MPU6050_I2C_SLV4_CTRL, MPU6050_I2C_MST_DLY_15);
+        res = _bus->writeByte(_addr, MPU6050_I2C_MST_DELAY_CTRL, MPU6050_I2C_DELAY_ES_SHADOW | MPU6050_I2C_SLV0_DLY_EN);
+      }
+      else
+      {
+        // enable I2C bypass mode
+        res = _bus->writeByte(_addr, MPU6050_INT_PIN_CFG, MPU6050_I2C_BYPASS_EN);
+        //D("mpu6050:i2c_bypass", res);
+      }
+      delay(10);
+
+      (void)res;
 
       return 1;
     }
@@ -140,7 +218,9 @@ class GyroMPU6050: public GyroDevice
     void setDLPFMode(uint8_t mode) override
     {
       _dlpf = mode;
-      _bus->writeBits(_addr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode);
+      bool res = _bus->writeByte(_addr, MPU6050_RA_CONFIG, mode);
+      //D("mpu6050:dlpf", mode, res);
+      (void)res;
     }
 
     int getRate() const override
@@ -160,36 +240,19 @@ class GyroMPU6050: public GyroDevice
       // Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
       // where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7),
       // and 1kHz when the DLPF is enabled (see Register 26).
-      const uint8_t divider = getRate() / (rate + 1);
-      _bus->writeByte(_addr, MPU6050_RA_SMPLRT_DIV, divider);
-    }
-
-    void setFullScaleGyroRange(uint8_t range) override
-    {
-      _bus->writeBits(_addr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
-    }
-
-    void setFullScaleAccelRange(uint8_t range) override
-    {
-      _bus->writeBits(_addr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
+      // therefore: SMPLRT_DIV = (Gyroscope Output Rate / Sample Rate) - 1
+      const uint8_t divider = (getRate() / rate) - 1;
+      uint8_t res = _bus->writeByte(_addr, MPU6050_RA_SMPLRT_DIV, divider);
+      //D("mpu6050:rate", rate, divider, res);
+      (void)res;
     }
 
     bool testConnection() override
     {
       uint8_t whoami = 0;
-      _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
-      //D("mpu6050:whoami", _addr, whoami);
-      return whoami == 0x68 || whoami == 0x72;
-    }
-
-    void setSleepEnabled(bool enabled)
-    {
-      _bus->writeBit(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
-    }
-
-    void setClockSource(uint8_t source)
-    {
-      _bus->writeBits(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
+      uint8_t len = _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
+      //D("mpu6050:whoami", _addr, whoami, len);
+      return len == 1 && (whoami == 0x68 || whoami == 0x72);
     }
 
     uint8_t _dlpf;
diff --git a/lib/Espfc/src/Device/GyroMPU6500.h b/lib/Espfc/src/Device/GyroMPU6500.h
index 7fb7bf3e..63f5f612 100644
--- a/lib/Espfc/src/Device/GyroMPU6500.h
+++ b/lib/Espfc/src/Device/GyroMPU6500.h
@@ -6,18 +6,9 @@
 #include "helper_3dmath.h"
 #include "Debug_Espfc.h"
 
-#define MPU6500_USER_CTRL         0x6A
-#define MPU6500_I2C_MST_EN        0x20
-#define MPU6500_I2C_IF_DIS        0x10
-#define MPU6500_I2C_MST_400       0x0D
-#define MPU6500_I2C_MST_500       0x09
-#define MPU6500_I2C_MST_CTRL      0x24
-#define MPU6500_I2C_MST_RESET     0x02
-
-#define MPU6500_ACCEL_CONF2       0x1D
-
+#define MPU6500_ACCEL_CONF2          0x1D
 #define MPU6500_WHOAMI_DEFAULT_VALUE 0x70
-#define MPU6555_WHOAMI_DEFAULT_VALUE 0x75
+#define MPU6500_WHOAMI_ALT_VALUE     0x75
 
 namespace Espfc {
 
@@ -26,55 +17,6 @@ namespace Device {
 class GyroMPU6500: public GyroMPU6050
 {
   public:
-    int begin(BusDevice * bus) override
-    {
-      return begin(bus, MPU6050_ADDRESS_FIRST) ? 1 : begin(bus, MPU6050_ADDRESS_SECOND) ? 1 : 0;
-    }
-
-    int begin(BusDevice * bus, uint8_t addr) override
-    {
-      setBus(bus, addr);
-
-      if(!testConnection()) return 0;
-
-      _bus->writeByte(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_RESET);
-      delay(100);
-
-      setClockSource(MPU6050_CLOCK_PLL_XGYRO);
-
-      setSleepEnabled(false);
-      delay(100);      
-
-      // reset I2C master
-      //_bus->writeByte(_addr, MPU6500_USER_CTRL, MPU6500_I2C_MST_RESET);
-
-      // disable I2C master to reset slave registers allocation
-      _bus->writeByte(_addr, MPU6500_USER_CTRL, 0);
-      //delay(100);
-
-      // temporary force 1k sample rate for mag initiation, will be overwritten in GyroSensor
-      setDLPFMode(GYRO_DLPF_188);
-      setRate(9); // 1000 / (9+1) = 100hz
-      delay(100);
-
-      // enable I2C master mode, and disable I2C if SPI
-      if(_bus->isSPI())
-      {
-        _bus->writeByte(_addr, MPU6500_USER_CTRL, MPU6500_I2C_MST_EN | MPU6500_I2C_IF_DIS);
-      }
-      else
-      {
-        _bus->writeByte(_addr, MPU6500_USER_CTRL, MPU6500_I2C_MST_EN);
-      }
-      //delay(100);
-
-      // set the I2C bus speed to 400 kHz
-      _bus->writeByte(_addr, MPU6500_I2C_MST_CTRL, MPU6500_I2C_MST_400);
-      //delay(100);
-
-      return 1;
-    }
-
     GyroDeviceType getType() const override
     {
       return GYRO_MPU6500;
@@ -86,13 +28,12 @@ class GyroMPU6500: public GyroMPU6050
       _bus->writeByte(_addr, MPU6500_ACCEL_CONF2, mode);
     }
 
-
     bool testConnection() override
     {
       uint8_t whoami = 0;
-      _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
+      uint8_t len = _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
       //D("MPU6500:whoami", _addr, whoami);
-      return whoami == MPU6500_WHOAMI_DEFAULT_VALUE || whoami == MPU6555_WHOAMI_DEFAULT_VALUE;
+      return len == 1 && (whoami == MPU6500_WHOAMI_DEFAULT_VALUE || whoami == MPU6500_WHOAMI_ALT_VALUE);
     }
 };
 
diff --git a/lib/Espfc/src/Device/GyroMPU9250.h b/lib/Espfc/src/Device/GyroMPU9250.h
index d9112710..e1b46295 100644
--- a/lib/Espfc/src/Device/GyroMPU9250.h
+++ b/lib/Espfc/src/Device/GyroMPU9250.h
@@ -6,15 +6,9 @@
 #include "helper_3dmath.h"
 #include "Debug_Espfc.h"
 
-#define MPU9250_USER_CTRL         0x6A
-#define MPU9250_I2C_MST_EN        0x20
-#define MPU9250_I2C_IF_DIS        0x10
-#define MPU9250_I2C_MST_400       0x0D
-#define MPU9250_I2C_MST_500       0x09
-#define MPU9250_I2C_MST_CTRL      0x24
-#define MPU9250_I2C_MST_RESET     0x02
-
-#define MPU9250_ACCEL_CONF2       0x1D
+#define MPU9250_ACCEL_CONF2          0x1D
+#define MPU9250_WHOAMI_DEFAULT_VALUE 0x71
+#define MPU9250_WHOAMI_ALT_VALUE     0x73
 
 namespace Espfc {
 
@@ -23,55 +17,6 @@ namespace Device {
 class GyroMPU9250: public GyroMPU6050
 {
   public:
-    int begin(BusDevice * bus) override
-    {
-      return begin(bus, MPU6050_ADDRESS_FIRST) ? 1 : begin(bus, MPU6050_ADDRESS_SECOND) ? 1 : 0;
-    }
-
-    int begin(BusDevice * bus, uint8_t addr) override
-    {
-      setBus(bus, addr);
-
-      if(!testConnection()) return 0;
-
-      _bus->writeByte(_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_RESET);
-      delay(100);
-
-      setClockSource(MPU6050_CLOCK_PLL_XGYRO);
-
-      setSleepEnabled(false);
-      delay(100);      
-
-      // reset I2C master
-      //_bus->writeByte(_addr, MPU9250_USER_CTRL, MPU9250_I2C_MST_RESET);
-
-      // disable I2C master to reset slave registers allocation
-      _bus->writeByte(_addr, MPU9250_USER_CTRL, 0);
-      //delay(100);
-
-      // temporary force 1k sample rate for mag initiation, will be overwritten in GyroSensor
-      setDLPFMode(GYRO_DLPF_188);
-      setRate(9); // 1000 / (9+1) = 100hz
-      delay(100);
-
-      // enable I2C master mode, and disable I2C if SPI
-      if(_bus->isSPI())
-      {
-        _bus->writeByte(_addr, MPU9250_USER_CTRL, MPU9250_I2C_MST_EN | MPU9250_I2C_IF_DIS);
-      }
-      else
-      {
-        _bus->writeByte(_addr, MPU9250_USER_CTRL, MPU9250_I2C_MST_EN);
-      }
-      //delay(100);
-
-      // set the I2C bus speed to 400 kHz
-      _bus->writeByte(_addr, MPU9250_I2C_MST_CTRL, MPU9250_I2C_MST_400);
-      //delay(100);
-
-      return 1;
-    }
-
     GyroDeviceType getType() const override
     {
       return GYRO_MPU9250;
@@ -83,13 +28,12 @@ class GyroMPU9250: public GyroMPU6050
       _bus->writeByte(_addr, MPU9250_ACCEL_CONF2, mode);
     }
 
-
     bool testConnection() override
     {
       uint8_t whoami = 0;
-      _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
+      uint8_t len = _bus->readByte(_addr, MPU6050_RA_WHO_AM_I, &whoami);
       //D("mpu9250:whoami", _addr, whoami);
-      return whoami == 0x71 || whoami == 0x73;
+      return len == 1 && (whoami == MPU9250_WHOAMI_DEFAULT_VALUE || whoami == MPU9250_WHOAMI_ALT_VALUE);
     }
 };
 
diff --git a/lib/Espfc/src/Device/MagAK8963.h b/lib/Espfc/src/Device/MagAK8963.h
index 7a9324f9..da1515b6 100644
--- a/lib/Espfc/src/Device/MagAK8963.h
+++ b/lib/Espfc/src/Device/MagAK8963.h
@@ -4,18 +4,8 @@
 #include "MagDevice.h"
 #include "BusDevice.h"
 
-#define MPU9250_ADDRESS_FIRST     0x68
-#define MPU9250_ADDRESS_SECOND    0x68
-#define MPU9250_I2C_SLV0_ADDR     0x25
-#define MPU9250_I2C_SLV0_REG      0x26
-#define MPU9250_I2C_SLV0_DO       0x63
-#define MPU9250_I2C_SLV0_CTRL     0x27
-#define MPU9250_I2C_SLV0_EN       0x80
-#define MPU9250_WHO_AM_I          0x75
-#define MPU9250_EXT_SENS_DATA_00  0x49
-
-#define AK8963_ADDRESS            0x0C // this device only has one address
-#define AK8963_DEFAULT_ADDRESS    AK8963_ADDRESS
+#define AK8963_ADDRESS_FIRST      0x0C // this device only has one address
+#define AK8963_ADDRESS_SECOND     0x0D // 0x0E and 0x0F also possible
 #define AK8963_HXL                0x03
 #define AK8963_CNTL1              0x0A
 #define AK8963_PWR_DOWN           0x00
@@ -37,69 +27,51 @@ class MagAK8963: public MagDevice
   public:
     int begin(BusDevice * bus) override
     {
-      return begin(bus, AK8963_DEFAULT_ADDRESS, MPU9250_ADDRESS_FIRST) ? 1 : begin(bus, AK8963_DEFAULT_ADDRESS, MPU9250_ADDRESS_SECOND) ? 1 : 0;
+      return begin(bus, AK8963_ADDRESS_FIRST) ? 1 : begin(bus, AK8963_ADDRESS_SECOND);
     }
 
     int begin(BusDevice * bus, uint8_t addr) override
     {
-      if(bus->getType() == BUS_I2C)
-      {
-        return begin(bus, AK8963_DEFAULT_ADDRESS, MPU9250_ADDRESS_FIRST) ? 1 : begin(bus, AK8963_DEFAULT_ADDRESS, MPU9250_ADDRESS_SECOND) ? 1 : 0;
-      }
-      else
-      {
-        return begin(bus, addr, addr); // for SPI addr is CS pin
-      }
-    }
-
-    int begin(BusDevice * bus, uint8_t addr, uint8_t masterAddr) override
-    {
-      setBus(bus, addr, masterAddr);
-
-      if(!testMasterConnection()) return 0;
+      setBus(bus, addr);
 
-      //testConnection(); // wtf?
       if(!testConnection()) return 0;
 
-      init();
-
-      return 1;
-    }
-
-    void init()
-    {
       // set AK8963 to Power Down
-      writeSlave(AK8963_CNTL1, AK8963_PWR_DOWN);
+      _bus->writeByte(_addr, AK8963_CNTL1, AK8963_PWR_DOWN);
       delay(AK8963_INIT_DELAY); // long wait between AK8963 mode changes
 
       // set AK8963 to FUSE ROM access
-      writeSlave(AK8963_CNTL1, AK8963_FUSE_ROM);
+      _bus->writeByte(_addr, AK8963_CNTL1, AK8963_FUSE_ROM);
       delay(AK8963_INIT_DELAY);
 
       /* get the magnetometer calibration */
       // read the AK8963 ASA registers and compute magnetometer scale factors
-      readSlave(AK8963_ASA, 3, buffer);
+      _bus->read(_addr, AK8963_ASA, 3, buffer);
       // align CW90_FLIP (swap X Y)
       scale.y = ((((float)buffer[0]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
       scale.x = ((((float)buffer[1]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
       scale.z = ((((float)buffer[2]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
-      scale *= 10.f; // convert to milli Gauss
+      //scale *= 10.f; // convert to milli Gauss
+      scale *= 0.01f; // convert to Gauss
 
       // set AK8963 to Power Down
-      writeSlave(AK8963_CNTL1, AK8963_PWR_DOWN);
+      _bus->writeByte(_addr, AK8963_CNTL1, AK8963_PWR_DOWN);
       delay(AK8963_INIT_DELAY);
 
       // set AK8963 to 16 bit resolution, 100 Hz update rate
-      writeSlave(AK8963_CNTL1, AK8963_CNT_MEAS2);
+      _bus->writeByte(_addr, AK8963_CNTL1, AK8963_CNT_MEAS2);
       delay(AK8963_INIT_DELAY);
 
-      // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
-      readSlave(AK8963_HXL, 7, buffer);
+      // instruct master to get 7 bytes of data from the AK8963 at the sample rate
+      _bus->read(_addr, AK8963_HXL, 7, buffer);
+
+      return 1;
     }
 
     int readMag(VectorInt16& v) override
     {
-      _bus->readFast(_masterAddr, MPU9250_EXT_SENS_DATA_00, 6, buffer);
+      _bus->readFast(_addr, AK8963_HXL, 7, buffer);
+
       // align CW90_FLIP (swap X Y, invert Z)
       v.y =  ((((int16_t)buffer[1]) << 8) | buffer[0]);
       v.x =  ((((int16_t)buffer[3]) << 8) | buffer[2]);
@@ -122,77 +94,14 @@ class MagAK8963: public MagDevice
       return MAG_AK8963;
     }
 
-    bool testMasterConnection()
-    {
-      if(readMaster(MPU9250_WHO_AM_I, 1, buffer) != 1) return false;
-      return buffer[0] == 0x71 || buffer[0] == 0x73;
-    }
-
     bool testConnection() override
     {
-      if(readSlave(AK8963_WHO_AM_I, 1, buffer) != 1) return false;
-      return buffer[0] == 0x48;
+      uint8_t res = _bus->read(_addr, AK8963_WHO_AM_I, 1, buffer);
+      //D("ak8963:whoami", _addr, buffer[0], res);
+      return res == 1 && buffer[0] == 0x48;
     }
 
   private:
-    int8_t writeMaster(uint8_t regAddr, uint8_t data)
-    {
-      int8_t res = _bus->writeByte(_masterAddr, regAddr, data);
-      delay(10);
-      return res;
-    }
-
-    int8_t readMaster(uint8_t regAddr, uint8_t length, uint8_t *data)
-    {
-      return _bus->read(_masterAddr, regAddr, length, data);
-    }
-
-    int8_t readSlave(uint8_t regAddr, uint8_t length, uint8_t *data)
-    {
-      // set slave 0 to the AK8963 and set for read
-      if(!writeMaster(MPU9250_I2C_SLV0_ADDR, AK8963_ADDRESS | 0x80)) {
-        return -1;
-      }
-      // set the register to the desired AK8963 sub address
-      if(!writeMaster(MPU9250_I2C_SLV0_REG, regAddr)) {
-        return -2;
-      }
-      // enable I2C and request the bytes
-      if(!writeMaster(MPU9250_I2C_SLV0_CTRL, MPU9250_I2C_SLV0_EN | length)) {
-        return -3;
-      }
-
-      // takes some time for these registers to fill
-      delay(1);
-
-      // read the bytes off the MPU9250 EXT_SENS_DATA registers
-      int8_t res = readMaster(MPU9250_EXT_SENS_DATA_00, length, data);
-
-      return res;
-    }
-
-    int8_t writeSlave(uint8_t regAddr, uint8_t data)
-    {
-      // set slave 0 to the AK8963 and set for write
-      if(!writeMaster(MPU9250_I2C_SLV0_ADDR, AK8963_ADDRESS)) {
-        return -1;
-      }
-      // set the register to the desired AK8963 sub address 
-      if(!writeMaster(MPU9250_I2C_SLV0_REG, regAddr)) {
-        return -2;
-      }
-      // store the data for write
-      if(!writeMaster(MPU9250_I2C_SLV0_DO, data)) {
-        return -3;
-      }
-      // enable I2C and send 1 byte
-      if(!writeMaster(MPU9250_I2C_SLV0_CTRL, MPU9250_I2C_SLV0_EN | 0x01)) {
-        return -4;
-      }
-
-      return true;
-    }
-
     uint8_t _mode;
     VectorFloat scale;
     uint8_t buffer[7];
diff --git a/lib/Espfc/src/Device/MagDevice.h b/lib/Espfc/src/Device/MagDevice.h
index 9c63a360..83904ab2 100644
--- a/lib/Espfc/src/Device/MagDevice.h
+++ b/lib/Espfc/src/Device/MagDevice.h
@@ -24,7 +24,6 @@ class MagDevice: public BusAwareDevice
 
     virtual int begin(BusDevice * bus) = 0;
     virtual int begin(BusDevice * bus, uint8_t addr) = 0;
-    virtual int begin(BusDevice * bus, uint8_t addr, uint8_t masterAddr) = 0;
 
     virtual DeviceType getType() const = 0;
 
@@ -36,7 +35,7 @@ class MagDevice: public BusAwareDevice
 
     static const char ** getNames()
     {
-      static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("HMC5883"), PSTR("AK8975"), PSTR("AK8963"), NULL };
+      static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("HMC5883L"), PSTR("AK8975"), PSTR("AK8963"), NULL };
       return devChoices;
     }
 
diff --git a/lib/Espfc/src/Device/MagHMC5338L.h b/lib/Espfc/src/Device/MagHMC5338L.h
index 69daf9b2..f287c5d4 100644
--- a/lib/Espfc/src/Device/MagHMC5338L.h
+++ b/lib/Espfc/src/Device/MagHMC5338L.h
@@ -77,30 +77,30 @@ class MagHMC5338L: public MagDevice
   public:
     int begin(BusDevice * bus) override
     {
-      return begin(bus, HMC5883L_DEFAULT_ADDRESS, 0);
+      return begin(bus, HMC5883L_DEFAULT_ADDRESS);
     }
 
     int begin(BusDevice * bus, uint8_t addr) override
     {
-      return begin(bus, addr, 0);
-    }
-
-    int begin(BusDevice * bus, uint8_t addr, uint8_t masterAddr) override
-    {
-      setBus(bus, addr, masterAddr);
+      setBus(bus, addr);
 
-      return testConnection();
+      if(!testConnection()) return 0;
 
       setMode(HMC5883L_MODE_CONTINUOUS);
       setSampleAveraging(HMC5883L_AVERAGING_1);
       setSampleRate(HMC5883L_RATE_75);
       setGain(HMC5883L_GAIN_1090);
+
+      uint8_t buffer[6];
+      _bus->read(_addr, HMC5883L_RA_DATAX_H, 6, buffer);
+
+      return 1;
     }
 
     int readMag(VectorInt16& v) override
     {
       uint8_t buffer[6];
-      _bus->read(_addr, HMC5883L_RA_DATAX_H, 6, buffer);
+      _bus->readFast(_addr, HMC5883L_RA_DATAX_H, 6, buffer);
       if (_mode == HMC5883L_MODE_SINGLE)
       {
         _bus->writeByte(_addr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
@@ -108,13 +108,14 @@ class MagHMC5338L: public MagDevice
       v.x = (((int16_t)buffer[0]) << 8) | buffer[1];
       v.z = (((int16_t)buffer[2]) << 8) | buffer[3];
       v.y = (((int16_t)buffer[4]) << 8) | buffer[5];
+
       return 1;
     }
 
     const VectorFloat convert(const VectorInt16& v) const override
     {
       const float scale = 1.f / 1090.f;
-      return VectorFloat(v) * scale;
+      return VectorFloat{v} * scale;
     }
 
     int getRate() const override
@@ -129,33 +130,39 @@ class MagHMC5338L: public MagDevice
 
     void setSampleAveraging(uint8_t averaging)
     {
-      _bus->writeBits(_addr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, averaging);
+      uint8_t res = _bus->writeBits(_addr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, averaging);
+      //D("hmc5338l:avg", averaging, res);
+      (void)res;
     }
 
     void setSampleRate(uint8_t rate)
     {
-      _bus->writeBits(_addr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, rate);
+      uint8_t res = _bus->writeBits(_addr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, rate);
+      //D("hmc5338l:rate", rate, res);
+      (void)res;
     }
 
     void setMode(uint8_t mode)
     {
-      _bus->writeByte(_addr, HMC5883L_RA_MODE, mode << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
       _mode = mode; // track to tell if we have to clear bit 7 after a read
+      uint8_t res = _bus->writeByte(_addr, HMC5883L_RA_MODE, mode << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+      //D("hmc5338l:mode", mode, res);
+      (void)res;
     }
 
     void setGain(uint8_t gain)
     {
-      _bus->writeByte(_addr, HMC5883L_RA_CONFIG_B, gain << (HMC5883L_CRB_GAIN_BIT - HMC5883L_CRB_GAIN_LENGTH + 1));
+      uint8_t res = _bus->writeByte(_addr, HMC5883L_RA_CONFIG_B, gain << (HMC5883L_CRB_GAIN_BIT - HMC5883L_CRB_GAIN_LENGTH + 1));
+      //D("hmc5338l:gain", gain, res);
+      (void)res;
     }
 
     bool testConnection() override
     {
-      uint8_t buffer[3];
-      if(_bus->read(_addr, HMC5883L_RA_ID_A, 3, buffer) == 3)
-      {
-        return (buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3');
-      }
-      return false;
+      uint8_t buffer[3] = { 0, 0, 0 };
+      uint8_t len = _bus->read(_addr, HMC5883L_RA_ID_A, 3, buffer);
+      //D("hmc5338l:whoami", len, buffer[0], buffer[1], buffer[2]);
+      return len == 3 && buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3';
     }
 
   private:
diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h
index c5f46fd0..a367ec98 100644
--- a/lib/Espfc/src/Espfc.h
+++ b/lib/Espfc/src/Espfc.h
@@ -77,7 +77,6 @@ class Espfc
         _buzzer.update();
         _model.state.stats.update();
       }
-      //_model.state.appQueue.send(Event(EVENT_IDLE));
 
       return 1;
 #else
@@ -111,7 +110,7 @@ class Espfc
     {
 #if defined(ESPFC_MULTI_CORE)
       Event e = _model.state.appQueue.receive();
-      //Serial2.write((uint8_t)e.type);
+
       Stats::Measure measure(_model.state.stats, COUNTER_CPU_1);
 
       _sensor.onAppEvent(e);
diff --git a/lib/Espfc/src/Filter.h b/lib/Espfc/src/Filter.h
index 5f8691da..5ef825d3 100644
--- a/lib/Espfc/src/Filter.h
+++ b/lib/Espfc/src/Filter.h
@@ -53,7 +53,7 @@ class FilterConfig
 
     FilterConfig sanitize(int rate) const
     {
-      const int halfRate = rate / 2;
+      const int halfRate = rate * 0.49f;
       FilterType t = (FilterType)type;
       int16_t f = Math::clamp((int)freq, 0, halfRate);   // adj cut freq below nyquist rule
       int16_t c = Math::clamp((int)cutoff, 0, (int)(f * 0.98f));      // sanitize cutoff to be slightly below filter freq
@@ -77,6 +77,7 @@ class DynamicFilterConfig {
     int16_t q;
     int16_t min_freq;
     int16_t max_freq;
+    static const int MIN_FREQ = 1000;
 };
 
 class FilterStatePt1 {
diff --git a/lib/Espfc/src/Fusion.h b/lib/Espfc/src/Fusion.h
index 5be5c9c4..fbc3a486 100644
--- a/lib/Espfc/src/Fusion.h
+++ b/lib/Espfc/src/Fusion.h
@@ -309,7 +309,7 @@ class Fusion
 
     void madgwickFusion()
     {
-      if(_model.magActive())
+      if(false && _model.magActive())
       {
         _madgwick.update(
           _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z,
@@ -354,7 +354,7 @@ class Fusion
 
     void mahonyFusion()
     {
-      if(_model.magActive())
+      if(false && _model.magActive())
       {
         _mahony.update(
           _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z,
diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h
index e65accab..0ba4c0a7 100644
--- a/lib/Espfc/src/Hardware.h
+++ b/lib/Espfc/src/Hardware.h
@@ -11,6 +11,7 @@
 #if defined(ESPFC_SPI_0)
 #include "Device/BusSPI.h"
 #endif
+#include "Device/BusSlave.h"
 #include "Device/GyroDevice.h"
 #include "Device/GyroMPU6050.h"
 #include "Device/GyroMPU6500.h"
@@ -34,6 +35,7 @@ namespace {
 #if defined(ESPFC_I2C_0)
   static Espfc::Device::BusI2C i2cBus(WireInstance);
 #endif
+  static Espfc::Device::BusSlave gyroSlaveBus;
   static Espfc::Device::GyroMPU6050 mpu6050;
   static Espfc::Device::GyroMPU6500 mpu6500;
   static Espfc::Device::GyroMPU9250 mpu9250;
@@ -96,10 +98,11 @@ class Hardware
         if(!detectedGyro && detectDevice(icm20602, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &icm20602;
         if(!detectedGyro && detectDevice(bmi160, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &bmi160;
         if(!detectedGyro && detectDevice(lsm6dso, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &lsm6dso;
+        if(detectedGyro) gyroSlaveBus.begin(&spiBus, detectedGyro->getAddress());
       }
 #endif
 #if defined(ESPFC_I2C_0)
-      if(_model.config.pin[PIN_I2C_0_SDA] != -1 && _model.config.pin[PIN_I2C_0_SCL] != -1)
+      if(!detectedGyro && _model.config.pin[PIN_I2C_0_SDA] != -1 && _model.config.pin[PIN_I2C_0_SCL] != -1)
       {
         if(!detectedGyro && detectDevice(mpu9250, i2cBus)) detectedGyro = &mpu9250;
         if(!detectedGyro && detectDevice(mpu6500, i2cBus)) detectedGyro = &mpu6500;
@@ -107,6 +110,7 @@ class Hardware
         if(!detectedGyro && detectDevice(bmi160, i2cBus)) detectedGyro = &bmi160;
         if(!detectedGyro && detectDevice(mpu6050, i2cBus)) detectedGyro = &mpu6050;
         if(!detectedGyro && detectDevice(lsm6dso, i2cBus)) detectedGyro = &lsm6dso;
+        if(detectedGyro) gyroSlaveBus.begin(&i2cBus, detectedGyro->getAddress());
       }
 #endif
       if(!detectedGyro) return;
@@ -123,22 +127,18 @@ class Hardware
       if(_model.config.magDev == MAG_NONE) return;
 
       Espfc::Device::MagDevice * detectedMag  = nullptr;
-#if defined(ESPFC_SPI_0)
-      if(_model.config.pin[PIN_SPI_CS0] != -1 && _model.state.gyroDev && _model.state.gyroDev->getType() == GYRO_MPU9250)
-      {
-        if(!detectedMag && detectDevice(ak8963, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedMag = &ak8963;
-      }
-#endif
 #if defined(ESPFC_I2C_0)
       if(_model.config.pin[PIN_I2C_0_SDA] != -1 && _model.config.pin[PIN_I2C_0_SCL] != -1)
       {
-        if(_model.state.gyroDev && _model.state.gyroDev->getType() == GYRO_MPU9250)
-        {
-          if(!detectedMag && detectDevice(ak8963, i2cBus)) detectedMag = &ak8963;
-        }
+        if(!detectedMag && detectDevice(ak8963, i2cBus)) detectedMag = &ak8963;
         if(!detectedMag && detectDevice(hmc5883l, i2cBus)) detectedMag = &hmc5883l;
       }
 #endif
+      if(gyroSlaveBus.getBus())
+      {
+        if(!detectedMag && detectDevice(ak8963, gyroSlaveBus)) detectedMag = &ak8963;
+        if(!detectedMag && detectDevice(hmc5883l, gyroSlaveBus)) detectedMag = &hmc5883l;
+      }
       _model.state.magDev = detectedMag;
       _model.state.magPresent = (bool)detectedMag;
       _model.state.magRate = detectedMag ? detectedMag->getRate() : 0;
@@ -175,7 +175,7 @@ class Hardware
     {
       typename Dev::DeviceType type = dev.getType();
       bool status = dev.begin(&bus, cs);
-      _model.logger.info().log(F("SPI DETECT")).log(FPSTR(Dev::getName(type))).log(cs).logln(status);
+      _model.logger.info().log(F("SPI DETECT")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : "");
       return status;
     }
 #endif
@@ -186,11 +186,20 @@ class Hardware
     {
       typename Dev::DeviceType type = dev.getType();
       bool status = dev.begin(&bus);
-      _model.logger.info().log(F("I2C DETECT")).log(FPSTR(Dev::getName(type))).logln(status);
+      _model.logger.info().log(F("I2C DETECT")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : "");
       return status;
     }
 #endif
 
+    template<typename Dev>
+    bool detectDevice(Dev& dev, Device::BusSlave& bus)
+    {
+      typename Dev::DeviceType type = dev.getType();
+      bool status = dev.begin(&bus);
+      _model.logger.info().log(F("SLV DETECT")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : "");
+      return status;
+    }
+
     int update()
     {
       return 1;
diff --git a/lib/Espfc/src/Math/Bits.h b/lib/Espfc/src/Math/Bits.h
new file mode 100644
index 00000000..5eca168d
--- /dev/null
+++ b/lib/Espfc/src/Math/Bits.h
@@ -0,0 +1,96 @@
+#pragma once
+
+namespace Espfc {
+
+namespace Math {
+
+  inline bool getBit(uint8_t data, uint8_t bitNum)
+  {
+    return data & (1 << bitNum);
+  }
+
+  inline uint8_t setBit(uint8_t data, uint8_t bitNum, bool bitVal)
+  {
+    data = (bitVal != 0) ? (data | (1 << bitNum)) : (data & ~(1 << bitNum));
+    return data;
+  }
+
+  inline uint8_t getMaskMsb(uint8_t bitStart, uint8_t bitLen)
+  {
+    uint8_t mask = ((1 << bitLen) - 1) << (bitStart - bitLen + 1);
+    return mask;
+  }
+
+  inline uint8_t setMasked(uint8_t data, uint8_t mask, uint8_t newData)
+  {
+    newData &= mask; // zero all non-important bits in data
+    data &= ~(mask); // zero all important bits in existing byte
+    data |= newData; // combine data with existing byte
+    return data;
+  }
+
+  /**
+   * I2Cdev::readBits() compatible 
+   * Read multiple bits from an 8-bit data
+   * @param bitStart First bit position to read (0-7), Most Significant Bit
+   * @param bitLen Number of bits to read (no more than 8, no more than start + 1)
+   */
+  inline uint8_t getBitsMsb(uint8_t data, uint8_t bitStart, uint8_t bitLen)
+  {
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t mask = ((1 << bitLen) - 1) << (bitStart - bitLen + 1);
+    data &= mask;
+    data >>= (bitStart - bitLen + 1);
+    return data;
+  }
+
+  inline uint8_t setBitsMsb(uint8_t data, uint8_t bitStart, uint8_t bitLen, uint8_t bitVal)
+  {
+    uint8_t mask = ((1 << bitLen) - 1) << (bitStart - bitLen + 1);
+    bitVal <<= (bitStart - bitLen + 1); // shift data into correct position
+    bitVal &= mask; // zero all non-important bits in data
+    data &= ~(mask); // zero all important bits in existing byte
+    data |= bitVal; // combine data with existing byte
+    return data;
+  }
+
+  inline uint8_t getMaskLsb(uint8_t bitStart, uint8_t bitLen)
+  {
+    uint8_t mask = ((1 << bitLen) - 1) << bitStart;
+    return mask;
+  }
+
+  /**
+   * Read multiple bits from an 8-bit data
+   * @param bitStart Last bit position to read (0-7), Least Significant Bit
+   * @param bitLen Number of bits to read (not more than 8 - bitStart)
+   */
+  inline uint8_t getBitsLsb(uint8_t data, uint8_t bitStart, uint8_t bitLen)
+  {
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //   xxx   args: bitStart=4, length=3
+    //   101    masked
+    //   -> 101 shifted
+    uint8_t mask = ((1 << bitLen) - 1);
+    data >>= bitStart;
+    data &= mask;
+    return data;
+  }
+
+  inline uint8_t setBitsLsb(uint8_t data, uint8_t bitStart, uint8_t bitLen, uint8_t bitVal)
+  {
+    uint8_t mask = ((1 << bitLen) - 1) << bitStart;
+    bitVal <<= bitStart; // shift data into correct position
+    bitVal &= mask; // zero all non-important bits in data
+    data &= ~(mask); // zero all important bits in existing byte
+    data |= bitVal; // combine data with existing byte
+    return data;
+  }
+}
+
+}
diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h
index 66dbd213..e41f73e9 100644
--- a/lib/Espfc/src/Model.h
+++ b/lib/Espfc/src/Model.h
@@ -384,7 +384,7 @@ class Model
       uint32_t featureAllowMask = FEATURE_RX_PPM | FEATURE_MOTOR_STOP | FEATURE_TELEMETRY;// | FEATURE_AIRMODE;
 
       // allow dynamic filter only above 1k sampling rate
-      if(state.gyroRate >= 1000)
+      if(state.loopRate >= DynamicFilterConfig::MIN_FREQ)
       {
         featureAllowMask |= FEATURE_DYNAMIC_FILTER;
       }
@@ -430,7 +430,7 @@ class Model
       // sample rate = clock / ( divider + 1)
       state.gyroTimer.setRate(state.gyroRate);
       state.accelTimer.setRate(constrain(state.gyroTimer.rate, 100, 500));
-      state.accelTimer.setInterval(state.accelTimer.interval - 5);
+      state.accelTimer.setInterval(state.accelTimer.interval);
       //state.accelTimer.setRate(state.gyroTimer.rate, 2);
       state.loopTimer.setRate(state.gyroTimer.rate, config.loopSync);
       state.mixerTimer.setRate(state.loopTimer.rate, config.mixerSync);
diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h
index fea2307a..ef1e5968 100644
--- a/lib/Espfc/src/ModelConfig.h
+++ b/lib/Espfc/src/ModelConfig.h
@@ -26,20 +26,6 @@ enum GyroDlpf {
   GYRO_DLPF_EX  = 0x07,
 };
 
-enum GyroGain {
-  GYRO_FS_250  = 0x00,
-  GYRO_FS_500  = 0x01,
-  GYRO_FS_1000 = 0x02,
-  GYRO_FS_2000 = 0x03
-};
-
-enum AccelGain {
-  ACCEL_FS_2  = 0x00,
-  ACCEL_FS_4  = 0x01,
-  ACCEL_FS_8  = 0x02,
-  ACCEL_FS_16 = 0x03
-};
-
 enum AccelMode {
   ACCEL_DELAYED   = 0x00,
   ACCEL_GYRO      = 0x01,
@@ -548,7 +534,6 @@ class ModelConfig
     int8_t gyroBus;
     int8_t gyroDev;
     int8_t gyroDlpf;
-    int8_t gyroFsr;
     int8_t gyroAlign;
     FilterConfig gyroFilter;
     FilterConfig gyroFilter2;
@@ -559,7 +544,6 @@ class ModelConfig
 
     int8_t accelBus;
     int8_t accelDev;
-    int8_t accelFsr;
     int8_t accelAlign;
     FilterConfig accelFilter;
 
@@ -714,13 +698,12 @@ class ModelConfig
       pin[PIN_SPI_CS1] = ESPFC_SPI_CS_BARO;
       pin[PIN_SPI_CS2] = -1;
 #endif
-      i2cSpeed = 1000;
+      i2cSpeed = 800;
 
       gyroBus = BUS_AUTO;
       gyroDev = GYRO_AUTO;
       gyroAlign = ALIGN_DEFAULT;
       gyroDlpf = GYRO_DLPF_256;
-      gyroFsr  = GYRO_FS_2000;
 
       loopSync = 8; // MPU 1000Hz
       mixerSync = 1;
@@ -728,7 +711,6 @@ class ModelConfig
       accelBus = BUS_AUTO;
       accelDev = GYRO_AUTO;
       accelAlign = ALIGN_DEFAULT;
-      accelFsr = ACCEL_FS_16;
 
       magBus = BUS_AUTO;
       magDev = MAG_NONE;
diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h
index d3a26062..7df43558 100644
--- a/lib/Espfc/src/Msp/MspProcessor.h
+++ b/lib/Espfc/src/Msp/MspProcessor.h
@@ -1244,7 +1244,7 @@ class MspProcessor
           }
           for (int i = 0; i < 3; i++)
           {
-            r.writeU16(lrintf(_model.state.mag[i]));
+            r.writeU16(lrintf(_model.state.mag[i] * 1090));
           }
           break;
 
diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h
index 40518fc4..a293255a 100644
--- a/lib/Espfc/src/Sensor/AccelSensor.h
+++ b/lib/Espfc/src/Sensor/AccelSensor.h
@@ -20,14 +20,7 @@ class AccelSensor: public BaseSensor
       _gyro = _model.state.gyroDev;
       if(!_gyro) return 0;
 
-      switch(_model.config.accelFsr)
-      {
-        case ACCEL_FS_16: _model.state.accelScale = 16.f * ACCEL_G / 32768.f; break;
-        case ACCEL_FS_8:  _model.state.accelScale =  8.f * ACCEL_G / 32768.f; break;
-        case ACCEL_FS_4:  _model.state.accelScale =  4.f * ACCEL_G / 32768.f; break;
-        case ACCEL_FS_2:  _model.state.accelScale =  2.f * ACCEL_G / 32768.f; break;
-      }
-      _gyro->setFullScaleAccelRange(_model.config.accelFsr);
+      _model.state.accelScale = 16.f * ACCEL_G / 32768.f;
 
       for(size_t i = 0; i < 3; i++)
       {
@@ -37,7 +30,7 @@ class AccelSensor: public BaseSensor
       _model.state.accelBiasAlpha = _model.state.accelTimer.rate > 0 ? 5.0f / _model.state.accelTimer.rate : 0;
       _model.state.accelCalibrationState = CALIBRATION_IDLE;
 
-      _model.logger.info().log(F("ACCEL INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_model.state.accelTimer.rate).log(_model.state.accelTimer.interval).logln(_model.state.accelPresent);
+      _model.logger.info().log(F("ACCEL INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.state.accelTimer.rate).log(_model.state.accelTimer.interval).logln(_model.state.accelPresent);
 
       return 1;
     }
diff --git a/lib/Espfc/src/Sensor/BaroSensor.h b/lib/Espfc/src/Sensor/BaroSensor.h
index b43d3b04..a789b5c1 100644
--- a/lib/Espfc/src/Sensor/BaroSensor.h
+++ b/lib/Espfc/src/Sensor/BaroSensor.h
@@ -40,7 +40,7 @@ class BaroSensor: public BaseSensor
       _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baroRate * 0.1f), _model.state.baroRate);
       _altitudeFilter.begin(_model.config.baroFilter, _model.state.baroRate);
 
-      _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(toGyroRate).log(_model.state.baroRate).logln(_model.config.baroFilter.freq);
+      _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baroRate).logln(_model.config.baroFilter.freq);
 
       return 1;
     }
@@ -56,10 +56,10 @@ class BaroSensor: public BaseSensor
     {
       if(!_baro || !_model.baroActive()) return 0;
       
-      Stats::Measure measure(_model.state.stats, COUNTER_BARO);
-      
       if(_wait > micros()) return 0;
 
+      Stats::Measure measure(_model.state.stats, COUNTER_BARO);
+
       if(_model.config.debugMode == DEBUG_BARO)
       {
         _model.state.debug[0] = _state;
diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h
index e50c539f..cb3b0e29 100644
--- a/lib/Espfc/src/Sensor/GyroSensor.h
+++ b/lib/Espfc/src/Sensor/GyroSensor.h
@@ -28,15 +28,7 @@ class GyroSensor: public BaseSensor
 
       _gyro->setDLPFMode(_model.config.gyroDlpf);
       _gyro->setRate(_gyro->getRate());
-
-      switch(_model.config.gyroFsr)
-      {
-        case GYRO_FS_2000: _model.state.gyroScale = radians(2000.f) / 32768.f; break;
-        case GYRO_FS_1000: _model.state.gyroScale = radians(1000.f) / 32768.f; break;
-        case GYRO_FS_500:  _model.state.gyroScale =  radians(500.f) / 32768.f; break;
-        case GYRO_FS_250:  _model.state.gyroScale =  radians(250.f) / 32768.f; break;
-      }
-      _gyro->setFullScaleGyroRange(_model.config.gyroFsr);
+      _model.state.gyroScale = radians(2000.f) / 32768.f;
 
       _model.state.gyroCalibrationState = CALIBRATION_START; // calibrate gyro on start
       _model.state.gyroCalibrationRate = _model.state.loopTimer.rate;
@@ -55,7 +47,7 @@ class GyroSensor: public BaseSensor
 #endif
       }
 
-      _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_model.config.gyroDlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval);
+      _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.config.gyroDlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval);
 
       return 1;
     }
@@ -107,7 +99,7 @@ class GyroSensor: public BaseSensor
 
       _model.state.gyroScaled = _model.state.gyro;
 
-      bool dynNotchEnabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.dynamicFilter.width > 0;
+      bool dynNotchEnabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.dynamicFilter.width > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ;
 
       // filtering
       for(size_t i = 0; i < 3; ++i)
@@ -171,6 +163,7 @@ class GyroSensor: public BaseSensor
     void dynNotchAnalyze()
     {
       if(!_model.gyroActive()) return;
+      if(_model.state.loopTimer.rate < DynamicFilterConfig::MIN_FREQ) return;
 
       Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FFT);
 
diff --git a/lib/Espfc/src/Sensor/MagSensor.h b/lib/Espfc/src/Sensor/MagSensor.h
index 34e6f36a..d183e54d 100644
--- a/lib/Espfc/src/Sensor/MagSensor.h
+++ b/lib/Espfc/src/Sensor/MagSensor.h
@@ -28,7 +28,7 @@ class MagSensor: public BaseSensor
       _model.state.magCalibrationState = CALIBRATION_IDLE;
       _model.state.magCalibrationValid = true;
 
-      _model.logger.info().log(F("MAG INIT")).log(FPSTR(Device::MagDevice::getName(_mag->getType()))).logln(_model.state.magTimer.rate);
+      _model.logger.info().log(F("MAG INIT")).log(FPSTR(Device::MagDevice::getName(_mag->getType()))).log(_mag->getAddress()).logln(_model.state.magTimer.rate);
       
       return 1;
     }
@@ -54,7 +54,7 @@ class MagSensor: public BaseSensor
 
     int filter()
     {
-      if(!_mag || !_model.magActive() || !_model.state.magTimer.check()) return 0;
+      if(!_mag || !_model.magActive()) return 0;
 
       Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER);
 
diff --git a/lib/Espfc/src/SensorManager.h b/lib/Espfc/src/SensorManager.h
index a8c65e80..7a0c6ff8 100644
--- a/lib/Espfc/src/SensorManager.h
+++ b/lib/Espfc/src/SensorManager.h
@@ -48,7 +48,6 @@ class SensorManager
           _model.state.imuUpdate = true;
           return 1;
         case EVENT_MAG_READ:
-          _mag.filter();
           return 1;
         case EVENT_BBLOG_UPDATED:
           _gyro.dynNotchAnalyze();
@@ -83,7 +82,7 @@ class SensorManager
 
       if (!status)
       {
-        status = _mag.read();
+        status = _mag.update();
       }
       if(status)
       {
diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h
index 8591a94a..65106466 100644
--- a/lib/Espfc/src/Stats.h
+++ b/lib/Espfc/src/Stats.h
@@ -61,21 +61,21 @@ class Stats
         _start[i] = 0;
         _avg[i] = 0;
         _sum[i] = 0;
+        _count[i] = 0;
+        _freq[i] = 0;
       }
     }
 
     inline void start(StatCounter c) IRAM_ATTR
     {
       _start[c] = micros();
-      //Serial1.write((uint8_t)c);
     }
 
     inline void end(StatCounter c) IRAM_ATTR
     {
       uint32_t diff = micros() - _start[c];
       _sum[c] += diff;
-      //uint8_t t = Math::clamp(diff, 0ul, 255ul);
-      //Serial1.write(t);
+      _count[c]++;
     }
 
     void loopTick()
@@ -99,6 +99,8 @@ class Stats
       {
         _avg[i] = (float)_sum[i] / timer.delta;
         _sum[i] = 0;
+        _freq[i] = (float)_count[i] * 1e6 / timer.delta;
+        _count[i] = 0;
       }
     }
 
@@ -115,6 +117,11 @@ class Stats
       return _avg[c] * timer.interval * 0.001f;
     }
 
+    float getFreq(StatCounter c) const
+    {
+      return _freq[c];
+    }
+
     float getTotalLoad() const
     {
       float ret = 0;
@@ -189,7 +196,9 @@ class Stats
   private:
     uint32_t _start[COUNTER_COUNT];
     uint32_t _sum[COUNTER_COUNT];
+    uint32_t _count[COUNTER_COUNT];
     float _avg[COUNTER_COUNT];
+    float _freq[COUNTER_COUNT];
     uint32_t _loop_last;
     int32_t _loop_time;
 };
diff --git a/lib/Espfc/src/Target/TargetRP2040.h b/lib/Espfc/src/Target/TargetRP2040.h
index 9ae106fa..8e3a7c97 100644
--- a/lib/Espfc/src/Target/TargetRP2040.h
+++ b/lib/Espfc/src/Target/TargetRP2040.h
@@ -69,8 +69,8 @@
 
 #define ESPFC_GUARD 0
 
-#define ESPFC_GYRO_I2C_RATE_MAX 2000
-#define ESPFC_GYRO_SPI_RATE_MAX 2000
+#define ESPFC_GYRO_I2C_RATE_MAX 1000
+#define ESPFC_GYRO_SPI_RATE_MAX 1000
 
 #define ESPFC_MULTI_CORE
 #define ESPFC_MULTI_CORE_RP2040
@@ -123,7 +123,7 @@ template<>
 inline int targetSerialInit(SerialUSB& dev, const SerialDeviceConfig& conf)
 {
   dev.begin(conf.baud);
-  while(!dev) delay(10);
+  //while(!dev) delay(10);
   return 1;
 }
 
diff --git a/test/test_math/test_math.cpp b/test/test_math/test_math.cpp
index f0ebd3d1..9a2159d4 100644
--- a/test/test_math/test_math.cpp
+++ b/test/test_math/test_math.cpp
@@ -1,6 +1,7 @@
 #include <unity.h>
 #include <EspGpio.h>
 #include "Math/Utils.h"
+#include "Math/Bits.h"
 #include "helper_3dmath.h"
 #include "Filter.h"
 #include "Control/Pid.h"
@@ -52,6 +53,74 @@ void test_math_deadband()
     TEST_ASSERT_EQUAL_INT32(10, Math::deadband( 20, 10));
 }
 
+void test_math_bit()
+{
+    TEST_ASSERT_EQUAL(0, Math::getBit(0, 0));
+    TEST_ASSERT_EQUAL(1, Math::getBit(1, 0));
+    TEST_ASSERT_EQUAL(0, Math::getBit(1, 1));
+    TEST_ASSERT_EQUAL(1, Math::getBit(3, 1));
+
+    TEST_ASSERT_EQUAL_UINT8(0, Math::setBit(0, 0, 0));
+    TEST_ASSERT_EQUAL_UINT8(1, Math::setBit(0, 0, 1));
+    TEST_ASSERT_EQUAL_UINT8(2, Math::setBit(0, 1, 1));
+    TEST_ASSERT_EQUAL_UINT8(3, Math::setBit(2, 0, 1));
+}
+
+void test_math_bitmask()
+{
+    TEST_ASSERT_EQUAL_UINT8(   0, Math::setMasked(0, 1, 0));
+    TEST_ASSERT_EQUAL_UINT8(   1, Math::setMasked(0, 1, 1));
+    TEST_ASSERT_EQUAL_UINT8(0x38, Math::setMasked(0x00, 0x38, 0xff));
+    TEST_ASSERT_EQUAL_UINT8(0xc7, Math::setMasked(0xff, 0x38, 0x00));
+}
+
+void test_math_bitmask_lsb()
+{
+    TEST_ASSERT_EQUAL_UINT8( 1, Math::getMaskLsb(0, 1));
+    TEST_ASSERT_EQUAL_UINT8( 2, Math::getMaskLsb(1, 1));
+    TEST_ASSERT_EQUAL_UINT8( 4, Math::getMaskLsb(2, 1));
+    TEST_ASSERT_EQUAL_UINT8(12, Math::getMaskLsb(2, 2));
+    TEST_ASSERT_EQUAL_UINT8(56, Math::getMaskLsb(3, 3));
+}
+
+void test_math_bitmask_msb()
+{
+    TEST_ASSERT_EQUAL_UINT8( 1, Math::getMaskMsb(0, 1));
+    TEST_ASSERT_EQUAL_UINT8( 2, Math::getMaskMsb(1, 1));
+    TEST_ASSERT_EQUAL_UINT8( 4, Math::getMaskMsb(2, 1));
+    TEST_ASSERT_EQUAL_UINT8( 6, Math::getMaskMsb(2, 2));
+    TEST_ASSERT_EQUAL_UINT8(14, Math::getMaskMsb(3, 3));
+    TEST_ASSERT_EQUAL_UINT8(30, Math::getMaskMsb(4, 4));
+}
+
+void test_math_bits_lsb()
+{
+    TEST_ASSERT_EQUAL_UINT8(0, Math::getBitsLsb(0x00, 1, 1));
+    TEST_ASSERT_EQUAL_UINT8(0, Math::getBitsLsb(0x55, 1, 1));
+    TEST_ASSERT_EQUAL_UINT8(1, Math::getBitsLsb(0x55, 2, 2));
+    TEST_ASSERT_EQUAL_UINT8(1, Math::getBitsLsb(0x55, 4, 2));
+    TEST_ASSERT_EQUAL_UINT8(5, Math::getBitsLsb(0x55, 2, 4));
+
+    TEST_ASSERT_EQUAL_UINT8( 8, Math::setBitsLsb(0x00, 3, 4, 1));
+    TEST_ASSERT_EQUAL_UINT8(80, Math::setBitsLsb(0x00, 3, 4, 10));
+    TEST_ASSERT_EQUAL_UINT8(16, Math::setBitsLsb(0x00, 4, 2, 1));
+    TEST_ASSERT_EQUAL_UINT8(160, Math::setBitsLsb(0x00, 4, 4, 10));
+}
+
+void test_math_bits_msb()
+{
+    TEST_ASSERT_EQUAL_UINT8( 0, Math::getBitsMsb(0x00, 1, 1));
+    TEST_ASSERT_EQUAL_UINT8( 0, Math::getBitsMsb(0x55, 1, 1));
+    TEST_ASSERT_EQUAL_UINT8( 2, Math::getBitsMsb(0x55, 2, 2));
+    TEST_ASSERT_EQUAL_UINT8( 2, Math::getBitsMsb(0x55, 4, 2));
+    TEST_ASSERT_EQUAL_UINT8(10, Math::getBitsMsb(0x55, 4, 4));
+
+    TEST_ASSERT_EQUAL_UINT8( 1, Math::setBitsMsb(0x00, 3, 4, 1));
+    TEST_ASSERT_EQUAL_UINT8(10, Math::setBitsMsb(0x00, 3, 4, 10));
+    TEST_ASSERT_EQUAL_UINT8( 8, Math::setBitsMsb(0x00, 6, 4, 1));
+    TEST_ASSERT_EQUAL_UINT8(80, Math::setBitsMsb(0x00, 6, 4, 10));
+}
+
 void test_math_peak_detect_full()
 {
     using Math::Peak;
@@ -242,7 +311,7 @@ void test_filter_sanitize_pt1_nyquist()
 
     FilterConfig conf2 = conf.sanitize(100);
     TEST_ASSERT_EQUAL(FILTER_PT1, conf2.type);
-    TEST_ASSERT_EQUAL_INT16(50, conf2.freq);
+    TEST_ASSERT_EQUAL_INT16(49, conf2.freq);
     TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff);
 }
 
@@ -294,7 +363,7 @@ void test_filter_sanitize_biquad_lpf_nyqist()
 
     FilterConfig conf2 = conf.sanitize(100);
     TEST_ASSERT_EQUAL(FILTER_BIQUAD, conf2.type);
-    TEST_ASSERT_EQUAL_INT16(50, conf2.freq);
+    TEST_ASSERT_EQUAL_INT16(49, conf2.freq);
     TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff);
 }
 
@@ -346,8 +415,8 @@ void test_filter_sanitize_biquad_notch_nyquist()
 
     FilterConfig conf2 = conf.sanitize(100);
     TEST_ASSERT_EQUAL(FILTER_NOTCH, conf2.type);
-    TEST_ASSERT_EQUAL_INT16(50, conf2.freq);
-    TEST_ASSERT_EQUAL_INT16(49, conf2.cutoff);
+    TEST_ASSERT_EQUAL_INT16(49, conf2.freq);
+    TEST_ASSERT_EQUAL_INT16(48, conf2.cutoff);
 }
 
 void assert_filter_off(Filter& filter)
@@ -389,9 +458,9 @@ void test_filter_pt1_50_100()
 
     TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f));
     TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.076f, filter.update(0.1f));
-    TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.397f, filter.update(0.5f));
-    TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.234f, filter.update(1.5f));
-    TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.436f, filter.update(1.5f));
+    TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.395f, filter.update(0.5f));
+    TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.229f, filter.update(1.5f));
+    TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.434f, filter.update(1.5f));
     TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.484f, filter.update(1.5f));
 }
 
@@ -908,6 +977,14 @@ int main(int argc, char **argv)
     RUN_TEST(test_math_map);
     RUN_TEST(test_math_map3);
     RUN_TEST(test_math_deadband);
+
+    RUN_TEST(test_math_bit);
+    RUN_TEST(test_math_bitmask);
+    RUN_TEST(test_math_bitmask_lsb);
+    RUN_TEST(test_math_bitmask_msb);
+    RUN_TEST(test_math_bits_lsb);
+    RUN_TEST(test_math_bits_msb);
+
     RUN_TEST(test_math_baro_altitude);
     RUN_TEST(test_math_peak_detect_full);
     RUN_TEST(test_math_peak_detect_partial);