Not getting output from FusionAhrsUpdate after Updating to latest version.

[mjs513]

I just updated to the latest version of the Fusion and now not seeing any results, i.e.

    flags = FusionAhrsGetFlags(&ahrs);
    euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
    q = FusionAhrsGetQuaternion(&ahrs);
    fHeading = FusionCompassCalculateHeading(FusionConventionNed, accelerometer, magnetometer);

output is show all zero's.

I am using a MPU-9250 with accel/gryo axis aligned with the magnetometer axis.

Algorithm is initialized as follows:

    // Initialise algorithms
//put at begining of sketch

    FusionOffsetInitialise(&offset, SAMPLE_RATE);
    FusionAhrsInitialise(&ahrs);

  // Set AHRS algorithm settings
  const FusionAhrsSettings settings = {
          .convention = FusionConventionNed,
          .gain = 0.5f, //1.5f,
          .gyroscopeRange = 2000.0f, /* replace this with actual gyroscope range in degrees/s */
          .accelerationRejection = 10.0f,
          .magneticRejection = 10.0f, //0.0f,
          .recoveryTriggerPeriod  = (uint8_t)(5.0f / SAMPLE_RATE ), /* 5 seconds */
  };
  FusionAhrsSetSettings(&ahrs, &settings);

Then in my fusion loop:

void getFusion() {
  getCalIMU();

  dt = (micros() - t_old) * 0.000001;
  t_old = micros();

    //Lets normalize magnetometer data
    //float mag_norm = sqrt(val[6]*val[6] + val[7]*val[7] + val[8]*val[8]);

    // Acquire latest sensor data
    FusionVector gyroscope = { val[3], val[4], val[5] }; // replace this with actual gyroscope data in degrees/s
    FusionVector accelerometer = { val[0], val[1], val[2] }; // replace this with actual accelerometer data in g
    FusionVector magnetometer = {  val[6], val[7], val[8] }; // replace this with actual magnetometer data in arbitrary units

    // Update gyroscope offset correction algorithm
    gyroscope = FusionOffsetUpdate(&offset, gyroscope);

    // Calculate delta time (in seconds) to account for gyroscope sample clock error
    FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, dt);
    //FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, dt);

    flags = FusionAhrsGetFlags(&ahrs);
    euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
    q = FusionAhrsGetQuaternion(&ahrs);
    fHeading = FusionCompassCalculateHeading(FusionConventionNed, accelerometer, magnetometer);

    //if(dump > 100) {
      if(raw_values_on == 1) {
        print_float_array(val, 9);
        Serial.println();
      }

      if(serial_values_on == 1) {
        Serial.printf("Fusion (RPY): %0.1f, %0.1f, %0.1f\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
      }

      if(x_values_on == 1) {
        timestamp = micros();
        char accelgyroBuffer[100];
        sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f,%0.6f,%0.6f,%0.6f", 'I', timestamp, val[3], val[4], val[5], val[0], val[1], val[2]);
        Serial.printf ("%s\n",accelgyroBuffer);
        sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f", 'M', timestamp, val[6], val[7], val[8]);
        Serial.printf ("%s\n",accelgyroBuffer);
        sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f,%0.6f", 'Q', timestamp, q.element.w, q.element.x, q.element.y, q.element.z);
        Serial.printf ("%s\n",accelgyroBuffer);
      }

      if(telem_values_on == 1) telemetryPortOut();
      //dump = 0;

}

Output from the MPU-9250 looks good:

When I tried with the old version it did produce results.

Any help would be appreciated.

[mjs513]

Never mind think I know the issue. Going to close this while I debug some more

Nope what I thought it was - wasn't

[mjs513]

Found one issue that I fixed but now data is kind of returning. Bytthat I mean 'q' values and euler angles are NANs. Flags for Initialization is 1 and timeouts are 0.

first 9 columns are the calibrated data (gyro, accel and magn), Next are the euler angles (RPY) and next 4 are quaternions and then the flags.

