ICM45686 SPI driver

[vidma]

I'm adding ICM45686 based on https://github.com/tdk-invn-oss/motion.arduino.ICM45686
Seems like that's the best <5-10eur IMU out there.

@qqqlab

Status:

• Already getting Gyro+Acc readings at 800Hz+ via SPI + interrupt. using this IMU board costing only 6 Euro
  https://store.kouno.xyz/products/icm-45686-ist8306-module
  • this module also has a IST8306 magnetometer connected to directly IMU ( i.e. to ICM45686 chip via I2C). didn't check about code/driver yet.
• Found/fixed a bug in https://github.com/tdk-invn-oss/motion.arduino.ICM45686 which was making code stuck during init
• need to verify the scale and readings is OK, and maybe axis orientations
• need to cleanup code
• @qqqlab do you prefer to copy-paste so your app has no dependencies or is it fine to keep it as dependency?
• alternatively, if time allows someone could check Ardupilot code for these IMUs as it's going to be well tested [1]

P.S. for #28 #29 probably one could use https://github.com/finani/ICM42688 (lol I even mixed up sensor name and started, hacking this one first to find out it was wrong driver from my sensor :D so I can tell APIs are rather similar, but a bit different )
or Ardupilot code [1] which support multiple sensors (but I guess might be more work to adapt it?):

---

[1] https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

  Ardupilot: driver for Invensensev3 IMUs

  Supported:
   ICM-40609
   ICM-42688
   ICM-42605
   ICM-40605 - EOL
   IIM-42652
   ICM-42670
   ICM-45686

https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

[qqqlab]

Great that your working on this @vidma

I actually have a couple of 42688's laying around, but did not get around to actually build the driver.

So yes, please go ahead and create a PR for 45686 (hopefully in combo with 42688). I'll gladly merge it. I prefer to copy/paste external code, to keep this project with minimal external dependencies which could potentially break in the future.

I was also looking at ArduPilot/Betaflight/INAV in order to "steal" their gyro drivers. Last week I ported the ArduPilot uBlox gps driver to madflight, but it was more work than expected. So maybe starting from scratch with the TDK code is the easier route.

[qqqlab]

resolved with PR #37. Thanks @vidma !

[brightproject]

I'm also trying to work with this sensor on this board.

I got the test code from here and tried changing the pins for the SPI bus.
I managed to connect by changing the pins directly in the class code.

tdk-invn-oss/motion.arduino.ICM45686#3

I don't know yet how to connect to the qmc6309 magnetometer.

My SPI connection is as follows: I receive data from the accelerometer and gyroscope, as well as from the built-in temperature sensor.

spi->begin(SCK, MISO, MOSI, SS);
spi->begin(GPIO_NUM_17, GPIO_NUM_38, GPIO_NUM_16, GPIO_NUM_39);

Please tell me how to connect to the magnetometer, which is connected to pins 2 and 3 of the chip icm45686?

[qqqlab]

Do you have a schematic of this board?

Looking at the image, you could try hooking up the SDX and SCX pins to talk i2c directly to the QMC chip. Madflight does not support ICM dual interface.

i.e. connect like this:

mf <--i2c--> qmc
mf <--spi--> icm

and not:

mf <--spi--> icm <--i2c--> qmc

[brightproject]

There is a diagram of a similar board where the magnetometer AK09940A

Here is the document in full
SM-ICM45686-AK09940A-DS_v1.01.pdf
On my white board, the ICM and QMC connections are similar.
But as far as I understand, the ICM needs to be connected via I2C and connected to a specific register to read the QMC contents that enter it.
The ICM acts as a host.
The problem is that I couldn't connect to the sensor on the ESP32S3 microcontroller via I2C, but I did connect via SPI.
Are you using the ICM separately in your code, without the magnetometer?
I'd like to use this code as a basis.

https://github.com/therealwokewok/ICM20948/blob/master/ICM20948.c

I'll rewrite it for ICM45686😀

[brightproject]

Hello @qqqlab

i.e. connect like this:

I figured out how to connect the board😀

Now I can connect via both SPI and I2C.

I'd like to connect like this:

mf <--i2c--> icm <--i2c--> qmc

or

mf <--spi--> icm <--i2c--> qmc

Could you help with this?

[brightproject]

