Hi, I am currently working on modifying the IMU source file for a drone flight controller because I am using a teensy 3.5 instead of an Arduino and I am using a different IMU. I am having the issue that when I rotate the IMU 90 degrees the yaw only goes to 5.494.
Now I could just scale the yaw by 90/5.494, but I feel like I would be losing a lot of resolution. 
If anyone sees an issue with the program any help would be greatly appreciated.
If I missed anything that would help, let me know and I will provide it as promptly as possible.
Here is the modified version of the code.
#include "MPU9250.h"
#include "Arduino.h"
#include "i2c_t3.h"
#include "Config1.h"
MPU9250 IMU(Wire, 0x68); // an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
const int MPU = 0x68; // I2C address of the MPU-6050
unsigned int i2cErrorCnt = 0; // running count of i2c errors
float gyroOff[3] = {0, 0, 0}; // calibration results
float accOff[3] = {0, 0, 0}; // calibration resluts
float Acc[3], Gyro[3]; // raw IMU data
float Tmp; // raw temperature data
float IMURoll, IMUPitch, IMUHead; // cooked IMU data
float throttleCorrection; // boost throttle for roll/pitch angle
//--- vars from Madgwick
#define gyroMeasError 3.14159265358979f * (5.0f / 180.0f) // gyroscope measurement error in rad/s (shown as 5 deg/s)
#define beta sqrt(3.0f / 4.0f) * gyroMeasError // beta is the gain between gyro and accel
float a_x, a_y, a_z; // accelerometer measurements
float w_x, w_y, w_z; // gyroscope measurements in rad/s
float SEq_1 = 1.0f, SEq_2 = 0.0f, SEq_3 = 0.0f, SEq_4 = 0.0f; // estimated orientation quaternion elements with initial conditions
// readIMU -- read available IMU bytes and process if full message
uint8_t imuBufPos = 0;
uint8_t imuBuf[14];
//CalibrateGyroAccel
#define ACC_1G (2048)
void requestIMU() // read gyro and accel
{
IMU.readSensor();
}
void requestGyro() // read gyro only
{
IMU.readSensor();
}
void readIMU() // read available IMU bytes and process if full message
{
requestIMU();
Acc[0] = IMU.getAccelX_mss();
Acc[1] = IMU.getAccelY_mss();
Acc[2] = IMU.getAccelZ_mss();
Tmp = IMU.getTemperature_C();
Gyro[0] = IMU.getGyroX_rads();
Gyro[1] = IMU.getGyroY_rads();
Gyro[2] = IMU.getGyroZ_rads();
}
void calibrateGyroAcc()
{
// --- calibrate gyro by repeatedly taking the average of blocks of readings
// until the XY vibration is low enough
#define CALIBBLOCKSIZE (512)
bool calibGood = false;
float gyroSum[3] = {0, 0, 0};
float accSum[3] = {0, 0, 0};
float vibSum;
delay(5000);
// LEDPIN_ON;
while (!calibGood)
{
// clear stuff and wait a second
for (int i = 0; i < 3; i++)
{
gyroSum[i] = 0;
accSum[i] = 0;
}
vibSum = 0;
delay(1000);
// read a block of acc/gyro data, computing horizontal vibration
readIMU();
for (int i = 0; i < CALIBBLOCKSIZE; i++)
{
delay(3);
readIMU();
for (int j = 0; j < 3; j++)
{
gyroSum[j] += Gyro[j];
accSum[j] += Acc[j];
}
vibSum = abs(a_x + accOff[0]) + abs(a_y + accOff[1]);// + abs(a_z - accOff[2]);
}
for (int j = 0; j < 3; j++)
{
gyroOff[j] = -(gyroSum[j] / CALIBBLOCKSIZE);
accOff[j] = -(accSum[j] / CALIBBLOCKSIZE);
}
calibGood = vibSum < 1.0f;
}
accOff[2] += ACC_1G;
// LEDPIN_OFF;
}
void initIMU() // setup the IMU modules and do calibration
{
IMU.begin();
Wire.setClock(400000);
//TWBR = ((F_CPU / 400000) - 16) / 2; // set the I2C clock rate to 400kHz
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_184HZ);
IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS); //Wire.write(0x1b); Wire.write(0x18); // FS_SEL = 3: Full scale set to 2000 deg/sec
IMU.setAccelRange(MPU9250::ACCEL_RANGE_8G); // Wire.write(0x1c); Wire.write(0x10); // ACCEL_CONFIG -- AFS_SEL=2 (Full Scale = +/-8G) ; ACCELL_HPF=0 //note something is wrong in the spec.
Wire.endTransmission(true);
IMU.setSrd(19); // setting SRD to 19 for a 50 Hz update rate
requestIMU();
calibrateGyroAcc();
}
#define gyro2radpersec (2000.0f / 32768.0f) // scale raw gyro @2000 deg/sec full scale to rad/sec (removed 3.1415926/180 becasue it is already in rad/s
float rad2deg = (180.0f / 3.1415926f);
void calcIMU()
{
a_x = (float)Acc[0] + accOff[0];
a_y = (float)Acc[1] + accOff[1];
a_z = (float)Acc[2] + accOff[2];
w_x = ((float)Gyro[0] + gyroOff[0]) * gyro2radpersec;
w_y = ((float)Gyro[1] + gyroOff[1]) * gyro2radpersec;
w_z = ((float)Gyro[2] + gyroOff[2]) * gyro2radpersec;
//--- Madgwick code ---
// Local system variables
float norm; // vector norm denominator
float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion derrivative from gyroscopes elements
float f_1, f_2, f_3; // objective function elements
float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements
float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error
// Axulirary variables to avoid reapeated calcualtions
float halfSEq_1 = 0.5f * SEq_1;
float halfSEq_2 = 0.5f * SEq_2;
float halfSEq_3 = 0.5f * SEq_3;
float halfSEq_4 = 0.5f * SEq_4;
float twoSEq_1 = 2.0f * SEq_1;
float twoSEq_2 = 2.0f * SEq_2;
float twoSEq_3 = 2.0f * SEq_3;
// Normalise the accelerometer measurement
norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
a_x /= norm;
a_y /= norm;
a_z /= norm;
throttleCorrection = a_z;
// Compute the quaternion derrivative measured by gyroscopes
SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
// only apply accels if .75 < g < 1.25
if ((0.75f < a_z) && (a_z < 1.25f))
{
// Compute the objective function and Jacobian
f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication
J_12or23 = 2.0f * SEq_4;
J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication
J_14or21 = twoSEq_2;
J_32 = 2.0f * J_14or21; // negated in matrix multiplication
J_33 = 2.0f * J_11or24; // negated in matrix multiplication
// Compute the gradient (matrix multiplication)
SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1;
SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2;
// Normalise the gradient
norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
SEqHatDot_1 /= norm;
SEqHatDot_2 /= norm;
SEqHatDot_3 /= norm;
SEqHatDot_4 /= norm;
// Compute then integrate the estimated quaternion derrivative
SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat;
SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat;
SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat;
SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;
}
else
{
// Compute then integrate the estimated quaternion derrivative
// with no accel
SEq_1 += (SEqDot_omega_1) * deltat;
SEq_2 += (SEqDot_omega_2) * deltat;
SEq_3 += (SEqDot_omega_3) * deltat;
SEq_4 += (SEqDot_omega_4) * deltat;
}
// Normalise quaternion
norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
SEq_1 /= norm;
SEq_2 /= norm;
SEq_3 /= norm;
SEq_4 /= norm;
//--- end Madgwick code ---
IMUPitch = -rad2deg * atan2(2.0*(SEq_3*SEq_4 + SEq_1*SEq_2), SEq_1*SEq_1 - SEq_2*SEq_2 - SEq_3*SEq_3 + SEq_4*SEq_4);
IMURoll = rad2deg * asin(-2.0*(SEq_2*SEq_4 - SEq_1*SEq_3));
IMUHead = -rad2deg * atan2(2.0*(SEq_2*SEq_3 + SEq_1*SEq_4), SEq_1*SEq_1 + SEq_2*SEq_2 - SEq_3*SEq_3 - SEq_4*SEq_4);
}