-1.0249195099, 3.9063048363, -1.2204788923,0.1223300993,0.0566682965,-0.9995157123,-0.3305439353,-0.4308943152,0.7984790802,-NAN       ,NAN       ,NAN       ,120.6902313232,NAN       ,NAN       ,-NAN       ,-NAN       ,  1.000000,  0.000000,  0.000000-0.9638834000, 3.2349078655, -0.5490819216,0.1019417495,0.0659501702,-0.9912832975,-0.3221757412,-0.3658536673,0.8136882186,-NAN       ,NAN       ,NAN       ,127.0753326416,-NAN       ,NAN       ,-NAN       ,NAN       ,  1.000000,  0.000000,  0.000000
-0.8418111801, 3.1128358841, -0.4270097613,0.0956310704,0.0693698078,-0.9859564304,-0.3221757412,-0.3658536673,0.8136882186,-NAN       ,NAN       ,NAN       ,127.9319076538,NAN       ,NAN       ,-NAN       ,-NAN       ,  1.000000,  0.000000,  0.000000
-0.8418111801, 3.1128358841, -0.4270097613,0.0956310704,0.0693698078,-0.9859564304,-0.3221757412,-0.3658536673,0.8136882186,NAN       ,NAN       ,NAN       ,127.9319076538,NAN       ,-NAN       ,-NAN       ,NAN       ,  1.000000,  0.000000,  0.000000
-0.8418111801, 3.1128358841, -0.4270097613,0.0956310704,0.0693698078,-0.9859564304,-0.3221757412,-0.3658536673,0.8136882186,-NAN       ,-NAN       ,-NAN       ,127.9319076538,NAN       ,NAN       ,NAN       ,NAN       ,  1.000000,  0.000000,  0.000000
-0.8418111801, 3.1128358841, -0.4270097613,0.0956310704,0.0693698078,-0.9859564304,-0.3221757412,-0.3658536673,0.8136882186,-NAN       ,NAN       ,NAN       ,127.9319076538,-NAN       ,NAN       ,-NAN       ,NAN       ,  1.000000,  0.000000,  0.000000

[xioTechnologies]

I believe that "latest version" refers to v1.2.7. Please confirm which version "old version" refers to.

Some comments on your code:

• (uint8_t)(5.0f / SAMPLE_RATE) will resolve to 0 for all values of SAMPLE_RATE greater than 5. Your comment suggests that you intend recoveryTriggerPeriod to be 5 seconds. For this, the expression should be recoveryTriggerPeriod = 5 * (unsigned int) SAMPLE_RATE.
• You have not shared variable declarations so that the type is unclear in many cases. Expressions such as dt = (micros() - t_old) * 0.000001; suggest that you are not using types appropriately.
• Your calculation of dt incorporates unnecessary errors because you call micros() twice. You should only call micros() per loop.

[mjs513]

Morning from NY.

I believe that "latest version" refers to v1.2.7. Please confirm which version "old version" refers to.

Actually I downloaded the zip file directly from the repo using "CODE" option.

Please confirm which version "old version" refers to. Over taken by events - tried to go back to a version I had from April of 2023. Going to try it again later today.

For this, the expression should be recoveryTriggerPeriod = 5 * (unsigned int) SAMPLE_RATE. Currently I am running at 100hz with SAMPLE_RATE defined as 0.01 seconds. Since the def for recoveryTriggerPeriod was that sample rate should be in Hz I did the diversion.

You have not shared variable declarations so that the type is unclear in many cases. Expressions such as dt = (micros() - t_old) * 0.000001; suggest that you are not using types appropriately Your calculation of dt incorporates unnecessary errors because you call micros() twice. You should only call micros() per loop. Yes you are right - caught that late last night. Changed the way I did it this morning so dt is now coming out close to the sample rate.

Even after that fix still getting nans. Here is the whole code: Main:

#include "mpu9250.h"
#include "Fusion.h"
#include <stdbool.h>
#include <stdio.h>

#include <Wire.h>
#include "Streaming.h"
#include <string>

#include "constants.h" 

/* Mpu9250 object */
bfs::Mpu9250 imu;

void gyroCalibration();

void telemetryPortOut();
void print_float_array(float *arr, int size);

bool dataRdy;

//Freeimu calibration
int cal_acc_off[] = {-37, 30, 130};
int cal_acc_scale[]  = {2047, 2060, 2065};
int cal_magn_off[]  = {99, 16, -186};
int cal_magn_scale[]  = {239, 246, 263};

// scale factors
float accelScale, gyroScale;
float magScale[3];

FusionOffset offset;
FusionAhrs ahrs;
FusionQuaternion q;
FusionEuler euler;
FusionAhrsFlags flags;

//new data available
volatile int newIMUData;
uint32_t lastUpdate, now1;

void setup() {
  /* Serial to display data */
  while(!Serial && millis() < 5000) {}
  Serial.begin(115200);

  // If Teensy 4.x fails print Crashreport to serial monitor
  if (CrashReport) {
      Serial.print(CrashReport);
      Serial.println("Press any key to continue");
      while (Serial.read() != -1) {
      }
      while (Serial.read() == -1) {
      }
      while (Serial.read() != -1) {
      }
  }

  /* Start the I2C bus */
  Wire2.begin();
  Wire2.setClock(400000);
  /* I2C bus,  0x68 address */
  imu.Config(&Wire2, bfs::Mpu9250::I2C_ADDR_PRIM);
  /* Initialize and configure IMU */
  if (!imu.Begin()) {
    Serial.println("Error initializing communication with IMU");
    while(1) {}
  }

  cout.println("IMU Connected!");

  /* Set the sample rate divider */
  // rate = 1000 / (srd + 1)
  // = 1000/20 = 50 hz
  // = 100 hz
  if (!imu.ConfigSrd(9)) {
    Serial.println("Error configured SRD");
    while(1) {}
  }
  SAMPLE_RATE = 0.01f;

  //Get MPU sensitivity values
  imu.getScales(&accelScale, &gyroScale, magScale);

    FusionOffsetInitialise(&offset, SAMPLE_RATE);
    FusionAhrsInitialise(&ahrs);

  // Set AHRS algorithm settings
  const FusionAhrsSettings settings = {
          .convention = FusionConventionNed,
          .gain = 0.5f, //1.5f,
          .gyroscopeRange = 2000.0f, // replace this with actual gyroscope range in degrees/s
          .accelerationRejection = 5.0f,
          .magneticRejection = 5.0f, //0.0f,
          .recoveryTriggerPeriod  = (uint8_t)(5.0f / SAMPLE_RATE ), // 5 seconds
  };

  FusionAhrsSetSettings(&ahrs, &settings);

  gyroCalibration();

  cout.println("Ready for commands....");
}

void loop() {
    if ( cout.available() ) {
      char rr;
      rr = cout.read();
      switch (rr) {
        case 'g':
          rr = '0';
          gyroCalibration();
          break;
        case 'r':
        {
          rr ='0';
          if(raw_values_on == 1) {
            raw_values_on = 0;
          } else if(raw_values_on == 0) {
            raw_values_on = 1;
          }
        }
          break;
        case 's':
        {
          rr ='0';
          if(serial_values_on == 1) {
            serial_values_on = 0;
          } else if(serial_values_on == 0) {
            serial_values_on = 1;
          }
        }
          break;
        case 'x':
        {
          rr ='0';
          if(x_values_on == 1) {
            x_values_on = 0;
          } else if(serial_values_on == 0) {
            x_values_on = 1;
          }
        }
            break;
        case 't':
        {
          rr ='0';
          if(telem_values_on == 1) {
            telem_values_on = 0;
          } else if(telem_values_on == 0) {
            telem_values_on = 1;
          }
        }
            break;
        case 'f':
        {
          rr = '0';
          if(fusion_on == 1) {
            fusion_on = 0;
          } else if(fusion_on == 0) {
            fusion_on = 1;
          }
        }
        case '\r':
        case '\n':
        case 'h': menu(); break;
      }
      while (cout.read() != -1) ; // remove rest of characters.
    }

  if(fusion_on == 1) {
    getFusion();
  }
}

void getCalIMU() {
  // read the sensor
  /* Check if data read */
  //NOTE for Fusion gyro data rate is the driver not the magnetomer
  //if (imu.Read_raw(raw_values) & imu.new_imu_data_()) {
  if (imu.Read_raw(raw_val)) {
    newIMUData = imu.new_imu_data();
    now1= micros(); // This is when the data reported READY
    val[4] = ((float)(raw_val[3] * gyroScale) - gyrox_off);
    val[3] = ((float)(raw_val[4] * gyroScale) - gyroy_off);
    val[5] = -((float)(raw_val[5] * gyroScale) - gyroz_off);
    val[0] = ((float)(raw_val[1] - cal_acc_off[1]) / (float)cal_acc_scale[1]);
    val[1] = ((float)(raw_val[0] - cal_acc_off[0]) / (float)cal_acc_scale[0]); 
    val[2] = -((float)(raw_val[2] - cal_acc_off[2])  / (float)cal_acc_scale[2]);
    val[6] = ((float)(raw_val[6] - cal_magn_off[0]) / (float)cal_magn_scale[0]);
    val[7] =  ((float)(raw_val[7] - cal_magn_off[1]) / (float)cal_magn_scale[1]) ;
    val[8] = ((float)(raw_val[8] - cal_magn_off[2])  / (float)cal_magn_scale[2]) ;
  }
}

