AHRS library for Teensy 4.1 and MPU6500 gyro/accel sensor (Reefwing?)

ajreijn · May 28, 2024

Hi there,

I'm looking for a library that can calculate roll, pitch, yaw and heading angles using the MPU6500 IMU and that is compatible with Teensy 4.1.

I found the Reefwing AHRS library. There's an example patch for the MPU6500 and MKR boards (file attached). I'm not able to compile that code, though - even when I install and select a MKR Arduino board.

error message:

/Users/rita/Arduino/libraries/ReefwingAHRS/src/ReefwingAHRS.cpp: In member function 'void ReefwingAHRS::begin()':
/Users/rita/Arduino/libraries/ReefwingAHRS/src/ReefwingAHRS.cpp:97:27: error: expected primary-expression before ',' token
if (strncmp(BOARD_NAME, _boardTypeStr[4], 25) == 0) {

Any one an idea of what is going wrong here and how this library could be made to use with the Teensy 4.1 board?

Or perhaps another library fit for the job.

Would love some tips. Thank you!!!

mjs513 · May 28, 2024

Don't know anything about that particular library. Typically I use @brtaylors library to get data from the 6500: https://github.com/bolderflight/invensense-imu and use either Mahoney or Madgwick filter libraries: https://github.com/PaulStoffregen/MadgwickAHRS, or https://github.com/PaulStoffregen/MahonyAHRS. Madgwick does have a new version out but a more confusing to use. You can see the discussion here: https://forum.pjrc.com/index.php?threads/new-madgwick-fusion-filter.72604/#post-327775

Be forewarned you will still need to calibrate your accel and gyro and mag if you are using it.

mjs513 · May 28, 2024

ajreijn said:
I found the Reefwing AHRS library. There's an example patch for the MPU6500 and MKR boards (file attached). I'm not able to compile that code, though - even when I install and select a MKR Arduino board.

error message:

/Users/rita/Arduino/libraries/ReefwingAHRS/src/ReefwingAHRS.cpp: In member function 'void ReefwingAHRS::begin()':
/Users/rita/Arduino/libraries/ReefwingAHRS/src/ReefwingAHRS.cpp:97:27: error: expected primary-expression before ',' token
if (strncmp(BOARD_NAME, _boardTypeStr[4], 25) == 0) {

Any one an idea of what is going wrong here and how this library could be made to use with the Teensy 4.1 board?

I got curious about the Reefwing library and it actually looks rather interesting. So I decided to check it out.

The issue is that the library defines board types that are supported and it is not configured for the Teensies only arduino boards. I have managed to get it to compile by adding Teensy 40 and Teensy 41 to the ReefwingAHRS library. Not set up right now to test but here are the .h and .cpp files for the AHRS library. Try replacing what you currently have with the attached file.

Sorry but the zip file of the whole library is too large to attach. Just do a copy and paste of the files in the zip file to the ReefwingAHRS src directory

ajreijn · May 28, 2024

Yes this compiles. Thank you so much!

Now the next step... The example connects using SPI. I was hoping to connect using I2C. According to the library documentation this should be possible, but I can't find any examples that show how...

mjs513 · May 28, 2024

ajreijn said:
Now the next step... The example connects using SPI. I was hoping to connect using I2C. According to the library documentation this should be possible, but I can't find any examples that show how...

If you look at the source file for the MPU65x0 it is only supporting SPI. However, if you use the https://github.com/Reefwing-Software/Reefwing-MPU6050/tree/main MPU6050 library that supports I2C. You would just have to modify the start of the sketch like this:

Code:

#include <ReefwingMPU6050.h>


ReefwingMPU6050 imu;


static const uint8_t INT_PIN = 1;
static const uint8_t LED0_PIN = A4;
static const uint8_t LED1_PIN = A5;


float mpuTemp = 0.0f;
int loopFrequency = 0;
const long displayPeriod = 1000;
unsigned long previousMillis = 0;


static MPU6500 imu;
static bool gotInterrupt;


static void handleInterrupt(void) {
    gotInterrupt = true;
}


void setup(void) {
    //  Pin Configuration
    pinMode(INT_PIN, INPUT);
    pinMode(LED0_PIN, OUTPUT);
    pinMode(LED1_PIN, OUTPUT);


    //  Start Serial and wait for connection
    Serial.begin(115200);
    while (!Serial);
    imu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G);
    if (imu.isconnected()) {  //use whatever settings you want
        Serial.println("MPU6500 IMU Connected.");
    }
    else {
        Serial.println("Error initializing IMU.");
        digitalWrite(LED1_PIN, HIGH);
        while(1);
    }


    attachInterrupt(INT_PIN, handleInterrupt, RISING);
}

You will have to look to see what else may have to change since you are using a different library

ajreijn · May 31, 2024

Sorry for the slow response... unfortunately I haven't been able to connect to the MPU6500 using the Reefwing MPU6500 library.
Just for the basic connection check I'm using this code;

Code:

#include <ReefwingMPU6050.h>

ReefwingMPU6050 imu;

void setup() {

  imu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G);

  Serial.begin(115200);
  while (!Serial);

  if (imu.connected()) {
    Serial.println("MPU6050 IMU Connected.");
    imu.calibrateGyro();
  } else {
    Serial.println("MPU6050 IMU Not Detected.");
    while(1);
  }
}

