Ian Hua / Quadcopter-mbedRTOS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Diff: RTOS-Threads/src/Task2_Master.cpp

Revision:
38:ef65533cca32
Parent:
36:d95e3d6f2fc4
Child:
39:02782ad251db
--- a/RTOS-Threads/src/Task2_Master.cpp	Tue May 13 05:28:05 2014 +0000
+++ b/RTOS-Threads/src/Task2_Master.cpp	Tue May 13 13:05:03 2014 +0000
@@ -4,7 +4,7 @@
  * Purpose:     Thread2M: Master PID control loop (attitude)
  * Functions:   AHRSSample: Read MPU6050 DMP and calculate YPR
  * Settings:    200Hz
- * Timing:      40us
+ * Timing:
  */
 #include "Task2_Slave.h"
 #include "setup.h"
@@ -18,7 +18,8 @@
 /* Orientation/motion variables: */
 Quaternion q;               // [w, x, y, z] quaternion container
 VectorFloat gravity;        // [x, y, z] gravity vector
-float ypr[3];               // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
+float ypr_rad[3];           // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
+volatile float ypr[3];      
 
 float altitude, temperature;
 
@@ -93,7 +94,8 @@
     if ((mpuIntStatus & 0x10) || fifoCount > 84) {
         // reset so we can continue cleanly
         imu.resetFIFO();
-        imu.debugSerial.printf("FIFO overflow!");
+        //imu.debugSerial.printf("FIFO overflow!");
+        //BT.printf("FIFO overflow!\n");
 
         // otherwise, check for DMP data ready interrupt (this should happen frequently)
     } else if (mpuIntStatus & 0x02) {
@@ -112,11 +114,10 @@
         // display YPR angles in degrees
         imu.dmpGetQuaternion(&q, fifoBuffer);
         imu.dmpGetGravity(&gravity, &q);
-        imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        imu.dmpGetYawPitchRoll(ypr_rad, &q, &gravity);
 
-        ypr[0] = ypr[0] * 180/M_PI;
-        ypr[1] = ypr[1] * 180/M_PI;
-        ypr[2] = ypr[2] * 180/M_PI;
+        for (int i = 0; i < 3; i++)
+            ypr[i] = ypr_rad[i] * 180/M_PI;
 
         /*
         if (compass.getDataReady()) {