Nano 33 BLE problem with IMU and Madgwick libary

Official Hardware Nano Family Nano 33 BLE
1

Hello,

I have big problem with my project and i have to ask You for help.

I want implement Madgwick libary to my projct on Arduino Nano 33 BLE. I read data from IMU sensor with use ArduinoLSM9DS1 libary.

First time when I used Arduino and IMU I used Euler angles to receive pitch, roll, and yaw. Everytching was working preety well.

Nextly I wanted to use Madgwick libary with quaternions and I receiving really crazy numbers in output.

Maybe someone of You done this before and can give me some advise? Why this dont working properly.

I calibrated manetometer,
Read data from sensor with properly format,
Data have this frequency:
Accelerometer -119H
Gyro -119Hz
Magneto - 20Hz

#include "Arduino.h"
#include "./../lib/Arduino_LSM9DS1/src/Arduino_LSM9DS1.h"
#include "./../lib/ArduinoBLE/src/ArduinoBLE.h"
#include "./../lib/Madgwick/src/MadgwickAHRS.h"
#include "./../lib/MahonyAHRS-master/src/MahonyAHRS.h"
#include "./../lib/MegunoLink/MegunoLink.h"

float accX, accY, accZ, accHZ, gyrHZ, gyrX, gyrY, gyrZ, magX, magY, magZ;
float heading, roll, pitch, yaw, thetaM, phiM;
float thetaFnew, thetaFold =0, phiFnew, phiFold =0;
float thetaG =0, phiG=0, dt, q1, q0, q2, q3;
unsigned long millisOld;

float theta, phi;
float psi, phiRad, thetaRad, Xm, Ym, psiM, psiFold, psiFnew;
unsigned long millisPerReading, microsPrevious;


Madgwick filter;

void setup() {
  Serial.begin(9600);         

  while (!Serial);


  if (!IMU.begin()) {
    while (1);
  }

  filter.begin(119.0);         //Magdgwick

  millisOld = millis();        
  millisPerReading = 0.0085;

}

void loop() {


  

    if (IMU.accelerationAvailable()&& IMU.gyroscopeAvailable()&&IMU.magneticFieldAvailable()) {
      IMU.readAcceleration(accX, accY, accZ);
      IMU.readGyroscope(gyrX, gyrY, gyrZ);
      IMU.readMagneticField(magX, magY, magZ);
      accHZ = IMU.accelerationSampleRate(); 
      gyrHZ = IMU.gyroscopeSampleRate(); 
      
    
 
     if (((millis()-millisOld)/1000) >= microsPerReading) {
       Serial.print((millis()-millisOld)/1000.0);
       Serial.print('\t');

      //Madgwick + offset magnetometr
      //filter.update(gyrX, gyrY, gyrZ, accX, accY, accZ, magX+5, magY-10, magZ);
      filter.updateIMU(gyrX, gyrY, gyrZ, accX, accY, accZ);
      }

    // thetaM = atan2(accX,accZ)/2/3.141592654*360;        //Euler angles
    // phiM = atan2(accY,accZ)/2/3.141592654*360;

    // thetaFnew = 0.9 * thetaFold + 0.1 * thetaM;
    // phiFnew = 0.9 * phiFold + 0.1 * phiM;

  
     //dt = (millis()-millisOld)/1000.0;
      //filter.Rate(119.00);
    
    //  millisOld = millis();

    // theta = (theta + gyrY *dt)*0.95 + thetaM *0.05;
    // phi = (phi - gyrX *dt)*0.95 + phiM *0.05;

    // thetaG = thetaG + gyrY * dt;
    // phiG = phiG - gyrX * dt;


    // phiRad = phi /360*(2*3.141592654);
    // thetaRad = theta /360*(2*3.141592654);

    // Xm=magX*cos(thetaRad)-magY*sin(phiRad)*sin(thetaRad)+magZ*cos(phiRad)*sin(thetaRad);
    // Ym=magY*cos(phiRad)+magZ*sin(phiRad);


    // Ym = magX * sin(phiRad) * sin(thetaRad)+ magY * cos(phiRad) - magZ * cos(thetaRad) * sin(phiRad);
    // Xm = magX * cos(thetaRad) + magZ *sin(thetaRad);
    // psi = atan2(Ym,Xm)/(2*3.141592654)*360;


  
    roll = filter.getRoll();             //Madgwick
    pitch = filter.getPitch();
    yaw = filter.getYaw();
    q0 = filter.getQ0();
    q1 = filter.getQ1();
    q2 = filter.getQ2();
    q3 = filter.getQ3();

 
    Serial.print(q0);
    Serial.print(',');
    Serial.print(q1);
    Serial.print(',');
    Serial.print(q2);
    Serial.print(',');
    Serial.print(q3);
    Serial.print(',');
    Serial.print(roll);
    Serial.print(',');
    Serial.print(pitch);
    Serial.print(',');
    Serial.println(yaw);
    
    millisOld = millis();

}
}
2

Not all libraries are meant for all the different architectures (mcu's).
I notice that one does not seem to be maintained anymore. (4 years)

You may want to revert to the library that did work or seek an alternative that is maintained.

Bob.

3

Honestly I don't think so.
Beacouse in every source which I found everybody talking about Madgwick or Mahony libaries.
I found usage of this libary with LSM9DS1 sensor on kriswiner github repository. Unfourtunetly for me he use there other additional sensors and in code everything is mixed out. Honestly for me is too hard to convert it into usable code with only lines abut LSM9DS1.

But this is only mathematical libary. In libary is Zero operations on registers, only input data and output data.

Maybe You have any other ideas ?
Of course I am really grateful for Your response.

4

Hi, I'm using this library GitHub - aster94/SensorFusion: A simple implementation of some complex Sensor Fusion algorithms that is based on Paul Stoffregen's libraries that the OP references. I have it running on Nano 33 after some effort. Planning to make a tutorial about it since we thought there should be some sensor fusion for Nano 33 that runs out of the box.

Also, SensorFusion library already implements quaternions.

5

mathlete:
Hi, I'm using this library GitHub - aster94/SensorFusion: A simple implementation of some complex Sensor Fusion algorithms that is based on Paul Stoffregen's libraries that the OP references. I have it running on Nano 33 after some effort. Planning to make a tutorial about it since we thought there should be some sensor fusion for Nano 33 that runs out of the box.

Also, SensorFusion library already implements quaternions.

Did you do any calibration for gyro/accelerometer get rid of the drift? Is the orientation stable? Can you share some of that logic?

I can't wait to see your tutorial :smiley:

6

Basically I used MotionCal by Paul Stoffregen to calibrate the magnetometer. I have not been able to verify the results other than to see that uTeslas are reported in the expected range for position on surface of planet earth.

Below is my simple code to calibrate the gyro. Even though I'm using SensorFusion.h, available in Arduino -> Manage Libraries, you can see it's not actually needed for the gyro call in case you are using a different IMU sensor fusion (Mahony/Madgwick/etc) library.

// Simple example of getting cooked orientation from IMU
// Uses commmon sensor fusion algorithms
// JS adapted from SensorFusion Example
// https://github.com/aster94/SensorFusion
// Probably uses the MahonyAHRS library under the hood
// https://github.com/PaulStoffregen/MahonyAHRS/blob/master/examples/PropShield/PropShield.ino

// IMPORTANT
// this is not a working example, this is just to show how to set the library
// if you need a working example please see the other example

#include <Arduino_LSM9DS1.h> . // this is for IMU on BLE 33

#include "SensorFusion.h" //SF
SF fusion;

float gx, gy, gz, ax, ay, az, mx, my, mz;
float pitch, roll, yaw;
float deltat;

int calibrationLength = 1024;
int calCount = 0;
//float savedAccel[calibrationLength][3];
//float savedGyro[calibrationLength][3];
float sumAccel[3];
float sumGyro[3];

void setup() {

  Serial.begin(9600); //serial to display data
  // your IMU begin code goes here

  Serial.println("Started");

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }

}

