#include <SPI.h>
#include <WireKinetis.h>
#include <Wire.h>
#include <MPU9250.h>
// MPU-9250 sensor on SPI bus 0 and chip select pin 10
const int MPU9250_INT_PIN = 20;
const int WAKE_ON_MOTION_PIN = 21;
const int TEENSY_SPI_CS_PIN = 10;
//EEPROM consts
const uint8_t ACCEL_BIAS_X = 0;
const uint8_t ACCEL_SCALE_X = 4;
const uint8_t ACCEL_BIAS_Y = 8;
const uint8_t ACCEL_SCALE_Y = 12;
const uint8_t ACCEL_BIAS_Z = 16;
const uint8_t ACCEL_SCALE_Z = 20;
const uint8_t MAG_BIAS_X = 24;
const uint8_t MAG_SCALE_X = 28;
const uint8_t MAG_BIAS_Y = 32;
const uint8_t MAG_SCALE_Y = 36;
const uint8_t MAG_BIAS_Z = 40;
const uint8_t MAG_SCALE_Z = 44;
float eInt[3] = { 0.0f, 0.0f, 0.0f }; // vector to hold integral error for Mahony method
#define Kp 2.0f * 5.0f // These are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 0.0f
float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s)
float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
char charBuff[200] = { 0 };
//Quaterion vector values
float quaternion[4] = { 1.0f, 0.0f, 0.0f, 0.0f }; // vector to hold quaternion
uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
uint32_t Now = 0; // used to calculate integration interval
float deltat = 0.0f, sum = 0.0f; // Integration interval for both filter schemes
void waitOnError();
volatile float fVectorAX;
volatile float fVectorAY;
volatile float fVectorAZ;
volatile float fVectorGX;
volatile float fVectorGY;
volatile float fVectorGZ;
volatile float fVectorMX;
volatile float fVectorMY;
volatile float fVectorMZ;
//MPU - 9250 object on SPI bus 0
MPU9250 IMU(SPI, TEENSY_SPI_CS_PIN);
int status;
bool bRead = false;
bool testStatus(int Value);
void clearQuaternionBuffer();
void printYPR();
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat);
void setup()
{
pinMode(MPU9250_INT_PIN, INPUT);
delay(5000);
Serial.println("IMU initialization starting...");
status = IMU.begin();
while (status < 0)
{
status = IMU.begin(); //ACCEL_RANGE_2G, GYRO_RANGE_250DPS);
if (status < 0)
{
delay(1000);
Serial.println("IMU initialization unsuccessful, please wait...");
}
}
Serial.println("IMU initialization Successful...");
status = IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_5HZ);
if (testStatus(status) == false)
{
Serial.println("setDlpfBandwidth FAILED");
waitOnError();
}
status = IMU.setSrd(49);
if (testStatus(status) == false)
{
Serial.println("setSrd FAILED");
waitOnError();
}
status = IMU.setAccelRange(MPU9250::ACCEL_RANGE_16G);
if (testStatus(status) == false)
{
Serial.println("setAccelRange FAILED");
waitOnError();
}
status = IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
if (testStatus(status) == false)
{
Serial.println("setGyroRange FAILED");
waitOnError();
}
attachInterrupt(MPU9250_INT_PIN, packReadings, RISING);
status = IMU.enableDataReadyInterrupt();
if (testStatus(status) == false)
{
//Do something to alarm the operators...
Serial.println("enableDataReadyInterrupt FAILED");
waitOnError();
}
}
/*------------------------------------------------------------------
loop
------------------------------------------------------------------*/
void loop()
{
Now = micros();
deltat = ((Now - lastUpdate) / 1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = Now;
}
/*-----------------------------------------------------------
packReadings()
-------------------------------------------------------------*/
void packReadings()
{
IMU.readSensor();
fVectorAX = IMU.getAccelX_mss();
fVectorAY = IMU.getAccelY_mss();
fVectorAZ = IMU.getAccelZ_mss();
fVectorGX = IMU.getGyroX_rads();
fVectorGY = IMU.getGyroY_rads();
fVectorGZ = IMU.getGyroZ_rads();
fVectorMX = IMU.getMagX_uT();
fVectorMY = IMU.getMagY_uT();
fVectorMZ = IMU.getMagZ_uT();
Serial.println("----------------------");
//memset(charBuff, 0, sizeof(charBuff));
sprintf(charBuff, "Readings for AX is %f\t, AY is %f\t, AZ is %f\t GX is %f rads/sec,\t GY is %f rads/sec\t, GZ is %f rads/sec\t\n", fVectorAX, fVectorAY, fVectorAZ, fVectorGX, fVectorGY, fVectorGZ);
Serial.println(charBuff);
clearQuaternionBuffer();
MadgwickQuaternionUpdate(fVectorAX, fVectorAY, fVectorAZ, fVectorGX, fVectorGY, fVectorGZ, fVectorMX, fVectorMY, fVectorMZ, deltat);
sprintf(charBuff, "Readings for Q1 is %f\t,Q2 is %f\t,Q3 is %f\t, Q4 is %f\n", quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
Serial.println(charBuff);
}
/*-----------------------------------------------------------------------------
clearQuaternionBuffer
-----------------------------------------------------------------------------*/
void clearQuaternionBuffer()
{
quaternion[0] = 1.0f;
quaternion[1] = 0.0f;
quaternion[2] = 0.0f;
quaternion[3] = 0.0f;
}
/*-------------------------------------------------------------
testStatus
-------------------------------------------------------------*/
bool testStatus(int value)
{
if (value > 0)
return true;
else
return false;
}
/*-------------------------------------------------------------
printYPR
-------------------------------------------------------------*/
void printYPR()
{
float yaw = atan2(2.0f * (quaternion[1] * quaternion[2] + quaternion[0] * quaternion[3]), quaternion[0] * quaternion[0] + quaternion[1] * quaternion[1] - quaternion[2] * quaternion[2] - quaternion[3] * quaternion[3]);
float pitch = -asin(2.0f*(quaternion[1] * quaternion[3] - quaternion[0] * quaternion[2]));
float roll = atan2(2.0f*(quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]), quaternion[0] * quaternion[0] - quaternion[1] * quaternion[1] - quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI;
yaw += 1.34; /* Declination at Potheri, Chennail ,India Model Used: IGRF12 Help
Latitude: 12.823640° N
Longitude: 80.043518° E
Date Declination
2016-04-09 1.34° W changing by 0.06° E per year (+ve for west )*/
roll *= 180.0f / PI;
Serial.print("Yaw, Pitch, Roll: ");
Serial.print(yaw + 180, 2);
Serial.print(", ");
Serial.print(pitch, 2);
Serial.print(", ");
Serial.println(roll, 2);
}
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
{
// short name local variable for readability
float q1 = quaternion[0], q2 = quaternion[1], q3 = quaternion[2], q4 = quaternion[3];
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f)
return; // handle NaN
norm = 1.0f / norm;
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f)
return; // handle NaN
norm = 1.0f / norm;
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 +
_2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
norm = 1.0f / norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * deltat;
q2 += qDot2 * deltat;
q3 += qDot3 * deltat;
q4 += qDot4 * deltat;
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f / norm;
quaternion[0] = q1 * norm;
quaternion[1] = q2 * norm;
quaternion[2] = q3 * norm;
quaternion[3] = q4 * norm;
}
void waitOnError()
{
while (1)
{
Serial.println("--Error in setup!--");
delay(2000);
}
}