I spent 3 days and 2 nights connecting the board and determining the I2C addresses😀

I can't connect to the QMC via IMU:

1. Because I don't have enough knowledge of coding for this, I wanted to use as examples

https://github.com/adafruit/Adafruit_ICM20X/blob/5089e9be159ba8ef654e4f0c607d14dc77069461/Adafruit_ICM20X.cpp#L662

and

https://github.com/therealwokewok/ICM20948/blob/210a96112b5578ace03c8caa5ffcdc122e416530/ICM20948.c#L67

2. The PCB is laid out without jumpers, which limits my experimentation.
   In the end, I cut the PCB traces from the SDA/SCL QMC to pins 2 and 3 of the IMU.

I removed two 3.7 kOhm resistors and connected the SDA/SCL pins of the QMC sensor directly to the SDA/SCL IMU.
Next, I played around with the I2C bus scanning code.
The code I used was as follows:

#include <Arduino.h>
#include <driver/i2c.h>

#define SERIAL_BAUD 115200

#define I2C_FREQ (400000)
#define SDA_PIN (GPIO_NUM_16)
#define SCL_PIN (GPIO_NUM_17)

static const char *TAG = "i2cscanner";

void task(void *ignore)
{
  Serial.begin(SERIAL_BAUD);
  Serial.println("\nI2C Scanner");

  i2c_config_t conf;
//   conf.mode = I2C_MODE_SLAVE,
  conf.mode = I2C_MODE_MASTER,
//   conf.mode = I2C_MODE_MAX,
  conf.sda_io_num = SDA_PIN,
  conf.scl_io_num = SCL_PIN,

  conf.master.clk_speed = I2C_FREQ,
      
  i2c_param_config(I2C_NUM_0, &conf);
  i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0);

  while (1)
  {
      esp_err_t res;
      // printf("     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f\n");
      Serial.print("     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f\n");
      // printf("00:         ");
      Serial.print("00:         ");
      // for (uint8_t i = 3; i < 0x78; i++)
      for (uint8_t i = 3; i < 0x7D; i++)
      // for (uint8_t i = 3; i < 127; i++)
      // for (uint8_t i = 1; i < 127; i++)
      {
          i2c_cmd_handle_t cmd = i2c_cmd_link_create();
          i2c_master_start(cmd);
          i2c_master_write_byte(cmd, (i << 1) | I2C_MASTER_WRITE, 1 /* expect ack */);
          i2c_master_stop(cmd);
  
          res = i2c_master_cmd_begin(I2C_NUM_0, cmd, 10 / portTICK_PERIOD_MS);
          if (i % 16 == 0)
              // printf("\n%.2x:", i);
              Serial.printf("\n%.2x:", i);
          if (res == 0)
              // printf(" %.2x", i);
              Serial.printf(" %.2x", i);
          else
              // printf(" --");
              Serial.print(" --");
          i2c_cmd_link_delete(cmd);
      }
      // printf("\n\n");
      Serial.print("\n\n");
      vTaskDelay(pdMS_TO_TICKS(1000));
  }
}

void setup()
{
    // Start task
    // xTaskCreatePinnedToCore(task, TAG, configMINIMAL_STACK_SIZE * 8, NULL, 5, NULL, 1);
    // xTaskCreatePinnedToCore(task, TAG, configMINIMAL_STACK_SIZE + 2048, NULL, 5, NULL, 1);
    // xTaskCreatePinnedToCore(task, TAG, 4096, NULL, 5, NULL, 0);
    xTaskCreate(task, TAG, configMINIMAL_STACK_SIZE * 4, NULL, 5, NULL);
}

void loop() {
// NOTHING
}

The initial problem was that the QMC address was 0x7C, but for some reason the scanner was scanning BEFORE this address:

for (uint8_t i = 3; i < 0x78; i++)

I tried increasing the range to:

for (uint8_t i = 1; i < 127; i++)

But the code didn't scan anything at all, not even the IMU address 0x68/0x69.
In the end, I had to scan the bus with an old scanner, the code for which is below:

#include <Arduino.h>
#include <Wire.h>

#define SERIAL_BAUD 115200

#define I2C_SCANNER

// Set I2C bus to use: Wire, Wire1, etc.
#define MY_WIRE Wire
#define I2C_FREQ (400000)
#define SDA_I2C (GPIO_NUM_16)
#define SCL_I2C (GPIO_NUM_17)
// #define SCL_I2C (GPIO_NUM_38)

