mjs513
Senior Member+
SensorFusion/NXPMotionSense using a MPU-9250
After using various Kalman filters decided to see if I could get the SensorFusion algorithm working with a MPU-9250 instead of the propshield. I did manage to get it operational but... The but is that yaw does converge to a solution but very very slowly on the order of minute or two. Pitch and Roll work fine and are responsive to imu movement. Any suggestion on what parameters that I can changed in SensorFusion.cpp would be appreciated.
Here is some added info:
I did have to make a couple of changes to SensorFusion.cpp and NXPMotionSense.h to get yaw working:
1. Added a function to return quaternions to NXPMotionSense.h:
2. changed the limits on the validCal to:
SENSORFUSION.CPP Parameters:
And finally, here is the sketch I am using:
After using various Kalman filters decided to see if I could get the SensorFusion algorithm working with a MPU-9250 instead of the propshield. I did manage to get it operational but... The but is that yaw does converge to a solution but very very slowly on the order of minute or two. Pitch and Roll work fine and are responsive to imu movement. Any suggestion on what parameters that I can changed in SensorFusion.cpp would be appreciated.
Here is some added info:
I did have to make a couple of changes to SensorFusion.cpp and NXPMotionSense.h to get yaw working:
1. Added a function to return quaternions to NXPMotionSense.h:
Code:
void getQuaternion(float quat[4]) {
quat[0] = qPl.q0;
quat[1] = qPl.q1;
quat[2] = qPl.q2;
quat[3] = qPl.q3;
}2. changed the limits on the validCal to:
Code:
if (fabsf(mx) >= 0.0f && fabsf(my) >= 0.0f && fabsf(mz) >= 0.0f) {
//Note was 20 for all cases
ValidMagCal = 1;
} else {
ValidMagCal = 0;
}SENSORFUSION.CPP Parameters:
Code:
// kalman filter noise variances
#define FQVA_9DOF_GBY_KALMAN 2E-6F // accelerometer noise g^2 so 1.4mg RMS
#define FQVM_9DOF_GBY_KALMAN 0.1F // magnetometer noise uT^2
#define FQVG_9DOF_GBY_KALMAN 0.3F // gyro noise (deg/s)^2
#define FQWB_9DOF_GBY_KALMAN 1E-9F // gyro offset drift (deg/s)^2: 1E-9 implies 0.09deg/s max at 50Hz
#define FQWA_9DOF_GBY_KALMAN 1E-4F // linear acceleration drift g^2 (increase slows convergence to g but reduces sensitivity to shake)
#define FQWD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance drift uT^2 (increase slows convergence to B but reduces sensitivity to magnet)
// initialization of Qw covariance matrix
#define FQWINITTHTH_9DOF_GBY_KALMAN 2000E-5F // th_e * th_e terms
#define FQWINITBB_9DOF_GBY_KALMAN 250E-3F // b_e * b_e terms
#define FQWINITTHB_9DOF_GBY_KALMAN 0.0F // th_e * b_e terms
#define FQWINITAA_9DOF_GBY_KALMAN 10E-5F // a_e * a_e terms (increase slows convergence to g but reduces sensitivity to shake)
#define FQWINITDD_9DOF_GBY_KALMAN 600E-3F // d_e * d_e terms (increase slows convergence to B but reduces sensitivity to magnet)
// linear acceleration and magnetic disturbance time constants
#define FCA_9DOF_GBY_KALMAN 0.5F // linear acceleration decay factor
#define FCD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance decay factor
// maximum geomagnetic inclination angle tracked by Kalman filter
#define SINDELTAMAX 0.9063078F // sin of max +ve geomagnetic inclination angle: here 65.0 deg
#define COSDELTAMAX 0.4226183F // cos of max +ve geomagnetic inclination angle: here 65.0 deg
#define DEFAULTB 50.0F // default geomagnetic field (uT)
#define X 0 // vector components
#define Y 1
#define Z 2
#define FDEGTORAD 0.01745329251994F // degrees to radians conversion = pi / 180
#define FRADTODEG 57.2957795130823F // radians to degrees conversion = 180 / pi
#define ONEOVER48 0.02083333333F // 1 / 48
#define ONEOVER3840 0.0002604166667F // 1 / 3840And finally, here is the sketch I am using:
Code:
/*
NXPSensorfusion_MPU9250.ino
*/
#include <NXPMotionSense.h>
#include "MPU9250.h" //Brian Taylor's MPU9250 library from Bolderfilight
#include "EEPROM.h"
#include <elapsedMillis.h>
// IMU Declares ----------------------------------
#define IMU_BUS Wire //Wire // SPI
#define IMU_ADDR 0x68 //0x69 // 0x68 // SPI 9
#define IMU_SCL 19 //47 // 0x255
#define IMU_SDA 18 //48 // 0x255
#if (F_CPU == 240000000)
#define IMU_SPD 1000000 //1000000 // 0==NULL or other
#else
#define IMU_SPD 1000000 //1000000 // 0==NULL or other
#endif
#define IMU_SRD 9 // Used in initIMU setSRD() - settin gSRD to 9 for a 100 Hz update rate
#define IMU_INT_PIN 24 //50 // 1 // 16
// an MPU-9250 object on SPI bus 0 with chip select 10
MPU9250 Imu(IMU_BUS, IMU_ADDR);
int status;
// a Sensor Fusion object
NXPSensorFusion filter;
// a flag for when the MPU-9250 has new data
volatile int newData;
// EEPROM buffer and variables to load accel and mag bias
// and scale factors from CalibrateMPU9250.ino
uint8_t EepromBuffer[48];
float axb, axs, ayb, ays, azb, azs;
float hxb, hxs, hyb, hys, hzb, hzs;
float q[4];
float ax, ay, az;
float gx, gy, gz;
float mx, my, mz;
// timers to measure performance
unsigned long tstart, tstop;
elapsedMillis sincePrint;
float roll, pitch, heading;
void setup() {
// serial to display data
Serial.begin(115200);
while(!Serial && millis()<2000) {}
// start communication with IMU
status = Imu.begin();
Wire.setClock(400000);
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) {}
}
Serial.println("IMU initialization successful");
// 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(&axb,EepromBuffer+0,4);
memcpy(&axs,EepromBuffer+4,4);
memcpy(&ayb,EepromBuffer+8,4);
memcpy(&ays,EepromBuffer+12,4);
memcpy(&azb,EepromBuffer+16,4);
memcpy(&azs,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);
//Print calibration values
Serial.print( axb,6); Serial.print(", "); Serial.println(axs,6);
Serial.print( ayb,6); Serial.print(", "); Serial.println(ays,6);
Serial.print( azb,6); Serial.print(", "); Serial.println(azs,6);
Serial.print( hxb,6); Serial.print(", "); Serial.println(hxs,6);
Serial.print( hyb,6); Serial.print(", "); Serial.println(hys,6);
Serial.print( hzb,6); Serial.print(", "); Serial.println(hzs,6);
Serial.println("Accel axes offset/scale factors:");
Serial.printf("%f, %f, \n%f, %f, \n%f, %f\n", axb, axs, ayb, ays, azb, azs);
Serial.println("Magnetometer axes offset/scale factors:");
Serial.printf("%f, %f, \n%f, %f, \n%f, %f\n", hxb, hxs, hyb, hys, hzb, hzs);
//Load calibration values into sketch
Imu.setAccelCalX(axb,axs);
Imu.setAccelCalY(ayb,ays);
Imu.setAccelCalZ(azb,azs);
Imu.setMagCalX(hxb,hxs);
Imu.setMagCalY(hyb,hys);
Imu.setMagCalZ(hzb,hzs);
//Options to change ranges.
// setting the accelerometer full scale range to +/-8G
//Imu.setAccelRange(MPU9250::ACCEL_RANGE_16G);
// setting the gyroscope full scale range to +/-500 deg/s
//Imu.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
// setting a 20 Hz DLPF bandwidth
Imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
// setting SRD to 9 for a 100 Hz update rate
// 1000/(1+SRD) = update rate
// SRD = (1000/rate)-1,9
Imu.setSrd(9);
filter.begin(100); // 100 measurements per second
//Calibrate Gyro, do not move board
Imu.calibrateGyro();
// enabling the data ready interrupt
Imu.enableDataReadyInterrupt();
//attaching the interrupt to microcontroller pin 1
pinMode(IMU_INT_PIN,INPUT);
attachInterrupt(IMU_INT_PIN,runFilter,RISING);
}
void loop() {
//newData = 1;
if (newData == 1) {
newData = 0;
tstart = micros();
// read the sensor
Imu.readSensor();
// update the filter
gx = Imu.getGyroY_rads();
gy = Imu.getGyroX_rads();
gz = Imu.getGyroZ_rads();
ax = Imu.getAccelY_mss();
ay = Imu.getAccelX_mss();
az = Imu.getAccelZ_mss();
mx = Imu.getMagX_uT();
my = Imu.getMagY_uT();
mz = Imu.getMagZ_uT();
filter.update(gx, -gy, gz, ax, -ay, -az, -my, mx, mz);
//filter.update(gx*180/PI, -gy*180/PI, gz*180/PI, ax*9.8203, -ay*9.8203, -az*9.8203, -my, mx, mz);
tstop = micros();
if (sincePrint > 100){
sincePrint=0;
//Uncomment to print orientation based on
//Euler Angles
//Serial.print(filter.getPitch());
//Serial.print(", ");
//Serial.print(filter.getRoll());
//Serial.print(", ");
//Serial.println(filter.getYaw());
//Determine orientation based on Quaternion
filter.getQuaternion(q);
getYawPitchRollDeg();
}
}
}
void runFilter() {
newData = 1;
}
void getYawPitchRollDeg() {
float ypr[4];
float gx, gy, gz; // estimated gravity direction
//getQ(q, val);
gx = 2 * (q[1]*q[3] - q[0]*q[2]);
gy = 2 * (q[0]*q[1] + q[2]*q[3]);
gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
ypr[0] = (180/PI) * atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
ypr[1] = -(180/PI) * atan(gx / sqrt(gy*gy + gz*gz));
ypr[2] = (180/PI) * atan(gy / sqrt(gx*gx + gz*gz));
Serial.print(ypr[1]); Serial.print(",");
Serial.print(ypr[2]); Serial.print(",");
Serial.print(ypr[0]); Serial.println();
}
Last edited: