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.

RTOS-Threads/src/Task1.cpp@16:9072cd6fa8d1, 2014-05-03 (annotated)

Committer:
pHysiX
Date:
Sat May 03 01:33:28 2014 +0000
Revision:
16:9072cd6fa8d1
Parent:
15:10edc6b12122
Child:
17:18c3bd016e49
Better FIFO handling. Output YPR stable.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 12:953d25061417 1 /* YPR (100Hz) */
pHysiX 2:ab967d7b4346 2
pHysiX 1:43f8ac7ca6d7 3 #include "Task1.h"
pHysiX 1:43f8ac7ca6d7 4 #include "setup.h"
pHysiX 1:43f8ac7ca6d7 5
pHysiX 2:ab967d7b4346 6 /* MPU6050 control/status variables: */
pHysiX 2:ab967d7b4346 7 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
pHysiX 2:ab967d7b4346 8 uint16_t fifoCount; // count of all bytes currently in FIFO
pHysiX 16:9072cd6fa8d1 9 uint8_t fifoBuffer[128]; // FIFO storage buffer
pHysiX 1:43f8ac7ca6d7 10
pHysiX 2:ab967d7b4346 11 /* Orientation/motion variables: */
pHysiX 2:ab967d7b4346 12 Quaternion q; // [w, x, y, z] quaternion container
pHysiX 2:ab967d7b4346 13 VectorFloat gravity; // [x, y, z] gravity vector
pHysiX 2:ab967d7b4346 14 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
pHysiX 15:10edc6b12122 15 volatile float ypr_use[3];
pHysiX 12:953d25061417 16
pHysiX 12:953d25061417 17 #ifdef ENABLE_COMPASS
pHysiX 12:953d25061417 18 //int compassX, compassY, compassZ;
pHysiX 12:953d25061417 19 double heading = 0;
pHysiX 12:953d25061417 20 #endif
pHysiX 12:953d25061417 21
pHysiX 12:953d25061417 22 float altitude, temperature;
pHysiX 10:ef5fe86f67fe 23
pHysiX 10:ef5fe86f67fe 24 int counterTask1 = 0;
pHysiX 1:43f8ac7ca6d7 25
pHysiX 1:43f8ac7ca6d7 26 #ifndef M_PI
pHysiX 1:43f8ac7ca6d7 27 #define M_PI 3.1415
pHysiX 1:43f8ac7ca6d7 28 #endif
pHysiX 1:43f8ac7ca6d7 29
pHysiX 1:43f8ac7ca6d7 30 // ================================================================
pHysiX 12:953d25061417 31 // === YPR ROUTINE ===
pHysiX 2:ab967d7b4346 32 // ================================================================
pHysiX 1:43f8ac7ca6d7 33 void Task1(void const *argument)
pHysiX 1:43f8ac7ca6d7 34 {
pHysiX 1:43f8ac7ca6d7 35 // reset interrupt flag and get INT_STATUS byte
pHysiX 1:43f8ac7ca6d7 36 mpuIntStatus = imu.getIntStatus();
pHysiX 1:43f8ac7ca6d7 37
pHysiX 1:43f8ac7ca6d7 38 // get current FIFO count
pHysiX 1:43f8ac7ca6d7 39 fifoCount = imu.getFIFOCount();
pHysiX 16:9072cd6fa8d1 40 //imu.debugSerial.printf("FIFO Count: %d\n", fifoCount);
pHysiX 1:43f8ac7ca6d7 41
pHysiX 1:43f8ac7ca6d7 42 // check for overflow (this should never happen unless our code is too inefficient)
pHysiX 16:9072cd6fa8d1 43 if ((mpuIntStatus & 0x10) || fifoCount > 126) {
pHysiX 1:43f8ac7ca6d7 44 // reset so we can continue cleanly
pHysiX 1:43f8ac7ca6d7 45 imu.resetFIFO();
pHysiX 1:43f8ac7ca6d7 46 imu.debugSerial.printf("FIFO overflow!");
pHysiX 1:43f8ac7ca6d7 47
pHysiX 1:43f8ac7ca6d7 48 // otherwise, check for DMP data ready interrupt (this should happen frequently)
pHysiX 1:43f8ac7ca6d7 49 } else if (mpuIntStatus & 0x02) {
pHysiX 1:43f8ac7ca6d7 50 // wait for correct available data length, should be a VERY short wait
pHysiX 1:43f8ac7ca6d7 51 while (fifoCount < packetSize) fifoCount = imu.getFIFOCount();
pHysiX 1:43f8ac7ca6d7 52
pHysiX 16:9072cd6fa8d1 53 // while (fifoCount >= packetSize) {
pHysiX 1:43f8ac7ca6d7 54 // read a packet from FIFO
pHysiX 1:43f8ac7ca6d7 55 imu.getFIFOBytes(fifoBuffer, packetSize);
pHysiX 1:43f8ac7ca6d7 56
pHysiX 1:43f8ac7ca6d7 57 // track FIFO count here in case there is > 1 packet available
pHysiX 1:43f8ac7ca6d7 58 // (this lets us immediately read more without waiting for an interrupt)
pHysiX 15:10edc6b12122 59 //fifoCount -= packetSize;
pHysiX 16:9072cd6fa8d1 60 //}
pHysiX 1:43f8ac7ca6d7 61
pHysiX 1:43f8ac7ca6d7 62 // display YPR angles in degrees
pHysiX 1:43f8ac7ca6d7 63 imu.dmpGetQuaternion(&q, fifoBuffer);
pHysiX 1:43f8ac7ca6d7 64 imu.dmpGetGravity(&gravity, &q);
pHysiX 1:43f8ac7ca6d7 65 imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
pHysiX 16:9072cd6fa8d1 66 imu.resetFIFO();
pHysiX 16:9072cd6fa8d1 67
pHysiX 15:10edc6b12122 68 ypr_use[0] = ypr[0] * 180/M_PI;
pHysiX 15:10edc6b12122 69 ypr_use[1] = ypr[1] * 180/M_PI;
pHysiX 15:10edc6b12122 70 ypr_use[2] = ypr[2] * 180/M_PI;
pHysiX 12:953d25061417 71
pHysiX 12:953d25061417 72 /*
pHysiX 12:953d25061417 73 if (compass.getDataReady()) {
pHysiX 12:953d25061417 74 // compass.getValues(&compass_x, &compass_y, &compass_z);
pHysiX 12:953d25061417 75 heading = compass.getHeadingXY() * 180/M_PI;
pHysiX 12:953d25061417 76 }
pHysiX 12:953d25061417 77
pHysiX 12:953d25061417 78 ypr[0] *= 0.98;
pHysiX 12:953d25061417 79 ypr[0] += 0.02*heading;
pHysiX 12:953d25061417 80 */
pHysiX 10:ef5fe86f67fe 81
pHysiX 3:605fbcb54e75 82 if (box_demo)
pHysiX 15:10edc6b12122 83 BT.printf("\nY%3.2f\nP%3.2f\nR%3.2f\n", ypr_use[0] - ypr_offset[0],
pHysiX 15:10edc6b12122 84 ypr_use[1] - ypr_offset[1], ypr_use[2] - ypr_offset[2]);
pHysiX 1:43f8ac7ca6d7 85 }
pHysiX 1:43f8ac7ca6d7 86
pHysiX 2:ab967d7b4346 87 //LED[1] = !LED[1];
pHysiX 2:ab967d7b4346 88 }