Diff: BroBot_IMU.h

Revision:

15:d6d7623a17f8

Parent:

11:2553f5798f84

--- a/BroBot_IMU.h	Tue Jan 24 23:09:10 2017 +0000
+++ b/BroBot_IMU.h	Thu Jan 26 21:37:24 2017 +0000
@@ -12,15 +12,14 @@
 // MPU control/status vars
 #include "rtos_definations.h"
 
-float angle, angle_old;
+
 bool dmpReady = false;  // set true if DMP init was successful
 uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
 uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
 uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
 uint16_t fifoCount;     // count of all bytes currently in FIFO
 uint8_t fifoBuffer[64]; // FIFO storage buffer
-float dAngle;
-float new_angle;
+
 // Orientation/motion vars
 Quaternion q;           // [w, x, y, z]         quaternion container
 
@@ -45,7 +44,7 @@
 }
 
 // Quick calculation to obtein Phi angle from quaternion solution (from DMP internal quaternion solution)
-float dmpGetPhi()
+void dmpGetReadings(float * angle, float *theta)
 {
     mpu.getFIFOBytes(fifoBuffer, 16); // We only read the quaternion
     mpu.dmpGetQuaternion(&q, fifoBuffer);
@@ -53,7 +52,9 @@
 
     //return( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); //roll
     //return Phi angle (robot orientation) from quaternion DMP output
-    return (atan2(2 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * RAD2GRAD);
+    *angle = (atan2(2 * ((q.y * q.z) + (q.w * q.x)), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * RAD2GRAD);
+    *theta = (atan2(2 * (q.y * q.x + q.z * q.w),1 - 2 * (q.w * q.w + q.x * q.x)) * RAD2GRAD); 
+    return;
 }
 
 // ================================================================