#include "MPU9250.h"
#include "Fusion.h"
#include <stdbool.h>
#include <stdio.h>
#include <Wire.h>
#include "EEPROM.h"
#include "Streaming.h"
#define cout Serial
uint64_t prev_time, cur_time;
float dt;
elapsedMillis dump;
#define SAMPLE_PERIOD (0.01f) // replace this with actual sample period
float g = 9.80665;
float rads2degs = 57.296;
FusionOffset offset;
FusionAhrs ahrs;
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
void setup() {
// serial to display data
Serial.begin(115200);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
cout.println("IMU Connected!");
loadCal();
// setting the accelerometer full scale range to +/-8G
IMU.setAccelRange(MPU9250::ACCEL_RANGE_2G);
// setting the gyroscope full scale range to +/-500 deg/s
IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
// setting DLPF bandwidth to 20 Hz
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
// setting SRD to 19 for a 50 Hz update rate
IMU.setSrd(19);
IMU.calibrateGyro();
FusionAhrsInitialise(&ahrs);
// Set AHRS algorithm settings
const FusionAhrsSettings settings = {
.convention = FusionConventionNed,
.gain = 0.5f,
.accelerationRejection = 10.0f,
.magneticRejection = 20.0f,
.rejectionTimeout = 5 * SAMPLE_PERIOD, /* 5 seconds */
};
FusionAhrsSetSettings(&ahrs, &settings);
}
void loop() {
float val[10];
// read the sensor
cur_time = micros();
IMU.readSensor(val);
// Acquire latest sensor data
FusionVector gyroscope = {val[3]*rads2degs, val[4]*rads2degs, val[5]*rads2degs}; // replace this with actual gyroscope data in degrees/s
FusionVector accelerometer = {val[0]/g, val[1]/g, val[2]/g}; // 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
gyroscope = FusionOffsetUpdate(&offset, gyroscope);
// Calculate delta time (in seconds) to account for gyroscope sample clock error
dt = (cur_time - prev_time)/1e6;
FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, dt);
prev_time = cur_time;
const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
if(dump > 1000) {
//print_float_array(val, 10);
//Serial.println();
Serial.printf("Fusion (RPY): %0.1f, %0.1f, %0.1f\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
dump = 0;
}
}
void loadCal() {
// EEPROM buffer and variables to load accel and mag bias
// and scale factors from CalibrateMPU9250.ino
uint8_t EepromBuffer[48];
float a_xb, a_xs, a_yb, a_ys, a_zb, a_zs;
float hxb, hxs, hyb, hys, hzb, hzs;
// load and set accel and mag bias and scale
// factors from CalibrateMPU9250.ino
for (size_t i = 0; i < sizeof(EepromBuffer); i++) {
EepromBuffer[i] = EEPROM.read(i);
}
memcpy(&a_xb, EepromBuffer + 0, 4);
memcpy(&a_xs, EepromBuffer + 4, 4);
memcpy(&a_yb, EepromBuffer + 8, 4);
memcpy(&a_ys, EepromBuffer + 12, 4);
memcpy(&a_zb, EepromBuffer + 16, 4);
memcpy(&a_zs, EepromBuffer + 20, 4);
memcpy(&hxb, EepromBuffer + 24, 4);
memcpy(&hxs, EepromBuffer + 28, 4);
memcpy(&hyb, EepromBuffer + 32, 4);
memcpy(&hys, EepromBuffer + 36, 4);
memcpy(&hzb, EepromBuffer + 40, 4);
memcpy(&hzs, EepromBuffer + 44, 4);
Serial.println("Accelerometer bias and scale factors:");
cout.print( a_xb, 6); cout.print(", "); Serial.println(a_xs, 6);
cout.print( a_yb, 6); cout.print(", "); Serial.println(a_ys, 6);
cout.print( a_zb, 6); cout.print(", "); Serial.println(a_zs, 6);
Serial.println(" ");
Serial.println("Magnetometer bias and scale factors:");
cout.print( hxb, 6); cout.print(", "); Serial.println(hxs, 6);
cout.print( hyb, 6); cout.print(", "); Serial.println(hys, 6);
cout.print( hzb, 6); cout.print(", "); Serial.println(hzs, 6);
Serial.println(" ");
IMU.setAccelCalX(a_xb, a_xs);
IMU.setAccelCalY(a_yb, a_ys);
IMU.setAccelCalZ(a_zb, a_zs);
IMU.setMagCalX(hxb, hxs);
IMU.setMagCalY(hyb, hys);
IMU.setMagCalZ(hzb, hzs);
}
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]);
}
}
