Problems using library with Arduino NANO BLE Sense

[redflowIT]

Hi Aster94
I'm using library with an Arduino BLE Sense with embedded Gyroscope and an external micro display. It seems it needs seconds to stabilize values, should I use an external IMU or what I'm doing wrong?

`#include <Arduino.h>
#include <Arduino_LSM9DS1.h> // Sensoristica
#include <U8g2lib.h> // Display
#include <SensorFusion.h> // Calcolo

// Inizializzazione DISPLAY
U8G2_LD7032_60X32_F_4W_SW_SPI u8g2(U8G2_MIRROR, /* clock=/ 9, / data=/ 8, / cs=/ 11, / dc=/ 10, / reset=/ 12); // PROTOTIPO SALDATO SU BASETTA
//U8G2_LD7032_60X32_F_4W_SW_SPI u8g2(U8G2_MIRROR, / clock=/ 11, / data=/ 12, / cs=/ 9, / dc=/ 10, / reset=*/ 8);

const int samplingCicleBeforeRefresh = 30;
SF fusion;

float xAcc, yAcc, zAcc;
float xGyro, yGyro, zGyro;
float xMag, yMag, zMag;
float deltat;

int roll, pitch, yaw;

void setup() {
Serial.begin(115200);
//while (!Serial);
Serial.println("Started");

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

u8g2.begin();
deltat=fusion.deltatUpdate();
// Abbasso il PIN per poter attivare il display (aggancio la massa)
pinMode(A0, OUTPUT);
digitalWrite(A0, LOW);
}

void refreshDisplay(int yaw, int pitch, int roll)
{
char buff[16];

u8g2.clearBuffer();
u8g2.setFont(u8g2_font_artosserif8_8u);
snprintf (buff, sizeof(buff), "R%d ", yaw);
u8g2.drawStr(10,10,buff);
Serial.print(buff);
snprintf (buff, sizeof(buff), "F%d ", pitch);
u8g2.drawStr(10,19,buff);
Serial.print(buff);
snprintf (buff, sizeof(buff), "I%d ", roll);
u8g2.drawStr(10,28,buff);
Serial.println(buff);
u8g2.sendBuffer();
}

void loop() {
// read accelerometer & gyrometer:
IMU.readAcceleration(xAcc, yAcc, zAcc);
IMU.readGyroscope(xGyro, yGyro, zGyro);
IMU.readMagneticField(xMag, yMag, zMag);

deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
//choose only one of these two:
//fusion.MahonyUpdate(xGyro, yGyro, zGyro, xAcc, yAcc, zAcc, deltat); //mahony is suggested if there isn't the mag and the mcu is slow
fusion.MadgwickUpdate(xGyro * DEG_TO_RAD, yGyro * DEG_TO_RAD, zGyro * DEG_TO_RAD, xAcc, yAcc, zAcc, xMag, yMag, zMag, deltat); //else use the magwick, it is slower but more accurate

// print the heading, pitch and roll
roll = round(fusion.getRoll());//round(fusion.getRoll()-90);
pitch = round(fusion.getPitch());
yaw = round(fusion.getYaw());//round(180-fusion.getYaw());

//Serial.print("Y ");
//Serial.print(yaw);
//Serial.print(" P ");
//Serial.print(pitch);
//Serial.print(" R ");
//Serial.println(roll);

refreshDisplay (yaw, pitch, roll);
}`

Thank you
Regards

[aster94]

It seems it needs seconds to stabilize values

then it works?

[redflowIT]

The values seeems to 'stabilize' but then they gain a step.
For example starting in a position I have an angle X, moving and returning to same position I'm having the angle X (about X) for a while than I get X + Y. The situation will go on for a while (not every time the same period) and than I get X + Z....and so on.
I'm tring to get always same values.....and I don't understand if the system is 'loosing' time refreshing display or what

I've tried also don't using mag values with
fusion.MadgwickUpdate(xGyro * DEG_TO_RAD, yGyro * DEG_TO_RAD, zGyro * DEG_TO_RAD, xAcc, yAcc, zAcc, deltat); and the effect is the same (the period seems a little longer).

The integration you're calculating could be the cause of this angle difference?

[jonasBoss]

I am working with the NANO BLE Sense as well, this is my experience:
The first orientation is always wrong. Over a period of 5-15s the orientation slowly moves to the correct values and seems to be very accurate.

@redflowIT I was wondering about those "steps" as well. The gyroscope is usually set to 245 deg/s it is rather easy to move the Arduino faster than that. In those instances the raw gyro values are at the limit and the orientation destabilizes.

