Madgwick on Nano 33 BLE IMU

[UncleFatBacon]

Hi all,

I've been working on a tilt-compensated system based on a Nano 33 BLE and a DRV2605L haptic motor driver. The concept is quite simple, I'm triggering a haptic motor whenever the system is pointing north.

I'm basing my code on this code, explained in this video. The sample code works great, but it does not use the magnetometer on the IMU, resulting in substantial drift. The Madgwick library (found here) has the option of reading magnetometer data as well. Here's my code (with some parts adapted from Adafruit's DRV2605L sample code, rewritten to avoid using delays):

#include <Wire.h>
#include "Adafruit_DRV2605.h"
#include <Arduino_LSM9DS1.h>
#include "MadgwickAHRS.h"

//initialise haptic motor
Adafruit_DRV2605 drv;

// initialise a Madgwick filter:
Madgwick filter;

// sensor's sample rate is fixed at 104 Hz:
const float sensorRate = 104.00;

const float beta = 0.1;

// keep track of time and sample multiple times to get accurate triggering
unsigned long time_now = 0;
const int samplingTime = 200;

// bool set as TRUE if wearer is facing north within the specified cone of operation
bool north = false;

// cone of operation expressed in DEGREES
const float limit = 2.0;

void setup() {
  Serial.begin(9600);
  // attempt to start the IMU:
  if (!IMU.begin()) {
    Serial.println(F("Failed to initialize IMU"));
    // stop here if you can't access the IMU:
    while (true);
  }

  //start the haptic motor object
  drv.begin();
  drv.selectLibrary(1);

  // I2C trigger by sending 'go' command
  drv.setMode(DRV2605_MODE_INTTRIG); // default, internal trigger when sending GO command

  // start the filter to run at the sample rate:
  filter.begin(sensorRate, beta);

}

void loop() {
  time_now = millis();
  while (millis() < time_now + samplingTime) {
    north = getHeading(limit);
  }

  north = getHeading(limit);

  switch (north) {
    case false:
      north = getHeading(limit);
      break;

    case true:
      if (playSingleWaveform(1, 500) == true) {
        north = getHeading(limit);
      }
      break;
  }
}\

bool playSingleWaveform(int effectNumber, int coolDown) {
  unsigned long time_now = 0;
  time_now = millis();

  // set the effect to play
  drv.setWaveform(0, effectNumber);                                         // play effect
  drv.setWaveform(1, 0);                                                    // end waveform

  // play the effect
  drv.go();
  Serial.println("Playing effect");

  while (millis() < time_now + coolDown) {
    // wait for cooldown period
  }
  return true;
}

bool getHeading(float limit) {
  // declarations:
  float xAcc, yAcc, zAcc, xGyro, yGyro, zGyro, xMag, yMag, zMag;

  // values for orientation:
  float heading;

  // check if the IMU is ready to read:
  if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable() && IMU.magneticFieldAvailable()) {

    // read accelerometer, gyroscope and magnetometer:
    IMU.readAcceleration(xAcc, yAcc, zAcc);
    IMU.readGyroscope(xGyro, yGyro, zGyro);
    IMU.readMagneticField(xMag, yMag, zMag);

    // update the filter, to compute orientation:
    filter.update(xGyro, yGyro, zGyro, xAcc, yAcc, zAcc, xMag, yMag, zMag);

    // print the heading
    heading = filter.getYaw();
    Serial.println(heading);

    if (heading >= 0 && heading <= limit) {
      return true;
    }

    if (heading < 0 && heading >= -limit) {
      return true;
    }
    return false;
  }
  return false;
}

I'm having a similar issue as reported by this other post on the forum. The lag does not appear when I'm using the code that I linked before (that doesn't use the magnetometer). I suspect the issue lies in the rate the magnetometer is being sampled at since its rate is fixed at 20Hz, rather than the 104Hz of the accelerometer and gyroscope.

Any thought on what I';m doing wrong? Did I miss something obvious?

[jremington]

The magnetometer on the LSM9DS1 does not use the same coordinate axis orientation as the accel/gyro, and unless this is corrected, neither the Madwick or Mahony filter will work.

This code does work. Separate tilt-compensated compass code is also in that repository.

Also, it is essential that the sensor be calibrated. Usually not required for the accelerometer, but certainly so for the gyro offsets and the magnetometer.