Add getters for NXP linear acceleration, gravity vector, and geomagnetic vector

[ToMe25]

Changes:

• Add getters for the sensor frame linear acceleration, the gravity vector, and the geomagnetic vector to the NXP Fusion filter.
• Protect the NXP Fusion header from defining stuff twice.
• Closes #17

Limitations:

• Without this change it does not matter for practical purposes whether your accelerations are in m/s² or g, but with this change they have to be in g, as otherwise the linear acceleration doesn't work.
• This is not technically a limitation of my change, as this is the case because of code that i did not touch, but before its result wasn't accessible.

Tests:

• I used this class, and a bunch of modified versions of it for testing:
  Test class(Click to show)

#include <Arduino.h>
#include <Adafruit_LSM9DS1.h>
#include <Adafruit_AHRS_NXPFusion.h>
#include <Adafruit_AHRS.h>
#include <sys/time.h>
#include <cmath>

Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();
Adafruit_NXPSensorFusion filter;

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

    if (!lsm.begin()) {
        while (1) {
            Serial.println(
                    "Failed to initialize the LSM9DS1. Check your wiring!");
            delay(1000);
        }
    }

    lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G);
    lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
    lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);

    filter.begin(20);
}

void loop() {
    struct timeval tv_now;
    gettimeofday(&tv_now, NULL);
    uint64_t start_us = ((uint64_t) tv_now.tv_sec * 1000000L
            + (uint) tv_now.tv_usec);

    lsm.read();

    sensors_event_t a, m, g, t;

    lsm.getEvent(&a, &m, &g, &t);

    filter.update(g.gyro.x * SENSORS_RADS_TO_DPS,
            g.gyro.y * SENSORS_RADS_TO_DPS, g.gyro.z * SENSORS_RADS_TO_DPS,
            a.acceleration.x / 1,
            a.acceleration.y / 1,
            a.acceleration.z / 1, m.magnetic.x,
            m.magnetic.y, m.magnetic.z);

    Serial.println("--------------------");

    Serial.print("Pitch: ");
    Serial.println(filter.getPitch());

    Serial.print("Roll: ");
    Serial.println(filter.getRoll());

    Serial.print("Yaw: ");
    Serial.println(filter.getYaw());

    Serial.print("Gyro X: ");
    Serial.print(g.gyro.x);
    Serial.println(" rad/s");
    Serial.print("Gyro Y: ");
    Serial.print(g.gyro.y);
    Serial.println(" rad/s");
    Serial.print("Gyro Z: ");
    Serial.print(g.gyro.z);
    Serial.println(" rad/s");

    Serial.print("Acceleration X: ");
    Serial.print(a.acceleration.x);
    Serial.println(" m/s^2");
    Serial.print("Acceleration Y: ");
    Serial.print(a.acceleration.y);
    Serial.println(" m/s^2");
    Serial.print("Acceleration Z: ");
    Serial.print(a.acceleration.z);
    Serial.println(" m/s^2");

    Serial.print("Magnetic X: ");
    Serial.print(m.magnetic.x);
    Serial.println(" µT");
    Serial.print("Magnetic Y: ");
    Serial.print(m.magnetic.y);
    Serial.println(" µT");
    Serial.print("Magnetic Z: ");
    Serial.print(m.magnetic.z);
    Serial.println(" µT");

    float linear_x, linear_y, linear_z;
    filter.getLinearAcceleration(&linear_x, &linear_y, &linear_z);
    Serial.print("Linear Acceleration X: ");
    Serial.print(linear_x * SENSORS_GRAVITY_EARTH);
    Serial.println(" m/s^2");
    Serial.print("Linear Acceleration Y: ");
    Serial.print(linear_y * SENSORS_GRAVITY_EARTH);
    Serial.println(" m/s^2");
    Serial.print("Linear Acceleration Z: ");
    Serial.print(linear_z * SENSORS_GRAVITY_EARTH);
    Serial.println(" m/s^2");

    float gravity_x, gravity_y, gravity_z;
    filter.getGravityVector(&gravity_x, &gravity_y, &gravity_z);
    Serial.print("Gravity X: ");
    Serial.print(gravity_x);
    Serial.println(" g");
    Serial.print("Gravity Y: ");
    Serial.print(gravity_y);
    Serial.println(" g");
    Serial.print("Gravity Z: ");
    Serial.print(gravity_z);
    Serial.println(" g");

    float magnetic_x, magnetic_y, magnetic_z;
    filter.getGeomagneticVector(&magnetic_x, &magnetic_y, &magnetic_z);
    Serial.print("Geomagnetic X: ");
    Serial.print(magnetic_x);
    Serial.println(" µT");
    Serial.print("Geomagnetic Y: ");
    Serial.print(magnetic_y);
    Serial.println(" µT");
    Serial.print("Geomagnetic Z: ");
    Serial.print(magnetic_z);
    Serial.println(" µT");

    gettimeofday(&tv_now, NULL);
    uint64_t end_us = ((uint64_t) tv_now.tv_sec * 1000000L
            + (uint) tv_now.tv_usec);

    Serial.print("Measuring Time: ");
    Serial.print(uint(end_us - start_us));
    Serial.println("µs");
    delayMicroseconds(50000 - min(uint64_t(50000), (end_us - start_us)));
}

