add QMC6309, HP203B

Author: qqqlab

src/madflight.h

@@ -1,4 +1,4 @@
-#define MADFLIGHT_VERSION "madflight v2.0.0"
+#define MADFLIGHT_VERSION "madflight v2.0.1-DEV"
 
 //madflight.h - Flight Controller for ESP32 / ESP32-S3 / RP2350 / RP2040 / STM32
 

src/madflight/bar/BarGizmoHP203B.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "bar.h"
+#include "../hal/MF_I2C.h"
+
+class BarGizmoHP203B: public BarGizmo {
+private:
+  MF_I2CDevice *dev;
+public:
+  BarGizmoHP203B(MF_I2C *i2c, int8_t i2c_adr, uint32_t sampleRate) {
+    (void) sampleRate; //TODO
+    if(i2c_adr == 0) i2c_adr = 0XEC; // fixed: 0XEC or 0xEE
+    this->dev = new MF_I2CDevice(i2c, i2c_adr);
+    dev->write(0b01010000, nullptr, 0); // ADC_CVT = 010, 100 OSR=256 (8.2ms), 00 press+temp
+  }
+
+  bool update(float *press, float *temp) override {
+    uint8_t d[6];
+    dev->read(0x10, d, 6); //READ_PT
+    int32_t t = ((d[0] << 16) | (d[1] << 8) | d[2]) & 0x000fffff; //20 bit big-endian signed
+    if(t & 0x00080000) t |= 0xfff00000; //20bit sign expansion
+    uint32_t p = ((d[3] << 16) | (d[4] << 8) | d[5]) & 0x000fffff; //20 bit big-endian unsigned
+
+    *temp = t * 0.01; //temperature in [C]
+    *press = 0.01 * p; //pressure in [Pa]
+    return true;
+  }
+};

src/madflight/bar/bar.cpp

@@ -29,6 +29,7 @@ SOFTWARE.
 #include "BarGizmoBMP280.h"
 #include "BarGizmoBMP390.h"
 #include "BarGizmoMS5611.h"
+#include "BarGizmoHP203B.h"
 #include <math.h>
 
 //create global module instance
@@ -43,8 +44,8 @@ int Bar::setup() {
   press = 0;
   temp = 0;
   alt = 0;
-  ts = micros();
-  dt = 0;  
+  ts = 0;
+  dt = 0;
 
   //create gizmo
   delete gizmo;
@@ -68,6 +69,11 @@ int Bar::setup() {
         gizmo = new BarGizmoMS5611(config.i2c_bus, config.i2c_adr, config.sampleRate);
       }
       break;
+    case Cfg::bar_gizmo_enum::mf_HP203B :
+      if(config.i2c_bus) {
+        gizmo = new BarGizmoHP203B(config.i2c_bus, config.i2c_adr, config.sampleRate);
+      }
+      break;
   }
 
   //check gizmo
@@ -81,17 +87,18 @@ int Bar::setup() {
 
 bool Bar::update() {
   if(!gizmo) return false;
-  if (micros() - ts >= _samplePeriod) {
-    uint32_t tsnew = micros();
-    dt = (tsnew - ts) / 1000000.0;
-    gizmo->update(&press, &temp);
-    float P = press;
-    //float T = temp;
-    //alt = 153.84348f * (1 - pow(P/101325.0f, 0.19029496f)) * (T + 273.15f); //hypsometric formula - reduces to barometric with T=15C
-    alt = 44330.0f * (1 - pow(P/101325.0f, 0.19029496f)); //barometric formula  0.19029496 = 1/5.255
-    //alt = (101325.0f - P) / 12.0f; //linearisation of barometric formula at sealevel
-    ts = tsnew;
-    return true;
-  }
-  return false;
+  
+  uint32_t now = micros();
+  if(now - ts < _samplePeriod) return false;
+  if(now - ts < 2 * _samplePeriod) ts += _samplePeriod; else ts = now; //keep exact _samplePeriod timing, unless we missed an interval
+
+  dt = (now - ts) / 1000000.0;
+  gizmo->update(&press, &temp);
+  float P = press;
+  //float T = temp;
+  //alt = 153.84348f * (1 - pow(P / 101325.0f, 0.19029496f)) * (T + 273.15f); //hypsometric formula - reduces to barometric with T=15C
+  alt = 44330.0f * (1 - pow(P / 101325.0f, 0.19029496f)); //barometric formula  0.19029496 = 1/5.255
+  //alt = (101325.0f - P) / 12.0f; //linearisation of barometric formula at sealevel
+  ts = now;
+  return true;
 }

src/madflight/cfg/cfg.h

@@ -183,7 +183,7 @@ SOFTWARE.
   MF_PARAM( ahr_gizmo,         0, int32_t, 'e', mf_MAHONY,mf_MAHONY_BF,mf_MADGWICK,mf_VQF) \
 \
   /*BAR - Barometer*/ \
-  MF_PARAM( bar_gizmo,         0, int32_t, 'e', mf_NONE,mf_BMP280,mf_BMP388,mf_BMP390,mf_MS5611) \
+  MF_PARAM( bar_gizmo,         0, int32_t, 'e', mf_NONE,mf_BMP280,mf_BMP388,mf_BMP390,mf_MS5611,mf_HP203B) \
   MF_PARAM( bar_i2c_bus,      -1, int32_t, 'i') \
   MF_PARAM( bar_i2c_adr,       0, int32_t, 'i') \
   MF_PARAM( bar_rate,       1000, float, 'f') /*Barometer sample rate in Hz (default 100)*/ \
@@ -216,7 +216,7 @@ SOFTWARE.
   MF_PARAM( led_on,            0, int32_t, 'e', mf_LOW_IS_ON,mf_HIGH_IS_ON) \
 \
   /*MAG - Magnetometer*/ \
-  MF_PARAM( mag_gizmo,         0, int32_t, 'e', mf_NONE,mf_QMC5883) \
+  MF_PARAM( mag_gizmo,         0, int32_t, 'e', mf_NONE,mf_QMC5883,mf_QMC6309) \
   MF_PARAM( mag_i2c_bus,      -1, int32_t, 'i') \
   MF_PARAM( mag_i2c_adr,       0, int32_t, 'i') \
   MF_PARAM( mag_lp,         1e10, float, 'f') /*Magnetometer Gyro Low Pass Filter cutoff frequency in Hz (default 1e10Hz, i.e. no filtering) */ \

src/madflight/hal/MF_I2C.h

@@ -35,20 +35,60 @@ class MF_I2C {
       return requestFrom(address, len, true);
     }
 
-    //non "Arduino standard" extension 
-    uint32_t transceive(uint8_t address, uint8_t* wbuf, uint32_t wlen, uint8_t* rbuf, uint32_t rlen) {
-      beginTransmission(address);
-      write(wbuf, wlen);
-      endTransmission(false);
-      requestFrom(address, rlen);
-      return read(rbuf, rlen);
-    }
-
     //virtual void begin(uint8_t address) = 0; 
     //virtual void onReceive(void(*)(int)) = 0;
     //virtual void onRequest(void(*)(void)) = 0;
 };
 
+
+class MF_I2CDevice {
+  public:
+    MF_I2C *i2c;
+    uint8_t adr;
+
+    MF_I2CDevice(MF_I2C *i2c, uint8_t adr) {
+      this->i2c = i2c;
+      this->adr = adr;
+    }
+
+    uint32_t write(uint8_t reg, uint8_t *data, uint32_t len) {
+      i2c->beginTransmission(adr);
+      uint32_t rv = i2c->write(&reg, 1);
+      if(len > 0) {
+        rv += i2c->write(data, len);
+      }
+      return rv;
+    }
+
+    uint32_t write(uint8_t reg, uint8_t data) {
+      return write(reg, &data, 1);
+    }
+
+    uint32_t read(uint8_t reg, uint8_t *data, uint32_t len) {
+      uint32_t rv = 0;
+      i2c->beginTransmission(adr);
+      i2c->write(&reg, 1);
+      if(len > 0) {
+        i2c->endTransmission(false);
+        i2c->requestFrom(adr, len);
+        rv = i2c->read(data, len);
+      }
+      return rv;
+    }
+
+    uint32_t transceive(uint8_t* wbuf, uint32_t wlen, uint8_t* rbuf, uint32_t rlen) {
+      uint32_t rv = 0;
+      i2c->beginTransmission(adr);
+      if(wlen > 0) rv = i2c->write(wbuf, wlen);
+      if(wlen > 0 && rlen > 0) i2c->endTransmission(false);
+      if(rlen > 0) {
+        i2c->requestFrom(adr, rlen);
+        rv = i2c->read(rbuf, rlen);
+      }
+      return rv;
+    }
+};
+
 // Wrapper around an arduino TwoWire-like pointer T
 template<class T>
 class MF_I2CPtrWrapper : public MF_I2C {

src/madflight/mag/MagGizmoQMC5883L.h

@@ -15,7 +15,7 @@ class MagGizmoQMC5883L: public MagGizmo {
       //return (mag_QMC5883L.detect() ? 0 : 1);
     }
 
-    bool read_uT(float *x, float *y, float *z) override {
+    bool update(float *x, float *y, float *z) override {
       mag_QMC5883L.read_uT(x, y, z);
       return true; 
     }

src/madflight/mag/MagGizmoQMC6309.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include "mag.h"
+#include "../hal/MF_I2C.h"
+
+
+class MagGizmoQMC6309 : public MagGizmo {
+private:
+  MF_I2CDevice *dev;
+public:
+  MagGizmoQMC6309(MF_I2C *i2c, int8_t i2c_adr) {
+    i2c_adr = 0x7C; // fixed: 0x7C
+    this->dev = new MF_I2CDevice(i2c, i2c_adr);
+
+    //setup for 16 sample moving average (my interpretation of data sheet OSR2 setting), sample rate = 1500Hz (continous mode)
+    dev->write(0x0B, 0x04); //ODR=1Hz, Scale=8G, Reset
+    dev->write(0x0A, 0xFD); //OSR2(filter)=16, OSR=1, Continuous Mode
+  }
+
+  bool update(float *x, float *y, float *z) override {
+    uint8_t d[6];
+    dev->read(0x01, d, 6);
+    int16_t mx = d[0] | (d[1] << 8); //16 bit litte-endian signed
+    int16_t my = d[2] | (d[3] << 8);
+    int16_t mz = d[4] | (d[5] << 8);
+
+    *x = 200e-9 * mx; //in [T]
+    *y = 200e-9 * my;
+    *z = 200e-9 * mz;
+    return true;
+  }
+};

src/madflight/mag/mag.cpp

@@ -27,6 +27,7 @@ SOFTWARE.
 #include <Arduino.h> //Serial
 #include "mag.h"
 #include "MagGizmoQMC5883L.h"
+#include "MagGizmoQMC6309.h"
 
 //create global module instance
 Mag mag;
@@ -36,6 +37,12 @@ int Mag::setup() {
 
   _samplePeriod = 1000000 / config.sampleRate;
 
+  //clear state
+   x = 0; //"North" magnetic flux [uT]
+   y = 0; //"East" magnetic flux [uT]
+   z = 0; //"Down" magnetic flux [uT]
+   ts = 0; //last sample time in [us]
+
   //create gizmo
   delete gizmo;
   switch(config.gizmo) {
@@ -47,6 +54,11 @@ int Mag::setup() {
         gizmo = new MagGizmoQMC5883L(config.i2c_bus, config.i2c_adr);
       }
       break;
+    case Cfg::mag_gizmo_enum::mf_QMC6309 :
+      if(config.i2c_bus) {
+        gizmo = new MagGizmoQMC6309(config.i2c_bus, config.i2c_adr);
+      }
+      break;
   }
 
   //check gizmo
@@ -60,7 +72,12 @@ int Mag::setup() {
 
 bool Mag::update() {
   if(!gizmo) return false;
-  if(micros() - mag_time < _samplePeriod) return false;
-  mag_time = micros();
-  return gizmo->read_uT(&x, &y, &z);
+  
+  uint32_t now = micros();
+  if(now - ts < _samplePeriod) return false;
+  if(now - ts < 2 * _samplePeriod) ts += _samplePeriod; else ts = now; //keep exact _samplePeriod timing, unless we missed an interval
+
+  if(!gizmo->update(&x, &y, &z)) return false;
+  ts = now;
+  return true;
 }

src/madflight/mag/mag.h

@@ -32,6 +32,7 @@ struct MagState {
     float x = 0; //"North" magnetic flux [uT]
     float y = 0; //"East" magnetic flux [uT]
     float z = 0; //"Down" magnetic flux [uT]
+    uint32_t ts = 0; //last sample time in [us]
 };
 
 struct MagConfig {
@@ -45,7 +46,7 @@ struct MagConfig {
 class MagGizmo {
 public:
   virtual ~MagGizmo() {};
-  virtual bool read_uT(float *x, float *y, float *z) = 0; //returns true if new sample was retrieved
+  virtual bool update(float *x, float *y, float *z) = 0; //returns true if new sample was retrieved
 };
 
 class Mag : public MagState {
@@ -59,7 +60,6 @@ class Mag : public MagState {
     bool installed() {return (gizmo != nullptr); } // Returns true if a gizmo was setup
 
   protected:
-    uint32_t mag_time = 0; //last sample time in [us]
     uint32_t _samplePeriod = 0; //gizmo sample period in [us]
 
     bool _installed = false;