void getFusion() {
  getCalIMU();

  if(newIMUData) {
    newIMUData = 0;
    dt = ((now1 - lastUpdate) * 0.000001);
    lastUpdate = now1;
    Serial.println(dt, 6);
    //Lets normalize magnetometer data
    //float mag_norm = sqrt(val[6]*val[6] + val[7]*val[7] + val[8]*val[8]);

    // Acquire latest sensor data
    FusionVector gyroscope = { val[3], val[4], val[5] }; // replace this with actual gyroscope data in degrees/s
    FusionVector accelerometer = { val[0], val[1], val[2] }; // replace this with actual accelerometer data in g
    FusionVector magnetometer = {  val[6], val[7], val[8] }; // replace this with actual magnetometer data in arbitrary units

    // Update gyroscope offset correction algorithm
    gyroscope = FusionOffsetUpdate(&offset, gyroscope);

    // Calculate delta time (in seconds) to account for gyroscope sample clock error
    FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, dt);
    //FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, dt);

    flags = FusionAhrsGetFlags(&ahrs);
    euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
    q = FusionAhrsGetQuaternion(&ahrs);
    fHeading = FusionCompassCalculateHeading(FusionConventionNed, accelerometer, magnetometer);

    //if(dump > 100) {
      if(raw_values_on == 1) {
        print_float_array(val, 9);
        Serial.println();
      }

      if(serial_values_on == 1) {
        Serial.printf("Fusion (RPY): %0.1f, %0.1f, %0.1f\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
      }

      if(x_values_on == 1) {
        timestamp = micros();
        char accelgyroBuffer[100];
        sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f,%0.6f,%0.6f,%0.6f", 'I', timestamp, val[3], val[4], val[5], val[0], val[1], val[2]);
        Serial.printf ("%s\n",accelgyroBuffer);
        sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f", 'M', timestamp, val[6], val[7], val[8]);
        Serial.printf ("%s\n",accelgyroBuffer);
        sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f,%0.6f", 'Q', timestamp, q.element.w, q.element.x, q.element.y, q.element.z);
        Serial.printf ("%s\n",accelgyroBuffer);
      }

      if(telem_values_on == 1) telemetryPortOut();
      //dump = 0;
  }
}

void gyroCalibration() {
  int numSamples = 500;
  gyrox_off = 0;
  gyroy_off = 0;
  gyroz_off = 0;
  //imu.DisableDrdyInt();
  for(int i = 0; i < numSamples; i++){
    imu.Read_raw(raw_val);
    gyrox_off += raw_val[3] * gyroScale;
    gyroy_off += raw_val[4] * gyroScale;
    gyroz_off += raw_val[5] * gyroScale;
  }
  gyrox_off = gyrox_off / numSamples;
  gyroy_off = gyroy_off / numSamples;
  gyroz_off = gyroz_off / numSamples;
  //imu.EnableDrdyInt();
  Serial.printf("%f, %f, %f\n", gyrox_off, gyroy_off,gyroz_off );
}

void menu()
{
  cout.println();
  cout.println("Menu Options:");
  cout.println("========================================");
  cout.println("\tx - x-IMU3 GUI Output");
  cout.println("\tt - Telemetry Viewer Output");
  cout.println("\ts - Serial Print Output (Euler Angles)");
  cout.println("\tf - Fusion On");
  cout.println("\tr - Print Values");
  cout.println("========================================");
  cout.println("\tg - Zero Gyroscope");

  cout.println("========================================");
  cout.println("\th - Menu");
  cout.println("========================================");
  cout.println();
}

Constants.h

#define cout SerialUSB1

//elapsedMillis dump;

float SAMPLE_RATE; // replace this with actual sample RATE in Hz
float g = 9.80665;
float rads2degs = 57.296;

float fHeading = 0;