// #define SDA_I2C (GPIO_NUM_38)
// #define SCL_I2C (GPIO_NUM_16)

void i2c_scan()
{

  byte error, address;
  int nDevices;

  Serial.println("Scanning...");

  nDevices = 0;
  for(address = 1; address < 127; address++ )
  {
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    MY_WIRE.beginTransmission(address);
    error = MY_WIRE.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16)
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error==4)
    {
      Serial.print("Unknown error at address 0x");
      if (address<16)
        Serial.print("0");
      Serial.println(address,HEX);
    }
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");

  delay(2000);           // wait 2 seconds for next scan
}
void setup() {

  Serial.begin(SERIAL_BAUD);

  Serial.println("\nI2C Scanner");
  // MY_WIRE.begin();
  // With setClock separately this error
  // [    24][E][MY_WIRE.cpp:384] setClock(): could not acquire lock
  // MY_WIRE.setClock(I2C_FREQ);
  // https://github.com/espressif/arduino-esp32/issues/6616#issuecomment-1186305621
  MY_WIRE.begin(SDA_I2C, SCL_I2C, I2C_FREQ);

}

void loop() {

#ifdef I2C_SCANNER
i2c_scan();
#endif

}

Using this "old" scanner code, I confirmed that the I2C address of the qmc6309 is 0x7C, because the datasheet very modestly indicates this and adds that a different I2C address may be available upon request from the manufacturer.

Now I have specified the scanning range for the "new" scanner like this:

for (uint8_t i = 3; i < 0x7D; i++)

The result was as follows:

I2C Scanner
     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- -- 69 -- -- -- -- -- --
70: -- -- -- -- -- -- -- -- -- -- -- -- 7c

[brightproject]

Sketch code for obtaining 9-dof data accel | gyro | mag.

/*
 *
 * Copyright (c) [2020] by InvenSense, Inc.
 * 
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */
 
#include "ICM45686.h"

// attempt to integrate SlimeVR code - no success
// #include "QMC6309_SlimeVR.h"

// attempt to integrate Mad Flight code - no success
// #include "mag_mf.h"

// rewrote MF for my own use - success
#include "mag_qmc6309_mf.h"

/* If you don't specify the I2C address, it defaults to 0x68, and then you need to pull the SDO pin on the board to ground.
But you can explicitly specify the address below */

// 0x68 SDO LOW
// 0x69 SDO HIGH

// #define ADDR_0x68

// Instantiate an ICM456XX with LSB address set to 0
// ICM456xx IMU(Wire,0);
ICM456xx IMU(Wire,0x69);

// I2c addr 0x7C
QMC6309 magSensor;

float Mx, My, Mz;

void setup() {
  int ret;
  Serial.begin(115200);
  while(!Serial) {}

  #ifdef ADDR_0x68
    pinMode(GPIO_NUM_38, OUTPUT);
    digitalWrite(GPIO_NUM_38, LOW);
  #endif

  // Initializing the ICM456XX
  ret = IMU.begin();
  if (ret != 0) {
    Serial.print("ICM456xx initialization failed: ");
    Serial.println(ret);
    while(1);
  }
  // Accel ODR = 100 Hz and Full Scale Range = 16G
  IMU.startAccel(100,16);
  // Gyro ODR = 100 Hz and Full Scale Range = 2000 dps
  IMU.startGyro(100,2000);
  // Wait IMU to start
  delay(100);


  if (!magSensor.begin()) {
    Serial.println("Failed to initialize QMC6309 sensor!");
    while (1) {
      delay(1000);
    }
  } else {
    Serial.println("QMC6309 initialized successfully.");
  }

}