[ladyada]

hiya you'll need to add doxygen https://learn.adafruit.com/contribute-to-arduino-with-git-and-github

[ToMe25]

I now read mostly everything on that website until Doxygen Tips and installed Doxygen. However running doxygen doesn't cause any warnings, except two saying tags in the Doxyfile are obsolete. Is Documenting Functions what you want me to do? If yes, please let me know, i can do that for the new functions, but it isn't done anywhere else in this repo so i am not sure whether thats what you mean.

[ladyada]

oh looks like doxygen isnt enabled. well, you can add if you'd like, eventually it will need to be added so if you can, it will help!

[ToMe25]

@ladyada I will make a PR adding some method docu for things i know later today then :smiley:

[Hatuzilla]

Hi, thanks for adding the linear acceleration. I have just been testing it with the NXP 9DOF and noticed that gravity seems to still have an effect on the output. So my X & Y are at 0ish but my Z is at -6 (which is odd, why not 1 (or 9.81?)). If I tilt the board the output permanently changes to suit the effect of gravity. Which I thought it should not? Everything should return to 0.

The Gravity Vector works great.

I've attached 2 images. One with the board flat (showing -6 in the Z) and one with the board standing on it's end (showing -6ish in the X). As it's linear acceleration should these all be 0 as the board is not moving?

Edit - I should add that I am using the NXPSensorFusion

[ToMe25]

I have tested this with a LSM9DS1, and while the results weren't perfect, at least while not moving the sensor they were pretty close to 0:0:0. I will test this again tomorrow to be sure, but I don't know why it wouldn't work.

Also please post your full test code, so I can look at whether I am doing anything different.

[Hatuzilla]

I believe you :) I am starting to think it might be me.

The code is more or less taken from the examples so there is a lot of it, but everything is there.

Sorry, not sure how to collapse it.

#include <Arduino.h>
#include <Adafruit_Sensor_Calibration.h>
#include <Adafruit_AHRS.h>
#include <Adafruit_FXAS21002C.h>
#include <Adafruit_FXOS8700.h>

void setup_sensors(void){}
bool init_sensors(void);

Adafruit_FXOS8700 fxos = Adafruit_FXOS8700(0x8700A, 0x8700B);
Adafruit_FXAS21002C fxas = Adafruit_FXAS21002C(0x0021002C);

Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;

// uncomment one combo 9-DoF!
//#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 filter; // slowest
//Adafruit_Madgwick filter;  // 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->printSensorDetails();
  gyroscope->printSensorDetails();
  magnetometer->printSensorDetails();

  setup_sensors();
  filter.begin(FILTER_UPDATE_RATE_HZ);
  timestamp = millis();

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

void loop() {
  //float roll, pitch, heading;
  float lin_x, lin_y, lin_z;
  float acc_x, acc_y, acc_z;
  float gx, gy, gz;
  static uint8_t counter = 0;

  if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) {
    return;
  }
  timestamp = millis();
  // Read the motion sensors
  sensors_event_t accel, gyro, mag;
  accelerometer->getEvent(&accel);
  gyroscope->getEvent(&gyro);
  magnetometer->getEvent(&mag);
