MPUOrientation - Compass with IMU calculations

Approach

Not every sensor comes with a library which include orientation calculation. This library is compatible with C and C++ compilers and solve the calculations by applying the Kalman filter.

ESP32 native Core problem with I2C

The Arduino IDE with Espressif native port for ESP32 has conflicted with Wire library. So I met the core of stickbreaker (click here) so currently is not necessary any change in the code between ESP32 and ESP8266. The code pressupose a valid and calibrated value to sensors input.

Using

Including LIB

#include <MPUOrientation.h>

Creating contexts

Contexts are useful in recursive operations (like in Kalman filter). So we need to use one for each MPU.

pCompassContext CNTX = createContext();

Calculating time between recall (Arduino)

//Declare global variable:
uint32_t timer;

...

// Inside loop(), when need to use delta time "dt"
double dt = (double)(micros() - timer) / 1000000; // Calculate as second
timer = micros(); // Reset time

Calculating orientation as IMUFullFusion

IMUFullFusion SENS;
SENS.ACCEL.x = IMU.getAccelX_mss();
SENS.ACCEL.y = IMU.getAccelY_mss();
SENS.ACCEL.z = IMU.getAccelZ_mss();

SENS.GYRO.x = IMU.getGyroX_rads();
SENS.GYRO.y = IMU.getGyroY_rads();
SENS.GYRO.z = IMU.getGyroZ_rads();

SENS.MAG.x = IMU.getMagX_uT();
SENS.MAG.y = IMU.getMagY_uT();
SENS.MAG.z = IMU.getMagZ_uT();


// Context, ALGO, IMUFullFusion and time variance in seconds
IMUOrientation ori = getFullOrientation(CNTX, ALGO_SOMETHING, SENS, dt);

Or calculating orientation as IMUFusion (no Mag)

IMUFusion SENS;
SENS.ACCEL.x = IMU.getAccelX_mss();
SENS.ACCEL.y = IMU.getAccelY_mss();
SENS.ACCEL.z = IMU.getAccelZ_mss();

SENS.GYRO.x = IMU.getGyroX_rads();
SENS.GYRO.y = IMU.getGyroY_rads();
SENS.GYRO.z = IMU.getGyroZ_rads();


// Context, ALGO, IMUFusion and time variance in seconds
IMUOrientation ori = getOrientation(CNTX, ALGO_SOMETHING, SENS, dt);

Remember to replace "ALGO_SOMETHING" by ALGO_KALMAN_V1 or ALGO_MADGWICK_V1.

Print orientation

Serial.printf( "\t%f\t%f\t%f\n", ori.pitch, ori.roll, ori.yaw );

This and other examples you can find in examples folder.

Some optimizations

We consider to port the libfixmath to reduce cycles of calculation. Maybe a #define will be included in next versions of compassSet.h.

Description about examples

Examble path	Functionality
ESP-NO-LIB	Calculate the axis Pitch, Roll and Yaw using magnetometer without the library.
ESP-Kalman-NO-MAG	Calculate the axis Pitch and Roll with the library.
ESP-Kalman-Compass	Calculate the axis Pitch, Roll and Yaw with the library.
ESP-Madgwick-Kalman-Full-Compare	A comparision between Kalman Filter and Madgwick quaternions calculation result. Compatible with processing script "Graph"
ESP-All-Full-Compare	A comparision bretween all fusion algorithms. Compatible with processing script "Graph3Mod".

Libraries

• Kalman Filter. Now is already ported.
• MPU9250 Download here. That's a fork from original library from bolderflight but with some implementations that I made for ESP8266/ESP32 in the "begin" function.