void loop() {

  // now you should read the gyroscope, accelerometer (and magnetometer if you have it also)
  // NOTE: the gyroscope data have to be in radians
  // if you have them in degree convert them with: DEG_TO_RAD example: gx * DEG_TO_RAD

  if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable()) {
    IMU.readAcceleration(ax, ay, az);
    IMU.readGyroscope(gx, gy, gz);
    IMU.readMagneticField(mx, my, mz);
    float gyroScale = 0.097656f / 4.5;
    gx = gx * gyroScale;
    gy = gy * gyroScale;
    gz = gz * gyroScale;

    sumAccel[1] += ax;
    sumAccel[2] += ay;
    sumAccel[3] += az;


    sumGyro[1] += gx;
    sumGyro[2] += gy;
    sumGyro[3] += gz;

    calCount += 1;
    
  }

  if (calCount >= calibrationLength) {
    Serial.println("Accelerometer Mean Error:");
    Serial.print("axE: ");
    Serial.print(sumAccel[1] / calibrationLength,4);
    Serial.print(" ayE: ");
    Serial.print(sumAccel[2] / calibrationLength,4);
    Serial.print(" azE: ");
    Serial.println(sumAccel[3] / calibrationLength,4);
    Serial.println();
    
    Serial.println("Gyroscope Mean Error:");
    Serial.print("gxE: ");
    Serial.print(sumGyro[1] / calibrationLength,4);
    Serial.print(" gyE: ");
    Serial.print(sumGyro[2] / calibrationLength,4);
    Serial.print(" gzE: ");
    Serial.println(sumGyro[3] / calibrationLength,4);
    Serial.println();
    
    sumAccel[1] =0;
    sumAccel[2] =0;
    sumAccel[3] =0;

    sumGyro[1] =0;
    sumGyro[2] =0;
    sumGyro[3] =0;
    
    calCount = 0;
  }

  int myArray[10];
  unsigned int index;
  long arraySum;

  arraySum = 0;
  for (index = 0; index < sizeof(myArray) / sizeof( myArray[0]); index++)
  {
    arraySum += myArray[index];
  }


  deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
  //choose only one of these two:
  fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltat);  //mahony is suggested if there isn't the mag and the mcu is slow
  //fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat);  //else use the magwick, it is slower but more accurate

  pitch = fusion.getPitch();
  roll = fusion.getRoll();    //you could also use getRollRadians() etc
  yaw = fusion.getYaw();



  //orientation for processing (only yaw and roll inverted):
  yaw = -yaw;
  //pitch = -pitch;
  //roll = -roll;

}
7

mathlete:
Basically I used MotionCal by Paul Stoffregen to calibrate the magnetometer. I have not been able to verify the results other than to see that uTeslas are reported in the expected range for position on surface of planet earth.

Below is my simple code to calibrate the gyro. Even though I'm using SensorFusion.h, available in Arduino -> Manage Libraries, you can see it's not actually needed for the gyro call in case you are using a different IMU sensor fusion (Mahony/Madgwick/etc) library.

// Simple example of getting cooked orientation from IMU

// Uses commmon sensor fusion algorithms
// JS adapted from SensorFusion Example
// GitHub - aster94/SensorFusion: A simple implementation of some complex Sensor Fusion algorithms
// Probably uses the MahonyAHRS library under the hood
// MahonyAHRS/examples/PropShield/PropShield.ino at master · PaulStoffregen/MahonyAHRS · GitHub

// IMPORTANT
// this is not a working example, this is just to show how to set the library
// if you need a working example please see the other example

#include <Arduino_LSM9DS1.h> . // this is for IMU on BLE 33

#include "SensorFusion.h" //SF
SF fusion;

float gx, gy, gz, ax, ay, az, mx, my, mz;
float pitch, roll, yaw;
float deltat;

int calibrationLength = 1024;
int calCount = 0;
//float savedAccel[calibrationLength][3];
//float savedGyro[calibrationLength][3];
float sumAccel[3];
float sumGyro[3];

void setup() {

Serial.begin(9600); //serial to display data
 // your IMU begin code goes here

Serial.println("Started");

if (!IMU.begin()) {
   Serial.println("Failed to initialize IMU!");
   while (1);
 }

}

void loop() {

// now you should read the gyroscope, accelerometer (and magnetometer if you have it also)
 // NOTE: the gyroscope data have to be in radians
 // if you have them in degree convert them with: DEG_TO_RAD example: gx * DEG_TO_RAD

if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable()) {
   IMU.readAcceleration(ax, ay, az);
   IMU.readGyroscope(gx, gy, gz);
   IMU.readMagneticField(mx, my, mz);
   float gyroScale = 0.097656f / 4.5;
   gx = gx * gyroScale;
   gy = gy * gyroScale;
   gz = gz * gyroScale;

sumAccel[1] += ax;
   sumAccel[2] += ay;
   sumAccel[3] += az;

sumGyro[1] += gx;
   sumGyro[2] += gy;
   sumGyro[3] += gz;

calCount += 1;
   
 }

if (calCount >= calibrationLength) {
   Serial.println("Accelerometer Mean Error:");
   Serial.print("axE: ");
   Serial.print(sumAccel[1] / calibrationLength,4);
   Serial.print(" ayE: ");
   Serial.print(sumAccel[2] / calibrationLength,4);
   Serial.print(" azE: ");
   Serial.println(sumAccel[3] / calibrationLength,4);
   Serial.println();
   
   Serial.println("Gyroscope Mean Error:");
   Serial.print("gxE: ");
   Serial.print(sumGyro[1] / calibrationLength,4);
   Serial.print(" gyE: ");
   Serial.print(sumGyro[2] / calibrationLength,4);
   Serial.print(" gzE: ");
   Serial.println(sumGyro[3] / calibrationLength,4);
   Serial.println();
   
   sumAccel[1] =0;
   sumAccel[2] =0;
   sumAccel[3] =0;

sumGyro[1] =0;
   sumGyro[2] =0;
   sumGyro[3] =0;
   
   calCount = 0;
 }

int myArray[10];
 unsigned int index;
 long arraySum;

arraySum = 0;
 for (index = 0; index < sizeof(myArray) / sizeof( myArray[0]); index++)
 {
   arraySum += myArray[index];
 }

deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
 //choose only one of these two:
 fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltat);  //mahony is suggested if there isn't the mag and the mcu is slow
 //fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat);  //else use the magwick, it is slower but more accurate

pitch = fusion.getPitch();
 roll = fusion.getRoll();    //you could also use getRollRadians() etc
 yaw = fusion.getYaw();

//orientation for processing (only yaw and roll inverted):
 yaw = -yaw;
 //pitch = -pitch;
 //roll = -roll;

}

Mathlete, have you tried the calibration sketches available in the LSM9DS1 V2 library by Femme Verbeek? I'm attempting to use this library along with the same SensorFusion library and I'm generating values that don't make much sense and drift considerably (especially with yaw). But, when using the example XY_compass in the LSM9DS1 V2 library the heading value is correct. So I'm assuming that my magnetometer calibration is good but somehow I'm messing up the sensor fusion part. This is my first go at working with an IMU so I'm probably making a dumb mistake somewhere.

My attempt:

#include <Arduino_LSM9DS1.h>
#include "SensorFusion.h" //SF

SF fusion;
float igx, igy, igz, iax, iay, iaz, imx, imy, imz;
float gx, gy, gz, ax, ay, az, mx, my, mz;
float pitch, roll, yaw;
float deltat;