void loop() {

  inv_imu_sensor_data_t imu_data;

  // Read registers
  IMU.getDataFromRegisters(imu_data);

  // https://github.com/tdk-invn-oss/motion.mcu.icm45686.driver/blob/16e45db85ba817ad1ec401c602eccc09e4f4e82f/examples/basic_read_registers/basic_read_registers.c#L123

  // accel_g[0]  = (float)(d.accel_data[0] * 4 /* gee */) / 32768;
  // accel_g[1]  = (float)(d.accel_data[1] * 4 /* gee */) / 32768;
  // accel_g[2]  = (float)(d.accel_data[2] * 4 /* gee */) / 32768;
  // gyro_dps[0] = (float)(d.gyro_data[0] * 2000 /* dps */) / 32768;
  // gyro_dps[1] = (float)(d.gyro_data[1] * 2000 /* dps */) / 32768;
  // gyro_dps[2] = (float)(d.gyro_data[2] * 2000 /* dps */) / 32768;

  // Format data for Serial Plotter
  Serial.print("AccelX:");
  Serial.print(imu_data.accel_data[0]);
  Serial.print(",");
  Serial.print("AccelY:");
  Serial.print(imu_data.accel_data[1]);
  Serial.print(",");
  Serial.print("AccelZ:");
  Serial.print(imu_data.accel_data[2]);
  Serial.print(",");
  Serial.print("GyroX:");
  Serial.print(imu_data.gyro_data[0]);
  Serial.print(",");  
  Serial.print("GyroY:");
  Serial.print(imu_data.gyro_data[1]);
  Serial.print(",");
  Serial.print("GyroZ:");
  Serial.print(imu_data.gyro_data[2]);
  Serial.print(",");
  Serial.print("Temperature:");
  // Serial.print(imu_data.temp_data);
  Serial.print((float)25 + ((float)imu_data.temp_data / 128));
  Serial.println("");

  if (magSensor.read(Mx, My, Mz)) {
    Serial.printf("Magnetometer (uT): X=%.2f Y=%.2f Z=%.2f\n", Mx, My, Mz);
  } else {
    Serial.println("Data not ready or overflow.");
  }

  // Run @ ODR 10Hz
  delay(100);
}

Header file mag_qmc6309_mf.h

/*==========================================================================================
MIT License

Copyright (c) 2023-2025 https://madflight.com

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
===========================================================================================*/

#ifndef MAG_QMC6309_MF_H
#define MAG_QMC6309_MF_H

#include <Arduino.h>
#include <Wire.h>

// Регистры и константы QMC6309
#define QMC6309_WAI_REG 0x00
#define QMC6309_WAI_VALUE 0x90

#define QMC6309_OUTX_REG 0x01
#define QMC6309_STAT_REG 0x09

#define QMC6309_STAT_DRDY 0x01
#define QMC6309_STAT_OVFL 0x02

#define QMC6309_CTRL1_REG 0x0A
#define QMC6309_CTRL2_REG 0x0B

#define QMC6309_CTRL1_MD_CONTINUOUS 0b11
#define QMC6309_CTRL1_OSR_8 (0b00 << 3)
#define QMC6309_CTRL1_LPF_16 (0b100 << 5)

#define QMC6309_CTRL2_SET_RESET_ON 0b00
#define QMC6309_CTRL2_RNG_8G (0b10 << 2)
#define QMC6309_CTRL2_ODR_100Hz (0b011 << 4)

#define QMC6309_CTRL2_SOFT_RESET_START 0x80
#define QMC6309_CTRL2_SOFT_RESET_STOP 0x00

class QMC6309 {
private:
    TwoWire *wire;
    uint8_t address = 0x7C; // 7-bit I2C address
    float scale_uT = 0.025f;
    float magneticDeclinationDegrees = 0.0f; // user-adjustable magnetic declination

    int16_t mx = 0, my = 0, mz = 0;

    uint8_t readRegister(uint8_t reg) {
        wire->beginTransmission(address);
        wire->write(reg);
        wire->endTransmission(false);
        wire->requestFrom(address, (uint8_t)1);
        if (wire->available()) {
            return wire->read();
        }
        return 0xFF;
    }

    bool readRegisters(uint8_t reg, uint8_t *buffer, uint8_t len) {
        wire->beginTransmission(address);
        wire->write(reg);
        if (wire->endTransmission(false) != 0) return false;
        uint8_t count = wire->requestFrom(address, len);
        if (count != len) return false;
        for (uint8_t i = 0; i < len; i++) {
            buffer[i] = wire->read();
        }
        return true;
    }

    bool writeRegister(uint8_t reg, uint8_t val) {
        wire->beginTransmission(address);
        wire->write(reg);
        wire->write(val);
        return (wire->endTransmission() == 0);
    }

public:
    QMC6309(TwoWire &wirePort = Wire) : wire(&wirePort) {}