void loop() { }

mjs513 · May 31, 2024

I would look at this example and try it, Your not giving me much to work on - is it just saying not detected. Are you sure you are using a 6500, check the wiring - use I2C scanner to make sure the i2c address you have for the device is the same as in the library

ajreijn · May 31, 2024

I started off with an example from the Reefwing6500 library. The example code you posted in earlier post gave me this error;

Compilation error: 'MPU6500' does not name a type

I connected the IMU and using library underneath I've managed to make a connection;

GitHub - wollewald/MPU9250_WE: An Arduino library for the 9-axis accelerometer, gyroscope and magnetometer MPU9250 and MPU6500. It contains many example sketches make it easy to use.

An Arduino library for the 9-axis accelerometer, gyroscope and magnetometer MPU9250 and MPU6500. It contains many example sketches make it easy to use. - GitHub - wollewald/MPU9250_WE: An Arduino ...

github.com

From what I can see both libraries/examples use address 0x68

ajreijn · May 31, 2024

Just noticed that the library you suggested is for the MPU6050. Perhaps it won't work for the 6500...?

ajreijn · May 31, 2024

mjs513 said:
Don't know anything about that particular library. Typically I use @brtaylors library to get data from the 6500: https://github.com/bolderflight/invensense-imu and use either Mahoney or Madgwick filter libraries: https://github.com/PaulStoffregen/MadgwickAHRS, or https://github.com/PaulStoffregen/MahonyAHRS. Madgwick does have a new version out but a more confusing to use. You can see the discussion here: https://forum.pjrc.com/index.php?threads/new-madgwick-fusion-filter.72604/#post-327775

Be forewarned you will still need to calibrate your accel and gyro and mag if you are using it.

This method sounds great to me as well. Juist need to get my head around how to change the code along these lines; feeding imu data from the invensense library into the mahonyahrs library. Bottomline; I'm not much of a programmer... You don't happen to have an example code laying around I could work from?

mjs513 · May 31, 2024

ajreijn said:
This method sounds great to me as well. Juist need to get my head around how to change the code along these lines; feeding imu data from the invensense library into the mahonyahrs library. Bottomline; I'm not much of a programmer... You don't happen to have an example code laying around I could work from?

There are lots of examples already bundled with the libraries I cited. I would start with getting the sensor working @brtaylor's library first then can try and link to madwick or mahony ahrs

ajreijn · May 31, 2024

Will do. The sensor is working with @brtaylor's library. The linking is a bit of a challenge for me, but will dive into the matter tomorrow. Thanks!