This is my code:

#include <Wire.h>
#include <SPI.h>
#include <SparkFunLSM9DS1.h>
#include "SensorFusion.h"

#define GRAVITY 9.81
#define PRINT_INTERVAL 12
unsigned long lastPrint = 0; // Keep track of print time



LSM9DS1 imu;
SF filter;

float Axyz[3], Mxyz[3], Gxyz[3];
float pitch, roll, yaw;
float deltat;


void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.begin(115200);
  while (!Serial){}

  Wire1.begin();
  delay(100);
  if (imu.begin(0x6B, 0x1E, Wire1)==false)
  {
    while (1)
    {
      digitalWrite(LED_BUILTIN, HIGH);
      delay(10);
      digitalWrite(LED_BUILTIN, LOW);
      delay(20);
    }
  }
  for (int i = 0; i < 5; i++)
  {
      digitalWrite(LED_BUILTIN, HIGH);
      delay(100);
      digitalWrite(LED_BUILTIN, LOW);
      delay(50);
  }
}

void loop() {
  if (imu.accelAvailable() && imu.magAvailable() && imu.gyroAvailable())
  {
    imu.readAccel();
    imu.readMag();
    imu.readGyro();
    
    get_IMU(Axyz, Mxyz);
    Gxyz[0]= imu.calcGyro(imu.gx) * DEG_TO_RAD;
    Gxyz[1]= imu.calcGyro(imu.gy) * DEG_TO_RAD;
    Gxyz[2]= imu.calcGyro(imu.gz) * DEG_TO_RAD;

    Axyz[0] = imu.calcAccel(Axyz[0]) * GRAVITY;
    Axyz[1] = imu.calcAccel(Axyz[1]) * GRAVITY;
    Axyz[2] = imu.calcAccel(Axyz[2]) * GRAVITY;

    Mxyz[0] = imu.calcMag(Mxyz[0]);
    Mxyz[1] = imu.calcMag(Mxyz[1]);
    Mxyz[2] = imu.calcMag(Mxyz[2]);

    deltat = filter.deltatUpdate();
    filter.MadgwickUpdate( -Gxyz[0], Gxyz[1], Gxyz[2],  // Flip Gyro Handedness
                           -Axyz[0], Axyz[1], Axyz[2],  // Flip Accel Handedness
                            Mxyz[0], Mxyz[1], Mxyz[2], deltat);

    if (millis() - lastPrint >= PRINT_INTERVAL)
    {
      lastPrint = millis();
      /*
      Serial.print("A: ");
      Serial.print(-Axyz[0]);
      Serial.print(", ");
      Serial.print(Axyz[1]);
      Serial.print(", ");
      Serial.println(Axyz[2]);
      
      Serial.print("G: ");
      Serial.print(-Gxyz[0]);
      Serial.print(", ");
      Serial.print(Gxyz[1]);
      Serial.print(", ");
      Serial.println(Gxyz[2]);
      
      Serial.print("M: ");
      Serial.print(Mxyz[0]);
      Serial.print(", ");
      Serial.print(Mxyz[1]);
      Serial.print(", ");
      Serial.println(Mxyz[2]);      
      */
      
      Serial.print("Orientation: ");
      Serial.print(filter.getYaw());
      Serial.print(", ");
      Serial.print(filter.getPitch());
      Serial.print(", ");
      Serial.println(filter.getRoll()); 

      float * q;
      q = filter.getQuat();
      Serial.print("Quaternion: ");
      Serial.print(q[0], 4);
      Serial.print(", ");
      Serial.print(q[1], 4);
      Serial.print(", ");
      Serial.print(q[2], 4);
      Serial.print(", ");
      Serial.println(q[3], 4);
    }
  }
}


//Accel scale 16457.0 to normalize
float A_B[3]
{ -654.19,  161.87, -597.01};

float A_Ainv[3][3]
{{  1.00475, -0.03135, -0.00073},
{ -0.03135,  1.00864,  0.00819},
{ -0.00073,  0.00819,  0.99144}};

//Mag scale 3746.0 to normalize
float M_B[3]
{ 1326.39, 1526.05, 1503.85};

float M_Ainv[3][3]
{{  1.16966,  0.03568, -0.00257},
{  0.03568,  1.19523, -0.02172},
{ -0.00257, -0.02172,  1.15745}};