#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif

  cal.calibrate(mag);
  cal.calibrate(accel);
  cal.calibrate(gyro);
  // Gyroscope needs to be converted from Rad/s to Degree/s
  // the rest are not unit-important
  gx = gyro.gyro.x * SENSORS_RADS_TO_DPS;
  gy = gyro.gyro.y * SENSORS_RADS_TO_DPS;
  gz = gyro.gyro.z * SENSORS_RADS_TO_DPS;

  // Update the SensorFusion filter
  filter.update(gx, gy, gz, 
                accel.acceleration.x, accel.acceleration.y, accel.acceleration.z, 
                mag.magnetic.x, mag.magnetic.y, mag.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("Raw: ");
  Serial.print(accel.acceleration.x, 4); Serial.print(", ");
  Serial.print(accel.acceleration.y, 4); Serial.print(", ");
  Serial.print(accel.acceleration.z, 4); Serial.print(", ");
  Serial.print(gx, 4); Serial.print(", ");
  Serial.print(gy, 4); Serial.print(", ");
  Serial.print(gz, 4); Serial.print(", ");
  Serial.print(mag.magnetic.x, 4); Serial.print(", ");
  Serial.print(mag.magnetic.y, 4); Serial.print(", ");
  Serial.print(mag.magnetic.z, 4); Serial.println("");
#endif
/*
  // print the heading, pitch and roll
  roll = filter.getRoll();
  pitch = filter.getPitch();
  heading = filter.getYaw();
  Serial.print("Orientation: ");
  Serial.print(heading);
  Serial.print(", ");
  Serial.print(pitch);
  Serial.print(", ");
  Serial.println(roll);

  float qw, qx, qy, qz;
  filter.getQuaternion(&qw, &qx, &qy, &qz);
  Serial.print("Quaternion: ");
  Serial.print(qw, 4);
  Serial.print(", ");
  Serial.print(qx, 4);
  Serial.print(", ");
  Serial.print(qy, 4);
  Serial.print(", ");
  Serial.println(qz, 4);*/

  filter.getLinearAcceleration(&lin_x, &lin_y, &lin_z);
  Serial.print("Linear Acceleration: ");
  Serial.print(lin_x);
  Serial.print(", ");
  Serial.print(lin_y);
  Serial.print(", ");
  Serial.println(lin_z);

  /*filter.getGravityVector(&acc_x, &acc_y, &acc_z);
  Serial.print("Gravitational Acceleration: ");
  Serial.print(acc_x);
  Serial.print(", ");
  Serial.print(acc_y);
  Serial.print(", ");
  Serial.println(acc_z);*/

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

bool init_sensors(void) {
  if (!fxos.begin() || !fxas.begin()) {
    return false;
  }
  accelerometer = fxos.getAccelerometerSensor();
  gyroscope = &fxas;
  magnetometer = fxos.getMagnetometerSensor();

  return true;
}

[ToMe25]

@MrGaryB I did some testing and as far as I can tell everything works properly with the library. I also found the issue with your test program.

You are giving the filter acceleration values in m/s/s. This is generally fine, since for the actual filter calculations the acceleration values are normalized anyways. However linear acceleration is the only result for which this isn't fine. To get actual linear acceleration values you need to give the filter acceleration values in g.

Solution: convert the accelerometer values from m/s/s to g. Here a fixed version of your program in which I fixed that and added a comment about it: Disclaimer: I only tested this with a LSM9DS1, since that is the only accelerometer I have.

#include <Arduino.h>
#include <Adafruit_Sensor_Calibration.h>
#include <Adafruit_AHRS.h>
#include <Adafruit_FXAS21002C.h>
#include <Adafruit_FXOS8700.h>

void setup_sensors(void){}
bool init_sensors(void);

Adafruit_FXOS8700 fxos = Adafruit_FXOS8700(0x8700A, 0x8700B);
Adafruit_FXAS21002C fxas = Adafruit_FXAS21002C(0x0021002C);

Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;

// uncomment one combo 9-DoF!
//#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 filter; // slowest
//Adafruit_Madgwick filter;  // 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->printSensorDetails();
  gyroscope->printSensorDetails();
  magnetometer->printSensorDetails();

  setup_sensors();
  filter.begin(FILTER_UPDATE_RATE_HZ);
  timestamp = millis();

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

void loop() {
  //float roll, pitch, heading;
  float lin_x, lin_y, lin_z;
  float grav_x, grav_y, grav_z;
  float gx, gy, gz;
  float ax, ay, az;
  static uint8_t counter = 0;

  if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) {
    return;
  }
  timestamp = millis();
  // Read the motion sensors
  sensors_event_t accel, gyro, mag;
  accelerometer->getEvent(&accel);
  gyroscope->getEvent(&gyro);
  magnetometer->getEvent(&mag);
#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif

  cal.calibrate(mag);
  cal.calibrate(accel);
  cal.calibrate(gyro);

  // Gyroscope needs to be converted from Rad/s to Degree/s
  // Accelerometer needs to be converted to G for linear acceleration to work
  // For everything else its unit doesn't matter
  // Magnetometer unit doesn't matter
  gx = gyro.gyro.x * SENSORS_RADS_TO_DPS;
  gy = gyro.gyro.y * SENSORS_RADS_TO_DPS;
  gz = gyro.gyro.z * SENSORS_RADS_TO_DPS;

  ax = accel.acceleration.x / SENSORS_GRAVITY_EARTH;
  ay = accel.acceleration.y / SENSORS_GRAVITY_EARTH;
  az = accel.acceleration.z / SENSORS_GRAVITY_EARTH;

  // Update the SensorFusion filter
  filter.update(gx, gy, gz, ax, ay, az, mag.magnetic.x, mag.magnetic.y, mag.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("Raw: ");
  Serial.print(accel.acceleration.x, 4); Serial.print(", ");
  Serial.print(accel.acceleration.y, 4); Serial.print(", ");
  Serial.print(accel.acceleration.z, 4); Serial.print(", ");
  Serial.print(gx, 4); Serial.print(", ");
  Serial.print(gy, 4); Serial.print(", ");
  Serial.print(gz, 4); Serial.print(", ");
  Serial.print(mag.magnetic.x, 4); Serial.print(", ");
  Serial.print(mag.magnetic.y, 4); Serial.print(", ");
  Serial.print(mag.magnetic.z, 4); Serial.println("");
#endif
/*
  // print the heading, pitch and roll
  roll = filter.getRoll();
  pitch = filter.getPitch();
  heading = filter.getYaw();
  Serial.print("Orientation: ");
  Serial.print(heading);
  Serial.print(", ");
  Serial.print(pitch);
  Serial.print(", ");
  Serial.println(roll);

  float qw, qx, qy, qz;
  filter.getQuaternion(&qw, &qx, &qy, &qz);
  Serial.print("Quaternion: ");
  Serial.print(qw, 4);
  Serial.print(", ");
  Serial.print(qx, 4);
  Serial.print(", ");
  Serial.print(qy, 4);
  Serial.print(", ");
  Serial.println(qz, 4);*/

  filter.getLinearAcceleration(&lin_x, &lin_y, &lin_z);
  Serial.print("Linear Acceleration: ");
  Serial.print(lin_x);
  Serial.print(", ");
  Serial.print(lin_y);
  Serial.print(", ");
  Serial.println(lin_z);

  filter.getGravityVector(&grav_x, &grav_y, &grav_z);
  Serial.print("Gravitational Acceleration: ");
  Serial.print(grav_x);
  Serial.print(", ");
  Serial.print(grav_y);
  Serial.print(", ");
  Serial.println(grav_z);

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

bool init_sensors(void) {
  if (!fxos.begin() || !fxas.begin()) {
    return false;
  }
  accelerometer = fxos.getAccelerometerSensor();
  gyroscope = &fxas;
  magnetometer = fxos.getMagnetometerSensor();

  return true;
}

[Hatuzilla]

@ToMe25 Ok, this has been noted & thank you for checking. I'm won't be able to check this with the NXP for a couple of days, but once I'm back with the kit I'll test it and report back.

Thanks again.

[Hatuzilla]

@ToMe25 Many thanks. Your correction of my code works great. Thanks for your time, much appreciated. I'm sorry to have doubted it :)

This is using the Adafruit NXP 9DOF

[ToMe25]

@MrGaryB Im glad it worked :)

I'm sorry to have doubted it :)

No worries, I was confused by that as well at first :)