mjs513 · Jun 1, 2024

ajreijn said:
Will do. The sensor is working with @brtaylor's library. The linking is a bit of a challenge for me, but will dive into the matter tomorrow. Thanks!

I will give you a hint. After you get the data from Brian's library you will have to cast it into a data struct which reefwing is looking for:

Code:

struct SensorData {
  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;
  uint32_t gTimeStamp, aTimeStamp, mTimeStamp;
};

ajreijn · Jun 1, 2024

Thank you so much for your help!

ajreijn · Jun 2, 2024

I got the Madgwick and Mahony filters running icw Brian's MPU6500 library (added both files in case a future user might find them helpful). But the roll, pitch and heading are very slow responsive. I guess these algorithms are best suited for low dynamic situations?

I decided to give the Reefwing library a try to compare, but there I'm stuck with an error...

Compilation error: expected primary-expression before ')' token

Code:

#include <ReefwingAHRS.h>
#include <Wire.h>
#include "mpu6500.h"


/* Mpu6500 object */
bfs::Mpu6500 imu;

ReefwingAHRS ahrs;
SensorData data;


void setup() {


  Wire.begin();
  Wire.setClock(400000);
  /* I2C bus,  0x68 address */
  imu.Config(&Wire, bfs::Mpu6500::I2C_ADDR_PRIM);
  /* Initialize and configure IMU */
  if (!imu.Begin()) {
    Serial.println("Error initializing communication with IMU");
    while (1) {}
  }
  /* Set the sample rate divider */
  if (!imu.ConfigSrd(19)) {
    Serial.println("Error configured SRD");
    while (1) {}
  }

  //  Initialise the AHRS
  //  begin() will detect the Portenta H7 or MKR Vidor 4000
  //  but these have no default IMU.
  ahrs.begin();
  ahrs.setImuType(ImuType::MPU6500);
  ahrs.setDOF(DOF::DOF_6);
  ahrs.setFusionAlgorithm(SensorFusion::CLASSIC);
  //ahrs.setDeclination(12.717);                      //  Sydney, Australia
  ahrs.setDeclination(5.6);  //  Belgrade

  //  Start Serial and wait for connection
  Serial.begin(115200);
  while (!Serial)
    ;

  Serial.print("Detected Board - ");
  Serial.println(ahrs.getBoardTypeString());
}



void loop() {

  int ax, ay, az;
  int gx, gy, gz;
  float roll, pitch, yaw, heading;

  if (imu.Read()) {

    ax = imu.accel_x_mps2();
    ay = imu.accel_y_mps2();
    az = imu.accel_z_mps2();
    gx = imu.gyro_x_radps();
    gy = imu.gyro_y_radps();
    gz = imu.gyro_z_radps();
  }

  struct data {
    float ax, ay, az;
    float gx, gy, gz;
    float mx, my, mz;
    uint32_t gTimeStamp, aTimeStamp, mTimeStamp;
  };

  ahrs.setData(data);
  ahrs.update();

  roll = ahrs.angles.roll, 2;
  pitch = ahrs.angles.pitch, 2;
  yaw = ahrs.angles.yaw, 2;
  heading = ahrs.angles.heading, 2;

  delay(100);
}

mjs513 · Jun 2, 2024

I may give this a try today but just looking at your sketch a couple of things pop out.
1. You do not need to define the structure in the body of the sketch - its already defined in imutypes.h.
2. you need to populate the structure. For instance, at the begining of the sketch you have

Code:

SensorData data;

so after you get the data from the imu you have to do for example

Code:

data.ax  =  imu.accel_x_mps2();

etc

or better yet create a function in your sketch like if Reefwing's 6500 class

Code:

SensorData getSensorData() {
            SensorData data;
            float gx, gy, gz, ax, ay, az;

            //getCalibratedGyro(gx, gy, gz);
            //getCalibratedAccel(ax, ay, az);
           ax = imu.accel_x_mps2();
            ay = imu.accel_y_mps2();
            az = imu.accel_z_mps2();
           gx = imu.gyro_x_radps();
           gy = imu.gyro_y_radps();
            gz = imu.gyro_z_radps();

            data.gx = gx;
            data.gy = gy;
            data.gz = gz;
            data.gTimeStamp = micros();


            data.ax = ax;
            data.ay = ay;
            data.az = az;
            data.aTimeStamp = micros();


            return data;
        }

and call it after you do the imu.read:

Code:

if (imu.Read()) {
      getSensorData();
  }

try that.

3. you didn't show the line the error came from.

4. try following the example mkrMPU6500.ino with the mods I suggested with adding the getSensorData to your sketch

mjs513 · Jun 2, 2024

Oh forgot to mention - use the interrupt version with brian's lib. Works better.

ajreijn · Jun 2, 2024

The error came from line 76;

ahrs.setData(data);

mjs513 · Jun 2, 2024

ajreijn said:
The error came from line 76;

ahrs.setData(data);

yeah thats because of the reasons I listed with setdata and structure not being initialized

when you call setdata you can drop the ahrs since you include it in the sketch

ajreijn · Jun 2, 2024

This is a bit beyond my limits I'm afraid. Perhaps time to find some one a bit more skillfull than myself to tie this one together.

I was just wondering. Was my observation about the Mahony and Medgwick filters not being well suited for tracking more rapid movements correct? And if so, is it likely that the Reefwing library will be?

mjs513 · Jun 2, 2024

ajreijn said:
I was just wondering. Was my observation about the Mahony and Medgwick filters not being well suited for tracking more rapid movements correct? And if so, is it likely that the Reefwing library will be?

The convergence speed of the filters depend on setting the gains correctly. The ones used probably are giving you that slow response. They can be changed but would have to optimize them.

Right now in the middle of updating another library so as soon as I get a chance will try and get it working

mjs513 · Jun 3, 2024

Ok had a chance to play with this. Unfortunately didn;t have a Mpu6500 handy but did have a 9250. This example works but I did have to make a mod to the AHRS lib:

Code:

#include "mpu9250.h"
/* Mpu9250 object */
bfs::Mpu9250 imu;

#include <ReefwingAHRS.h>
ReefwingAHRS ahrs;

//  Display and Loop Frequency
int loopFrequency = 0;
const long displayPeriod = 1000;
unsigned long previousMillis = 0;

void setup() {
  /* Serial to display data */
  Serial.begin(115200);
  while (!Serial) {}
  /* Start the I2C bus */
  Wire.begin();
  Wire.setClock(400000);
  /* I2C bus,  0x68 address */
  imu.Config(&Wire, bfs::Mpu9250::I2C_ADDR_PRIM);
  /* Initialize and configure IMU */
  if (!imu.Begin()) {
    Serial.println("Error initializing communication with IMU");
    while (1) {}
  }
  /* Set the sample rate divider */
  if (!imu.ConfigSrd(19)) {
    Serial.println("Error configured SRD");
    while (1) {}
  }

  ahrs.begin();

  //  Nano will be detected but could use any IMU
  //  If your IMU isn't autodetected and has a mag you need
  //  to add: ahrs.setDOF(DOF::DOF_9);
  ahrs.setDOF(DOF::DOF_6);
  ahrs.setImuType(ImuType::MPU6050);
  ahrs.setFusionAlgorithm(SensorFusion::KALMAN);
  //ahrs.setDeclination(12.717);                      //  Sydney, Australia
}

