add FC1

Author: qqqlab

extras/I2C_Chip_Scanner/I2C_Chip_Scanner.ino

@@ -25,13 +25,13 @@ TwoWire *i2c = &Wire;
 #define ESP32_SDA_PIN 23
 #define ESP32_SCL_PIN 22
 
-#define RP2040_SDA_PIN 20 //Wire: 0, 4(default), 8, 12, 16, 20   Wire1: 2, 6, 10, 14, 18, 26(default)
-#define RP2040_SCL_PIN 21 //Wire: 1, 5(default), 9, 13, 17, 21   Wire1: 3, 7, 11, 15, 19, 27(default)
+#define RP2040_SDA_PIN 32 //RP2040/RP2350A Wire: 0, 4(default), 8, 12, 16, 20   Wire1: 2, 6, 10, 14, 18, 26(default)
+#define RP2040_SCL_PIN 33 //RP2040/RP2350A Wire: 1, 5(default), 9, 13, 17, 21   Wire1: 3, 7, 11, 15, 19, 27(default)
 
 #define STM32_SDA_PIN PB9
 #define STM32_SCL_PIN PB8
 
-#define I2C_FRQ 400000
+#define I2C_FRQ 100000
 
 
 
@@ -46,6 +46,7 @@ void setup() {
     i2c->setSDA(RP2040_SDA_PIN);
     i2c->setSCL(RP2040_SCL_PIN);
     i2c->setClock(I2C_FRQ);
+    i2c->setTimeout(25, true); //timeout, reset_with_timeout
     i2c->begin();
   #elif defined ARDUINO_ARCH_STM32
     i2c->setSDA(STM32_SDA_PIN);
@@ -63,16 +64,17 @@ void loop() {
 
   Serial.printf("I2C: Scanning ...\n");
   int num_adr_found = 0;
-  for (uint8_t adr = 8; adr < 120; adr++) {
+  for (uint8_t adr = 1; adr < 128; adr++) {
     i2c->beginTransmission(adr);       // Begin I2C transmission Address (i)
     if (i2c->endTransmission() == 0) { // Receive 0 = success (ACK response) 
       Serial.printf("I2C: Found address: 0x%02X (decimal %d)\n",adr,adr);
       num_adr_found++;
       i2c_identify_chip(adr);
     }
+    delay(10);
   }
   Serial.printf("I2C: Found %d device(s)\n", num_adr_found);
-  delay(1000);
+  delay(100);
 }
 
 struct test_struct{
@@ -108,6 +110,7 @@ test_struct tests[] = {
   {0x68, 0x69, 0x75, 1, 0x75, "FAKE MPU9250, got WHO_AM_I=0x75, real chip returns 0x71"}, 
   {0x68, 0x69, 0x75, 1, 0x78, "FAKE MPU9250, got WHO_AM_I=0x78, real chip returns 0x71"},  
   {0x68, 0x69, 0x75, 1, 0x98, "ICM20689 6DOF motion"},
+  {0x76, 0x77, 0x8F, 1, 0x80, "HP203B pressure"}, //needs to be first 0x76 sensor, otherwise this sensor will lock up
   {0x76, 0x77, 0x00, 1, 0x50, "BMP388 pressure"},
   {0x76, 0x77, 0x00, 1, 0x60, "BMP390 pressure"},  
   {0x76, 0x77, 0x0D, 1, 0x10, "DPS310, HP303B, or SPL06 pressure"},
@@ -117,6 +120,7 @@ test_struct tests[] = {
   {0x76, 0x77, 0xD0, 1, 0x58, "BMP280 pressure"},
   {0x76, 0x77, 0xD0, 1, 0x60, "BME280 pressure, temperature, humidity"},
   {0x76, 0x77, 0xD0, 1, 0x61, "BME680 pressure, temperature, humidity, gas sensor"},
+  {0x7C, 0x7C, 0x00, 1, 0x90, "QMC6309 magnetometer"},
 
   {0,0,0,0,0,""} //end
 };
@@ -202,6 +206,7 @@ void i2c_ReadRegs( uint8_t adr, uint8_t reg, uint8_t *data, uint8_t n ) {
   if(bytesReceived == n) {
     i2c->readBytes(data, bytesReceived);
   }
+  i2c->endTransmission();
 }
 
 void i2c_scan() {

src/madflight.h

@@ -124,12 +124,8 @@ void madflight_setup() {
   cfg.load_madflight(madflight_board, madflight_config); //load config
 
   #ifdef MF_DEBUG
-    if(madflight_config) {
-      Serial.println("\nDEBUG: madflight_config ==============\n");
-      Serial.println(madflight_config);
-    }
-    Serial.println("\nDEBUG: cfg.list() ================\n");
-    cfg.list();
+    //Serial.println("\nDEBUG: cfg.list() ================\n");
+    //cfg.list();
   #endif
 
   cfg.printPins();

src/madflight/bar/BarGizmoHP203B.h

@@ -9,24 +9,42 @@ class BarGizmoHP203B: public BarGizmo {
 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
+    if(i2c_adr == 0) i2c_adr = 0x76; // fixed: 0x76 or 0x77
     this->dev = new MF_I2CDevice(i2c, i2c_adr);
-    dev->writeReg(0b01010000, nullptr, 0); // ADC_CVT = 010, 100 OSR=256 (8.2ms), 00 press+temp
+
+    dev->writeReg(0x06, nullptr, 0); //SOFT_RST soft reset
+    dev->writeReg(0x28, nullptr, 0); //ANA_CAL Re-calibrate the Internal analog Blocks
+    dev->writeReg(0b01010000, nullptr, 0); //start conversion: ADC_CVT = 010, 100 OSR=256 (8.2ms), 00 press+temp
   }
 
   ~BarGizmoHP203B() {
     delete dev;
   }
 
+
+
+/*
+OSR  T[ms]  ADC_CVT
+ 128   4.1 0b01010100
+ 256   8.2 0b01010000
+ 512  16.4 0b01001100
+1024  32.8 0b01001000
+2048  65.6 0b01000100
+4096 131.1 0b01000000 
+*/
   bool update(float *press, float *temp) override {
     uint8_t d[6];
-    dev->readReg(0x10, d, 6); //READ_PT
+    dev->readReg(0x10, d, 6); //READ_PT pressure [Pa] + temp [0.01*C]
+    //dev->readReg(0x11, d, 6); //READ_AT altitude [0.01*m] + temp [0.01*C]
+    dev->writeReg(0b01010000, nullptr, 0); //start conversion: ADC_CVT = 010, 100 OSR=256 (8.2ms), 00 press+temp
+
     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]
+    *press = p; //pressure in [Pa]
+
     return true;
   }
 };

src/madflight/cli/cli.cpp

@@ -408,7 +408,7 @@ void Cli::print_i2cScan() {
     if(i2c) {
       Serial.printf("I2C: Scanning i2c_bus:%d - ", bus_i);
       int count = 0;
-      for (byte i = 8; i < 120; i++) {
+      for (byte i = 1; i < 128; i++) {
         i2c->beginTransmission(i);          // Begin I2C transmission Address (i)
         if (i2c->endTransmission() == 0) {  // Receive 0 = success (ACK response) 
           Serial.printf("0x%02X(%d) ", i, i);

src/madflight/cli/stat.h

@@ -36,6 +36,6 @@ class Stat {
     return sqrt(var());
   }
   void print(const char *hdr) {
-    Serial.printf("%s mean:%+f stdev:%f var:%f min:%+f max:%+f n:%d\n", hdr, mean(), std(), var(), min, max, n);
+    Serial.printf("%s\tmean:%+f\tstdev:%f\tmin:%+f\tmax:%+f\tn:%d\n", hdr, mean(), std(), min, max, n);
   }
 };

src/madflight/mag/MagGizmoQMC6309.h

@@ -27,36 +27,163 @@ SOFTWARE.
 #include "mag.h"
 #include "../hal/MF_I2C.h"
 
+//#include "../cli/stat.h" //for debugging
+
+
+// https://www.lcsc.com/datasheet/lcsc_datasheet_2410121623_QST-QMC6309_C5439871.pdf
+
+// Who Am I register
+#define QMC6309_WAI_REG 0x00
+
+#define QMC6309_WAI_VALUE 0x90
+
+// Data register
+#define QMC6309_OUTX_REG 0x01
+
+// Status register {DRDY:1;OVFL:1;...}
+#define QMC6309_STAT_REG 0x09
+
+#define QMC6309_STAT_DRDY 0x01 //1=data ready
+#define QMC6309_STAT_OVFL 0x02 //1=overflow
+#define QMC6309_STAT_ST_RDY 0x04 //1=self test ready
+#define QMC6309_STAT_NVM_RDY 0x08 //1=nvm ready for access
+#define QMC6309_STAT_NVM_LOAD_DONE 0x10 //1=nvm loading done
+
+// Control register 1 {MD:2;:1;OSR:2;LPF:3;}
+#define QMC6309_CTRL1_REG 0x0A
+
+#define QMC6309_CTRL1_MD_SUSPEND    0b00
+#define QMC6309_CTRL1_MD_NORMAL     0b01
+#define QMC6309_CTRL1_MD_SINGLE     0b10
+#define QMC6309_CTRL1_MD_CONTINUOUS 0b11
+
+#define QMC6309_CTRL1_OSR_1 (0b11<<3)
+#define QMC6309_CTRL1_OSR_2 (0b10<<3)
+#define QMC6309_CTRL1_OSR_4 (0b01<<3)
+#define QMC6309_CTRL1_OSR_8 (0b00<<3)
+
+#define QMC6309_CTRL1_LPF_1  (0b000<<5)
+#define QMC6309_CTRL1_LPF_2  (0b001<<5)
+#define QMC6309_CTRL1_LPF_4  (0b010<<5)
+#define QMC6309_CTRL1_LPF_8  (0b011<<5)
+#define QMC6309_CTRL1_LPF_16 (0b100<<5)
+
+// Control register 2 {SET:2;RGN:2;ODR:3;SRT:1;}
+#define QMC6309_CTRL2_REG 0x0B
+
+#define QMC6309_CTRL2_SET_RESET_ON  0b00
+#define QMC6309_CTRL2_SET_ONLY      0b01
+#define QMC6309_CTRL2_SET_RESET_OFF 0b11
+
+#define QMC6309_CTRL2_RNG_32G (0b00<<2)
+#define QMC6309_CTRL2_RNG_16G (0b01<<2)
+#define QMC6309_CTRL2_RNG_8G  (0b10<<2)
+
+//output data rate for normal mode
+#define QMC6309_CTRL2_ODR_1Hz   (0b000<<4)
+#define QMC6309_CTRL2_ODR_10Hz  (0b001<<4)
+#define QMC6309_CTRL2_ODR_50Hz  (0b010<<4)
+#define QMC6309_CTRL2_ODR_100Hz (0b011<<4)
+#define QMC6309_CTRL2_ODR_200Hz (0b100<<4)
+
+#define QMC6309_CTRL2_SOFT_RESET_START 0x80
+#define QMC6309_CTRL2_SOFT_RESET_STOP  0x00
+
+
 class MagGizmoQMC6309 : public MagGizmo {
 private:
   MF_I2CDevice *dev = nullptr;
 
+
 public:
-  MagGizmoQMC6309(MF_I2C *i2c, int8_t i2c_adr) {
-    i2c_adr = 0x7C; // fixed: 0x7C
-    this->dev = new MF_I2CDevice(i2c, i2c_adr);
+  const float scale_uT = 0.025; //scale factor to uT   (at +/-800uT (+/-8G) RNG)
+    int16_t mx; //raw adc values
+    int16_t my;
+    int16_t mz;
 
-    //setup for 16 sample moving average (my interpretation of data sheet OSR2 setting), sample rate = 1500Hz (continous mode)
-    dev->writeReg(0x0B, 0x04); //ODR=1Hz, Scale=8G, Reset
-    dev->writeReg(0x0A, 0xFD); //OSR2(filter)=16, OSR=1, Continuous Mode
+  ~MagGizmoQMC6309() {
+    delete dev;
   }
 
+  MagGizmoQMC6309(MF_I2C *i2c) {
+    i2c->setClock(400000);
 
-  ~MagGizmoQMC6309() {
-    delete dev;
+    this->dev = new MF_I2CDevice(i2c, 0x7C); //i2c address is always 0x7C
+
+    //soft reset
+    //dev->writeReg(QMC6309_CTRL2_REG, QMC6309_CTRL2_SOFT_RESET_START);
+    //dev->writeReg(QMC6309_CTRL2_REG, QMC6309_CTRL2_SOFT_RESET_STOP);
+
+    //configure 220Hz update rate with LPF_16
+    uint8_t ctrl2_val = QMC6309_CTRL2_SET_RESET_ON | QMC6309_CTRL2_RNG_8G | QMC6309_CTRL2_ODR_100Hz;
+    uint8_t ctrl1_val = QMC6309_CTRL1_MD_CONTINUOUS | QMC6309_CTRL1_OSR_8 | QMC6309_CTRL1_LPF_16; //mx	mean:-10.982379	stdev:1.401531	min:-14.000000	max:-8.000000	n:227
+    //uint8_t ctrl1_val = QMC6309_CTRL1_MD_CONTINUOUS | QMC6309_CTRL1_OSR_8 | QMC6309_CTRL1_LPF_1; //mx	mean:-10.060345	stdev:6.673316	min:-33.000000	max:+4.000000	n:232
+    //uint8_t ctrl1_val = QMC6309_CTRL1_MD_CONTINUOUS | QMC6309_CTRL1_OSR_1 | QMC6309_CTRL1_LPF_1; //mx	mean:-11.925335	stdev:38.645042	min:-119.000000	max:+106.000000	n:1125
+
+    int tries = 20;
+    uint8_t wai = 0;
+    while(tries) {
+      uint8_t ctrl1 = dev->readReg(QMC6309_CTRL1_REG);
+      uint8_t ctrl2 = dev->readReg(QMC6309_CTRL2_REG);
+      wai = dev->readReg(QMC6309_WAI_REG);
+
+      //setup
+      if(ctrl2 != ctrl2_val) dev->writeReg(QMC6309_CTRL2_REG, ctrl2_val);
+      if(ctrl1 != ctrl1_val) dev->writeReg(QMC6309_CTRL1_REG, ctrl1_val);
+      delay(1);
+
+      uint8_t stat = dev->readReg(QMC6309_STAT_REG);
+      if(stat & QMC6309_STAT_DRDY) {
+        break;
+      }
+
+      tries--;
+
+      //Serial.printf("try wai=%02X stat=%02X ctrl2=%02X(%02X) ctrl1=%02X(%02X)\n", wai, stat, ctrl2, ctrl2_val, ctrl1, ctrl1_val);
+    }
+    if(!tries) Serial.printf("MAG: ERROR could not configure QMC6309. wai:0x%02X, expected 0x%02X\n", wai, QMC6309_WAI_VALUE);
+
+    /* //DEBUG 
+    while(1) {
+      Stat mx,my,mz;
+      uint32_t start_ts = micros();
+      while(micros() - start_ts < 1000000) {
+        float x,y,z;
+        if(update(&x,&y,&z)) {
+          mx.append(x);
+          my.append(y);
+          mz.append(z);
+        }
+      }
+      
+      Serial.println("=== Magnetometer (external) ===");
+      mx.print("mx");
+      my.print("my");
+      mz.print("mz");
+    }
+    */
   }
 
+  bool update_raw() {
+    uint8_t stat = dev->readReg(QMC6309_STAT_REG);
+    if((stat & QMC6309_STAT_DRDY) == 0 || (stat & QMC6309_STAT_OVFL) != 0 ) return false;
 
-  bool update(float *x, float *y, float *z) override {
     uint8_t d[6];
-    dev->readReg(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;
+    dev->readReg(QMC6309_OUTX_REG, d, 6);
+
+    mx = d[0] | (d[1] << 8); //16 bit litte-endian signed - 40LSB/uT at +/-800uT scale
+    my = d[2] | (d[3] << 8);
+    mz = d[4] | (d[5] << 8);
+
+    return true;
+  }
+
+  bool update(float *x, float *y, float *z) override {
+    if(!update_raw()) return false;
+
+    *x =  scale_uT * mx; 
+    *y =  scale_uT * my;
+    *z =  scale_uT * mz;
     return true;
   }
 };

src/madflight/mag/mag.cpp

@@ -57,7 +57,7 @@ int Mag::setup() {
       break;
     case Cfg::mag_gizmo_enum::mf_QMC6309 :
       if(config.i2c_bus) {
-        gizmo = new MagGizmoQMC6309(config.i2c_bus, config.i2c_adr);
+        gizmo = new MagGizmoQMC6309(config.i2c_bus); //i2c address is always 0x7C
       }
       break;
     case Cfg::mag_gizmo_enum::mf_RM3100 :