AHRS Fusion With Two Adafruit LSM6DSOX + LIS3MDL Sensors Simultaneously

[simonvincenthk]

AHRS Fusion With Two Adafruit LSM6DSOX + LIS3MDL Sensors Simultaneously

I am working on generating AHRS data (see https://learn.adafruit.com/how-to-fuse-motion-sensor-data-into-ahrs-orientation-euler-quaternions) for two independent LSM6DSOX + LIS3MDL sensors (see https://www.adafruit.com/product/4517) simultaneously over one I2C wire. While I've been able to calibrate and read data from each sensor in separate programs with one sensor using its default addresses (mag: 0x1C; acc/gyr: 0x6A) and the other using its alternate addresses(mag: 0x1E; acc/gyr: 0x6B) (see https://learn.adafruit.com/st-9-dof-combo/lsm6ds33-lis3mdl-pinouts); running the fusion process on both in the same program yields faulty results:

This is a plot of the heading angles from both sensors:

The plot shows that after changes in the second sensor's orientation, its heading readings drift back to their original value relative to the first sensor. Likewise, when the first sensor's orientation is changed, its heading readings drift back to the original relative value relative to the second sensor. This indicates that one sensor may be affecting the other. I suspect that variables are getting mixed up between the two fusion computations or that the data being sent back over one wire is getting mixed, but am not resourced enough to find out what exactly is happening and how to change it.

I'm hoping someone can help with this issue.

These changes were made to the header file to accommodate two simultaneous sensors:

#include <Wire.h>
#include <Adafruit_LIS3MDL.h>
//Adafruit_LIS3MDL lis3mdl_1 = Adafruit_LIS3MDL(0x1C);
Adafruit_LIS3MDL lis3mdl_1;
Adafruit_LIS3MDL lis3mdl_2;
//lis3mdl_1.begin_i2c(0x1C);
//lis3mdl_1.setAddress(0x1C);

// Can change this to be LSM6DSOX or whatever ya like
#include <Adafruit_LSM6DSOX.h>
Adafruit_LSM6DSOX lsm6ds_1;
Adafruit_LSM6DSOX lsm6ds_2;

bool init_sensors(void) {
  if (!lsm6ds_1.begin_I2C(0x6A) || !lis3mdl_1.begin_I2C(0x1C)) {
    return false;
  }
   if (!lsm6ds_2.begin_I2C(0x6B) || !lis3mdl_2.begin_I2C(0x1E)) {
    return false;
  }
  accelerometer_1 = lsm6ds_1.getAccelerometerSensor();
  gyroscope_1 = lsm6ds_1.getGyroSensor();
  magnetometer_1 = &lis3mdl_1;

  accelerometer_2 = lsm6ds_2.getAccelerometerSensor();
  gyroscope_2 = lsm6ds_2.getGyroSensor();
  magnetometer_2 = &lis3mdl_2;

  return true;
}

void setup_sensors(void) {
  // set lowest range
  lsm6ds_1.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
  lsm6ds_1.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS);
  lis3mdl_1.setRange(LIS3MDL_RANGE_4_GAUSS);

  lsm6ds_2.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
  lsm6ds_2.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS);
  lis3mdl_2.setRange(LIS3MDL_RANGE_4_GAUSS);

  // set slightly above refresh rate
  lsm6ds_1.setAccelDataRate(LSM6DS_RATE_104_HZ);
  lsm6ds_1.setGyroDataRate(LSM6DS_RATE_104_HZ);
  lis3mdl_1.setDataRate(LIS3MDL_DATARATE_1000_HZ);
  lis3mdl_1.setPerformanceMode(LIS3MDL_MEDIUMMODE);
  lis3mdl_1.setOperationMode(LIS3MDL_CONTINUOUSMODE);

  lsm6ds_2.setAccelDataRate(LSM6DS_RATE_104_HZ);
  lsm6ds_2.setGyroDataRate(LSM6DS_RATE_104_HZ);
  lis3mdl_2.setDataRate(LIS3MDL_DATARATE_1000_HZ);
  lis3mdl_2.setPerformanceMode(LIS3MDL_MEDIUMMODE);
  lis3mdl_2.setOperationMode(LIS3MDL_CONTINUOUSMODE);
}

And these are the changes to the code:

//Included Libraries

#include <Adafruit_Sensor_Calibration.h>
#include <Adafruit_AHRS.h>
#include <Wire.h>

Adafruit_Sensor *accelerometer_1, *gyroscope_1, *magnetometer_1, *accelerometer_2, *gyroscope_2, *magnetometer_2;

// uncomment one combo 9-DoF!
// #include <Adafruit_LIS3MDL.h>
// #include <Adafruit_LSM6DSOX.h>
#include "LSM6DS_LIS3MDL.h"  // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
//#include "LSM9DS.h"           // LSM9DS1 or LSM9DS0
//#include "NXP_FXOS_FXAS.h"  // NXP 9-DoF breakout

// pick your filter! slower == better quality output
//Adafruit_NXPSensorFusion filter1; // slowest
//Adafruit_NXPSensorFusion filter2; // slowest
Adafruit_Madgwick filter1;  // faster than NXP
Adafruit_Madgwick filter2;  // faster than NXP
//Adafruit_Mahony filter;  // fastest/smalleset

#if defined(ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM)
  Adafruit_Sensor_Calibration_EEPROM cal;
#else
  Adafruit_Sensor_Calibration_SDFat cal;
#endif

#define FILTER_UPDATE_RATE_HZ 100
#define PRINT_EVERY_N_UPDATES 10
//#define AHRS_DEBUG_OUTPUT