void get_IMU(float Axyz[3], float Mxyz[3]) {
  byte i;
  float temp[3];
    Axyz[0] = imu.ax;
    Axyz[1] = imu.ay;
    Axyz[2] = imu.az;
    Mxyz[0] = imu.mx;
    Mxyz[1] = imu.my;
    Mxyz[2] = imu.mz;

  
  //apply offsets (bias) and scale factors from Magneto
  for (i = 0; i < 3; i++) temp[i] = (Axyz[i] - A_B[i]);
  Axyz[0] = A_Ainv[0][0] * temp[0] + A_Ainv[0][1] * temp[1] + A_Ainv[0][2] * temp[2];
  Axyz[1] = A_Ainv[1][0] * temp[0] + A_Ainv[1][1] * temp[1] + A_Ainv[1][2] * temp[2];
  Axyz[2] = A_Ainv[2][0] * temp[0] + A_Ainv[2][1] * temp[1] + A_Ainv[2][2] * temp[2];

  //apply offsets (bias) and scale factors from Magneto
  for (i = 0; i < 3; i++) temp[i] = (Mxyz[i] - M_B[i]);
  Mxyz[0] = M_Ainv[0][0] * temp[0] + M_Ainv[0][1] * temp[1] + M_Ainv[0][2] * temp[2];
  Mxyz[1] = M_Ainv[1][0] * temp[0] + M_Ainv[1][1] * temp[1] + M_Ainv[1][2] * temp[2];
  Mxyz[2] = M_Ainv[2][0] * temp[0] + M_Ainv[2][1] * temp[1] + M_Ainv[2][2] * temp[2]; 
}

I use the https://github.com/sparkfun/SparkFun_LSM9DS1_Arduino_Library library, make sure to have this sparkfun/SparkFun_LSM9DS1_Arduino_Library#37 PR.

The calibration was done with this https://github.com/jremington/LSM9DS1-AHRS library

[LazaroFilm]

Any solution to this? It feels like the sensors are not at the same scale. The gyro will make the plot line jump to a position, then it will slowly move to another one dictated by the accelerometer. For IMU with the Nano 33 I'm using FemmeVerbeek's code and have calibrated my gyro and accell, still the same issue. Is it a measuring unit issue?

[jonasBoss]

@LazaroFilm did you try the code in this #7 (comment) ? It works well, except that the gyro is not calibrated, but that is an easy fix. We can just gather a few values while holding the arduino still, an apply them in the get_IMU() function.

Also note that the LSM9DS1 gyro and accelerometer use a left handed coordinate system, while the Magnetometer uses a right handed one. Aside form the effect I mentioned in the comment above this will also introduce the jumps in the orientation.

Flipping the Gyro and Acc X-Axis will fix that and align all the coordinate systems:

filter.MadgwickUpdate( -Gxyz[0], Gxyz[1], Gxyz[2],  // Flip Gyro Handedness
                       -Axyz[0], Axyz[1], Axyz[2],  // Flip Accel Handedness
                        Mxyz[0], Mxyz[1], Mxyz[2], deltat);

[aster94]

Nice @jonasBoss !

I am curious too, @LazaroFilm have you tried it??

[Dominik-Schoen]

I used the code from @jonasBoss and visualized it, since I like to see things move, rather than raw numbers.

https://www.youtube.com/watch?v=McNhcrJgBuQ

You can see a slight drift at the beginning, which fades away after a few seconds. This was done without any calibration and it delivers solid data after the initial drifting.

[norahb]

Hi,

I used @jonasBoss code.. Do I need to use Wire1??
I am using the board alone (with built-in sensors) without connecting any external sensors.
When I commented Wire1.begin() , it worked but it does not show anything in the serial monitor.
What might cause the error ?
@Dominik-Schoen Did you use the same code with the nano alone? Can you please share your code to visualise it ?
Thanks

[jonasBoss]

@norahb
Yeah all the internal sensors are wired up to SDA1 and SDL1 Which is Wire1 in the ArduinoCore-mbed:

#if WIRE_HOWMANY > 0
arduino::MbedI2C Wire(I2C_SDA, I2C_SCL);
#endif
#if WIRE_HOWMANY > 1
arduino::MbedI2C Wire1(I2C_SDA1, I2C_SCL1);
#endif

Do you use the Arduino style setup() and loop() functions, or do your write your own main() function? It should work if you use setup() and loop() because the Arduino library provides a main() function which does some hardware specific stuff.

If you write your own main() function you need to also initialize the hardware properly. Have a look at cores/arduino/main.cpp for that

[norahb]

@jonasBoss I used setup() loop() style,,
It is working Now thanks..