From 441a028ed9ae1c039fc7d847cff77efb8a90ce97 Mon Sep 17 00:00:00 2001 From: Kris Winer Date: Thu, 3 Dec 2015 18:15:10 -0800 Subject: [PATCH] Update EM7180_MPU9250_MS5637.ino --- EM7180_MPU9250_MS5637.ino | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/EM7180_MPU9250_MS5637.ino b/EM7180_MPU9250_MS5637.ino index 0190509..520aa2c 100644 --- a/EM7180_MPU9250_MS5637.ino +++ b/EM7180_MPU9250_MS5637.ino @@ -874,7 +874,7 @@ void loop() // This is ok by aircraft orientation standards! // Pass gyro rate as rad/s MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz); -// if(passThru)MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, -my, mx, mz); +// if(passThru)MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz); // Serial print and/or display at 0.5 s rate independent of data rates delt_t = millis() - count; @@ -956,21 +956,21 @@ void loop() // applied in the correct order which for this configuration is yaw, pitch, and then roll. // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. //Software AHRS: - yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]) + PI; + yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); pitch *= 180.0f / PI; yaw *= 180.0f / PI; - yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 + yaw += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 if(yaw < 0) yaw += 360.0f; // Ensure yaw stays between 0 and 360 roll *= 180.0f / PI; //Hardware AHRS: - Yaw = atan2(2.0f * (Quat[1] * Quat[2] + Quat[0] * Quat[3]), Quat[0] * Quat[0] + Quat[1] * Quat[1] - Quat[2] * Quat[2] - Quat[3] * Quat[3]) + PI; - Pitch = -asin(2.0f * (Quat[1] * Quat[3] - Quat[0] * Quat[2])); - Roll = atan2(2.0f * (Quat[0] * Quat[1] + Quat[2] * Quat[3]), Quat[0] * Quat[0] - Quat[1] * Quat[1] - Quat[2] * Quat[2] + Quat[3] * Quat[3]); + Yaw = atan2(2.0f * (Quat[0] * Quat[1] + Quat[3] * Quat[2]), Quat[3] * Quat[3] + Quat[0] * Quat[0] - Quat[1] * Quat[1] - Quat[2] * Quat[2]); + Pitch = -asin(2.0f * (Quat[0] * Quat[2] - Quat[3] * Quat[1])); + Roll = atan2(2.0f * (Quat[3] * Quat[0] + Quat[1] * Quat[2]), Quat[3] * Quat[3] - Quat[0] * Quat[0] - Quat[1] * Quat[1] + Quat[2] * Quat[2]); Pitch *= 180.0f / PI; Yaw *= 180.0f / PI; - Yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 + Yaw += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 if(Yaw < 0) Yaw += 360.0f ; // Ensure yaw stays between 0 and 360 Roll *= 180.0f / PI; @@ -1144,10 +1144,10 @@ void readSENtralQuatData(float * destination) { uint8_t rawData[16]; // x/y/z quaternion register data stored here readBytes(EM7180_ADDRESS, EM7180_QX, 16, &rawData[0]); // Read the sixteen raw data registers into data array - destination[1] = uint32_reg_to_float (&rawData[0]); - destination[2] = uint32_reg_to_float (&rawData[4]); - destination[3] = uint32_reg_to_float (&rawData[8]); - destination[0] = uint32_reg_to_float (&rawData[12]); // SENtral stores quats as qx, qy, qz, q0! + destination[0] = uint32_reg_to_float (&rawData[0]); + destination[1] = uint32_reg_to_float (&rawData[4]); + destination[2] = uint32_reg_to_float (&rawData[8]); + destination[3] = uint32_reg_to_float (&rawData[12]); // SENtral stores quats as qx, qy, qz, q0! }