    bool begin() {
        wire->setClock(400000);
        wire->begin();

        // WHO_AM_I Check
        uint8_t wai = readRegister(QMC6309_WAI_REG);
        if (wai != QMC6309_WAI_VALUE) {
            Serial.printf("QMC6309: Wrong WHO_AM_I value: 0x%02X\n", wai);
            return false;
        }

        // configuration CTRL2 и CTRL1
        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;

        if (!writeRegister(QMC6309_CTRL2_REG, ctrl2_val)) return false;
        if (!writeRegister(QMC6309_CTRL1_REG, ctrl1_val)) return false;

        delay(10);

        return true;
    }

    bool updateRaw() {
        uint8_t stat = readRegister(QMC6309_STAT_REG);
        if ((stat & QMC6309_STAT_DRDY) == 0 || (stat & QMC6309_STAT_OVFL) != 0) return false;

        uint8_t data[6];
        if (!readRegisters(QMC6309_OUTX_REG, data, 6)) return false;

        mx = (int16_t)(data[0] | (data[1] << 8));
        my = (int16_t)(data[2] | (data[3] << 8));
        mz = (int16_t)(data[4] | (data[5] << 8));

        return true;
    }

    bool read(float &x, float &y, float &z, bool raw = false) {
        if (!updateRaw()) return false;

        if (raw) {
            // Return raw data without scaling
            x = mx;
            y = my;
            z = mz;
        } else {
            // Returning calibrated values
            x = mx * scale_uT;
            y = my * scale_uT;
            z = mz * scale_uT;
        }

        return true;
    }


    //  New azimuth calculation function with calibration
    int getAzimuth360calib(float y, float x) {
        float heading = -atan2(y, x) * 180.0 / PI;
        heading += magneticDeclinationDegrees; // use an internal class variable

        // normalization in range [0, 360)
        heading = fmod(heading, 360.0);
        if (heading < 0) heading += 360.0;

        return (int)heading;
    }

    // set magnetic declination (for calibration)
    void setDeclination(float degrees) {
        magneticDeclinationDegrees = degrees;
    }
};

#endif // MAG_QMC6309_MF_H

I'd like to fully understand the MF code topology. I just want to get the Euler angles by inputting the accelerometer, gyro, and magnetometer into the filter.
I'd like to see more simple code examples in the folder

https://github.com/qqqlab/madflight/tree/main/examples

[brightproject]

Hello @qqqlab 😀
Could you help with the c++ code to obtain data from the qmc magnetometer connected to the aux i2c in passthrough mode via the icm?

[qqqlab]

@brightproject Sorry, I have no interest to program this feature, as it would be for only this specific combination of chips.

[brightproject]

Sorry for wasting your time.

[brightproject]

Sorry, I have no interest to program this feature

#include <Arduino.h>
#include <Wire.h>

#define SERIAL_BAUD 115200
#define MY_WIRE Wire
#define I2C_FREQ (400000)
#define SDA_I2C (GPIO_NUM_6)
#define SCL_I2C (GPIO_NUM_7)

// Registers for passthrough (User Bank 0)
#define REG_SCENARIO_AUX_OVRD 0x30

#define ICM_ADDR 0x69 // Address ICM45686
#define QMC_ADDR 0x7C // Address QMC6309

void writeReg(uint8_t addr, uint8_t reg, uint8_t data) { 
  Wire.beginTransmission(addr); 
  Wire.write(reg); 
  Wire.write(data); 
  Wire.endTransmission();
}
void setup() { 

Serial.begin(SERIAL_BAUD); 
Wire.begin(SDA_I2C, SCL_I2C, I2C_FREQ); 

// Enable|Disable passthrough mode - saves icm45686 to memory 
writeReg(ICM_ADDR, REG_SCENARIO_AUX_OVRD, 0x18); // AUX1_MODE_OVRD, AUX1 in I2CM Bypass 

// writeReg(ICM_ADDR, REG_SCENARIO_AUX_OVRD, 0x00); // disable 

delay(50); // Wait for the configuration to take effect

} // SETUP
void loop() {

} //LOOP

I2C Scanner
Scanning...
I2C device found at address 0x69 !
I2C device found at address 0x7C !
done