void setup() {
  
  Serial.begin(9600);
  Serial.println("Started");
  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }

  IMU.setAccelFS(3);
  IMU.setAccelODR(4);
  IMU.setAccelOffset(0.008380, -0.053174, 0.000583);
  IMU.setAccelSlope (0.994386, 0.995541, 1.006324);


  IMU.setGyroFS(3);
  IMU.setGyroODR(4);
  IMU.setGyroOffset (1.057922, -1.028564, -0.349609);
  IMU.setGyroSlope (1.219327, 2.274642, 1.185046);

  
  IMU.setMagnetODR(8);  //norm 8
  IMU.setMagnetOffset(-9.563599, 20.178833, 25.125732);
  IMU.setMagnetSlope (1.229515, 1.205809, 1.250000);

}


void loop() {
  // NOTE: the gyroscope data have to be in radians

  if (IMU.accelerationAvailable()) {
    IMU.readAcceleration(iax, iay, iaz);
    ax = iax;
    ay = iay;
    az = iaz;
  }

  if (IMU.gyroscopeAvailable()) {
    IMU.readGyroscope(igx, igy, igz);       // if you have them in degree convert them with: DEG_TO_RAD example: gx * DEG_TO_RAD
    gx = (igx * (3.14159 / 180));
    gy = (igy * (3.14159 / 180));
    gz = (igz * (3.14159 / 180));
  }

  if (IMU.magneticFieldAvailable()) {
    IMU.readMagneticField(imx, imy, imz);
    mx = imx;
    my = imy;
    mz = imz;
  }

  deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update

  //fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltat);  //mahony is suggested if there isn't the mag and the mcu is slow
  fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat);  //else use the magwick, it is slower but more accurate

  pitch = fusion.getPitch();
  roll = fusion.getRoll();
  yaw = fusion.getYaw();

  Serial.print("pitch = ");
  Serial.print(pitch);
  Serial.print(" roll = ");
  Serial.print(roll);
  Serial.print(" yaw = ");
  Serial.println(yaw);


}
8

Hi jhoski

Looking at your calibration data, there is something wrong with the Y Slope calibration of the sensor. A value of 2.274642 is way to much. The expected value is somewhere around 1 close to the values of the other axes.
Just run the DIY_Calibration_Gyroscope again.

To see if the calibration makes sense, you can run the RPM_Meter_Rev_Counter program. Columns 4, 5 and 6 produce the integrated gyro signal. If you keep the sensor still, these values should remain unchanged.

If you put
IMU.gyroUnit= RADIANSPERSECOND;
in your code, you don't have to do the conversion yourself.

I did not try SensorFusion.
I got the Madgwick working with the MadgwickAHRS library
https://forum.arduino.cc/index.php?topic=663160.msg4676767#msg4676767

9

Hi all,

I was able to use this filter and I appreciate all the work you all did! I am now trying to take the roll, pitch, and yaw data and read it in python to control keyboard arrow functions so I can make the IMU into a keyboard game controller to play neverball which is a ball balance game.

I am stuck with the code on python trying to take the serial data and make it control the arrow functions? Does anyone know how to make the data control keyboard functions on the arduino code or on python using the nano 33?

Python Code so far that just prints the data and doesn't perform pressing the arrow keys:

from pynput.keyboard import Key, Controller
from time import *
import numpy as np
import math
import serial
ad=serial.Serial('com11',115200)
sleep(1)

keyboard = Controller()
toRad=2*np.pi/360
toDeg=1/toRad

while (True):
while (ad.inWaiting()==0):
pass
dataPacket=ad.readline()
dataPacket=str(dataPacket,'utf-8')
splitPacket=dataPacket.split(",")
roll=float(splitPacket[0])*toRad
pitch=float(splitPacket[1])*toRad
yaw=float(splitPacket[2])*toRad+np.pi

if pitch <= -5:
print('Down')
break
if pitch >= 5:
print('Up')
break

print("Pitch=",pitchtoDeg," Roll=",rolltoDeg,"Yaw=",yaw*toDeg)

10

Hey mathlete, do you have a sketch to get the data into the format for MotionCal or is there already sth out there? Also trying to get my Nano 33 BLE calibrated.

11

Hi,
did anyone have success after calibrating their imu using femmeverbeek's calibration codes and using sensor fusion algorithm for angle detection? I read somewhere that madgwick is faulty and it's not working for me either.

12

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.