void loop() {
  /* Check if data read */
  if (imu.Read()) {
    ahrs.setData(getSensorData());

  }

    ahrs.update();
   
  if (millis() - previousMillis >= displayPeriod) {
    //  Display sensor data every displayPeriod, non-blocking.
    Serial.print("--> Roll: ");
    Serial.print(ahrs.angles.roll, 2);
    Serial.print("\tPitch: ");
    Serial.print(ahrs.angles.pitch, 2);
    Serial.print("\tYaw: ");
    Serial.print(ahrs.angles.yaw, 2);
    Serial.print("\tHeading: ");
    Serial.print(ahrs.angles.heading, 2);
    Serial.print("\tLoop Frequency: ");
    Serial.print(loopFrequency);
    Serial.println(" Hz");

    loopFrequency = 0;
    previousMillis = millis();
  }

  loopFrequency++;

}

SensorData getSensorData() {
  SensorData data;
  float gx, gy, gz, ax, ay, az;

  //getCalibratedGyro(gx, gy, gz);
  //getCalibratedAccel(ax, ay, az);
  ax = imu.accel_x_mps2();
  ay = imu.accel_y_mps2();
  az = imu.accel_z_mps2();
  gx = imu.gyro_x_radps();
  gy = imu.gyro_y_radps();
  gz = imu.gyro_z_radps();

  data.gx = gx;
  data.gy = gy;
  data.gz = gz;
  data.gTimeStamp = micros();


  data.ax = ax;
  data.ay = ay;
  data.az = az;
  data.aTimeStamp = micros();


  return data;
}

Sample output:

Code:

--> Roll: 172.30    Pitch: 47.99    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1602 Hz
--> Roll: 160.81    Pitch: 48.86    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1602 Hz
--> Roll: -177.09    Pitch: 42.72    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1602 Hz
--> Roll: -146.89    Pitch: -31.95    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1602 Hz
--> Roll: -150.61    Pitch: -28.38    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1602 Hz
--> Roll: 152.05    Pitch: -13.13    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1602 Hz
--> Roll: 160.96    Pitch: -4.19    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1602 Hz
--> Roll: 168.97    Pitch: 1.48    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1602 Hz
--> Roll: 167.80    Pitch: 1.06    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1603 Hz
--> Roll: 168.91    Pitch: 1.36    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1602 Hz
--> Roll: 168.20    Pitch: 1.35    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1602 Hz
--> Roll: 168.50    Pitch: 0.70    Yaw: 0.00    Heading: 0.00    Loop Frequency: 1602 Hz

Note you will have to get a board with a magnetometer if you want yaw and heading.

The zip file has the changed library file.

ajreijn · Jun 3, 2024

Yes this works! That's wonderful! Pitch and roll are my my gain goal, but I will probably also have a magnetometer on the board, so I can try to feed that data into the Reefwing AHRS as well. I'm guessing just the raw xyz values from the magnetometer and replace current DOF setting with this line in the setup...?

Code:

ahrs.setDOF(DOF::DOF_9);

Response of the roll data is quite good for my application, but the pitch is still responding quite slow. Was looking for values related to gain in the library, but couldn't find them. Wouldn't it make sense that these ahrs libraries provide settings for speed/responsiveness?

mjs513 · Jun 3, 2024

ajreijn said:
Response of the roll data is quite good for my application, but the pitch is still responding quite slow. Was looking for values related to gain in the library, but couldn't find them. Wouldn't it make sense that these ahrs libraries provide settings for speed/responsiveness?

it does for example

Code:

void setAlpha(float a);
    void setBeta(float b);
    void setGyroMeasError(float gme);
    void setKp(float p);
    void setKi(float i);

You need to learn algorithms (not the detailed math) but how they generally work. then experimenting will be easier

AHRS library for Teensy 4.1 and MPU6500 gyro/accel sensor (Reefwing?)

Active member

Attachments

Senior Member+

Senior Member+

Attachments

Active member

Senior Member+

Active member

Senior Member+

Active member

Active member

Active member

Senior Member+

Active member

Senior Member+

Active member

Active member

Attachments

Senior Member+

Senior Member+

Active member

Senior Member+

Active member

Senior Member+

Senior Member+

Attachments

Active member

Senior Member+