float val[10];
int16_t raw_val[10];

uint64_t timestamp;
uint8_t raw_values_on = 0;
uint8_t x_values_on = 0;
uint8_t serial_values_on = 0;
uint8_t telem_values_on = 0;
uint8_t fusion_on = 0;

// gyro bias estimation
float gyrox_off, gyroy_off, gyroz_off;

float mag_norm;

double dt; long t_old;

SerialOutput:

void telemetryPortOut(){  
  // Set textLength to the number of parameters to print * 31
  int  textLength = 22 * 31;
  char text[textLength];

  //Temporary text parameters
  char gyroxText[30];
  char gyroyText[30];
  char gyrozText[30];

  char accxText[30];
  char accyText[30];
  char acczText[30];

  char magxText[30];
  char magyText[30];
  char magzText[30];

  char pitchText[30];
  char rollText[30];
  char yawText[30];
  char fHeadingText[30];

  char qwText[30];
  char qxText[30];
  char qyText[30];
  char qzText[30];

  char f01[30]; char f02[30]; char f05[30];
  //char f01[30]; char f02[30]; char f03[30]; char f04[30]; char f05[30];

  dtostrf(val[3], 10, 10, gyroxText);
  dtostrf(val[4], 10, 10, gyroyText);
  dtostrf(val[5], 10, 10, gyrozText);
  dtostrf(val[0], 10, 10, accxText);
  dtostrf(val[1], 10, 10, accyText);
  dtostrf(val[2], 10, 10, acczText);
  dtostrf(val[6], 10, 10, magxText);
  dtostrf(val[7], 10, 10, magyText);
  dtostrf(val[8], 10, 10, magzText);
  dtostrf(euler.angle.pitch, 10, 10, pitchText);
  dtostrf(euler.angle.roll, 10, 10, rollText);
  dtostrf(euler.angle.yaw, 10, 10, yawText);
  dtostrf(fHeading, 10, 10, fHeadingText);

  dtostrf( q.element.w, 10, 6, qwText);
  dtostrf(q.element.x, 10, 6, qxText);
  dtostrf(q.element.y, 10, 6, qyText);
  dtostrf(q.element.z, 10, 6, qzText);  

  dtostrf((float)flags.initialising, 10, 6, f01);
  dtostrf((float)flags.accelerationRecovery, 10, 6, f02);
  //dtostrf((float)flags.accelerationRejectionTimeout, 10, 6, f03);
  //dtostrf((float)flags.magneticRejectionWarning, 10, 6, f04);
  dtostrf((float)flags.magneticRecovery, 10, 6, f05);

  // Create single text parameter and print it
  snprintf(text, textLength, "%s, %s, %s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s",
           gyroxText, gyroyText, gyrozText,
           accxText, accyText, acczText,
           magxText, magyText, magzText,
           pitchText, rollText, yawText,fHeadingText,
           qwText, qxText, qyText, qzText,
           f01, f02, f05);
/*
  dtostrf((float)flags.initialising, 10, 6, f01);
  dtostrf((float)flags.accelerationRejectionWarning, 10, 6, f02);
  dtostrf((float)flags.accelerationRejectionTimeout, 10, 6, f03);
  dtostrf((float)flags.magneticRejectionWarning, 10, 6, f04);
  dtostrf((float)flags.magneticRejectionTimeout, 10, 6, f05);

  // Create single text parameter and print it
  snprintf(text, textLength, "%s, %s, %s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s",
           gyroxText, gyroyText, gyrozText,
           accxText, accyText, acczText,
           magxText, magyText, magzText,
           pitchText, rollText, yawText,fHeadingText,
           qwText, qxText, qyText, qzText,
           f01, f02, f03, f04, f05);
*/
    Serial.println(text);  
  //Serial.send_now();

} 

void print_float_array(float *arr, int size) {
  if (size == 0) {
    Serial.printf("[]");
  } else {
    Serial.print('[');
    for (int i = 0; i < size - 1; i++)
      Serial.printf("%f, ", arr[i]);
    Serial.printf("%f]", arr[size - 1]);
  }
}

[mjs513]

Fast update. Started to get data when I commented out the following

    // Update gyroscope offset correction algorithm
    gyroscope = FusionOffsetUpdate(&offset, gyroscope);

[mjs513]

Going to close this issue out now that is working but have other questions that will post separately