uint32_t timestamp;

void setup() {

  Serial.begin(115200);
  while (!Serial) yield();

  if (!cal.begin()) {
    Serial.println("Failed to initialize calibration helper");
  } else if (! cal.loadCalibration()) {
    Serial.println("No calibration loaded/found");
  }

  if (!init_sensors()) {
    Serial.println("Failed to find sensors");
    while (1) delay(10);
  }

  accelerometer_1->printSensorDetails();
  gyroscope_1->printSensorDetails();
  magnetometer_1->printSensorDetails();

  accelerometer_2->printSensorDetails();
  gyroscope_2->printSensorDetails();
  magnetometer_2->printSensorDetails();

  setup_sensors();
  filter1.begin(FILTER_UPDATE_RATE_HZ);
  filter2.begin(FILTER_UPDATE_RATE_HZ);
  timestamp = millis();

  Wire.setClock(400000); // 400KHz
}

void loop() {
  float roll1, pitch1, heading1, roll2, pitch2, heading2;
  float gx1, gy1, gz1, gx2, gy2, gz2;
  static uint8_t counter = 0;

  if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) {
    return;
  }
  timestamp = millis();

  sensors_event_t accel1, gyro1, mag1,
                  accel2, gyro2, mag2;

  accelerometer_1->getEvent(&accel1);
  gyroscope_1->getEvent(&gyro1);
  magnetometer_1->getEvent(&mag1);

  accelerometer_2->getEvent(&accel2);
  gyroscope_2->getEvent(&gyro2);
  magnetometer_1->getEvent(&mag2);

#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif

  cal.calibrate(mag1);
  cal.calibrate(accel1);
  cal.calibrate(gyro1);
  cal.calibrate(mag2);
  cal.calibrate(accel2);
  cal.calibrate(gyro2);
  // Gyroscope needs to be converted from Rad/s to Degree/s
  // the rest are not unit-important
  gx1 = gyro1.gyro.x * SENSORS_RADS_TO_DPS;
  gy1 = gyro1.gyro.y * SENSORS_RADS_TO_DPS;
  gz1 = gyro1.gyro.z * SENSORS_RADS_TO_DPS;

  gx2 = gyro2.gyro.x * SENSORS_RADS_TO_DPS;
  gy2 = gyro2.gyro.y * SENSORS_RADS_TO_DPS;
  gz2 = gyro2.gyro.z * SENSORS_RADS_TO_DPS;

  // Update the SensorFusion filter
  filter1.update(gx1, gy1, gz1,
                accel1.acceleration.x, accel1.acceleration.y, accel1.acceleration.z,
                mag1.magnetic.x, mag1.magnetic.y, mag1.magnetic.z);

  filter2.update(gx2, gy2, gz2,
                accel2.acceleration.x, accel2.acceleration.y, accel2.acceleration.z,
                mag2.magnetic.x, mag2.magnetic.y, mag2.magnetic.z);

#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif

  // only print the calculated output once in a while
  if (counter++ <= PRINT_EVERY_N_UPDATES) {
    return;
  }
  // reset the counter
  counter = 0;

#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("Raw1: ");
  Serial.print(accel1.acceleration.x, 4); Serial.print(", ");
  Serial.print(accel1.acceleration.y, 4); Serial.print(", ");
  Serial.print(accel1.acceleration.z, 4); Serial.print(", ");
  Serial.print(gx1, 4); Serial.print(", ");
  Serial.print(gy1, 4); Serial.print(", ");
  Serial.print(gz1, 4); Serial.print(", ");
  Serial.print(mag1.magnetic.x, 4); Serial.print(", ");
  Serial.print(mag1.magnetic.y, 4); Serial.print(", ");
  Serial.print(mag1.magnetic.z, 4); Serial.println("");

  Serial.print("Raw2: ");
  Serial.print(accel2.acceleration.x, 4); Serial.print(", ");
  Serial.print(accel2.acceleration.y, 4); Serial.print(", ");
  Serial.print(accel2.acceleration.z, 4); Serial.print(", ");
  Serial.print(gx2, 4); Serial.print(", ");
  Serial.print(gy2, 4); Serial.print(", ");
  Serial.print(gz2, 4); Serial.print(", ");
  Serial.print(mag2.magnetic.x, 4); Serial.print(", ");
  Serial.print(mag2.magnetic.y, 4); Serial.print(", ");
  Serial.print(mag2.magnetic.z, 4); Serial.println("");
#endif

  //Print trailer and bike sensor yaw angles for troubleshooting
  Serial.print("Orientation1: ");
  Serial.print(filter1.getYaw(),2);
  Serial.print(" Orientation2: ");
  Serial.println(filter2.getYaw(),2);

  // print the heading, pitch and roll
  /* roll1 = filter1.getRoll();
  pitch1 = filter1.getPitch();
  heading1 = filter1.getYaw();
  Serial.print("Orientation1: ");
  Serial.print(heading1);
  Serial.print(", ");
  Serial.print(pitch1);
  Serial.print(", ");
  Serial.println(roll1);

  roll2 = filter2.getRoll();
  pitch2 = filter2.getPitch();
  heading2 = filter2.getYaw();
  Serial.print("Orientation2: ");
  Serial.print(heading2);
  Serial.print(", ");
  Serial.print(pitch2);
  Serial.print(", ");
  Serial.println(roll2);*/

#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("Took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif

}

I appreciate the help.

-Simon

[LazaroFilm]

Turn on the debug output by u commenting to define. What is the speed of your loop? You might be saturating your processor with two filters to be calculated simultaneously.