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@12:953d25061417, 2014-05-02 (annotated)

Committer:
pHysiX
Date:
Fri May 02 17:01:56 2014 +0000
Revision:
12:953d25061417
Parent:
11:f9fd410c48c2
Child:
15:10edc6b12122
Added in all sensors. Need to add in EEPROM to complete control of Tilty. Finished all telemetry output and appropriate data rates.

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 2:ab967d7b4346 9 uint8_t fifoBuffer[64]; // 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 12:953d25061417 15
pHysiX 12:953d25061417 16 #ifdef ENABLE_COMPASS
pHysiX 12:953d25061417 17 //int compassX, compassY, compassZ;
pHysiX 12:953d25061417 18 double heading = 0;
pHysiX 12:953d25061417 19 #endif
pHysiX 12:953d25061417 20
pHysiX 12:953d25061417 21 float altitude, temperature;
pHysiX 10:ef5fe86f67fe 22
pHysiX 10:ef5fe86f67fe 23 int counterTask1 = 0;
pHysiX 1:43f8ac7ca6d7 24
pHysiX 1:43f8ac7ca6d7 25 #ifndef M_PI
pHysiX 1:43f8ac7ca6d7 26 #define M_PI 3.1415
pHysiX 1:43f8ac7ca6d7 27 #endif
pHysiX 1:43f8ac7ca6d7 28
pHysiX 1:43f8ac7ca6d7 29 // ================================================================
pHysiX 12:953d25061417 30 // === YPR ROUTINE ===
pHysiX 2:ab967d7b4346 31 // ================================================================
pHysiX 1:43f8ac7ca6d7 32 void Task1(void const *argument)
pHysiX 1:43f8ac7ca6d7 33 {
pHysiX 1:43f8ac7ca6d7 34 // reset interrupt flag and get INT_STATUS byte
pHysiX 1:43f8ac7ca6d7 35 mpuIntStatus = imu.getIntStatus();
pHysiX 1:43f8ac7ca6d7 36
pHysiX 1:43f8ac7ca6d7 37 // get current FIFO count
pHysiX 1:43f8ac7ca6d7 38 fifoCount = imu.getFIFOCount();
pHysiX 1:43f8ac7ca6d7 39
pHysiX 1:43f8ac7ca6d7 40 // check for overflow (this should never happen unless our code is too inefficient)
pHysiX 10:ef5fe86f67fe 41 if ((mpuIntStatus & 0x10) || fifoCount > 1023) {
pHysiX 1:43f8ac7ca6d7 42 // reset so we can continue cleanly
pHysiX 1:43f8ac7ca6d7 43 imu.resetFIFO();
pHysiX 1:43f8ac7ca6d7 44 imu.debugSerial.printf("FIFO overflow!");
pHysiX 1:43f8ac7ca6d7 45
pHysiX 1:43f8ac7ca6d7 46 // otherwise, check for DMP data ready interrupt (this should happen frequently)
pHysiX 1:43f8ac7ca6d7 47 } else if (mpuIntStatus & 0x02) {
pHysiX 1:43f8ac7ca6d7 48 // wait for correct available data length, should be a VERY short wait
pHysiX 1:43f8ac7ca6d7 49 while (fifoCount < packetSize) fifoCount = imu.getFIFOCount();
pHysiX 1:43f8ac7ca6d7 50
pHysiX 1:43f8ac7ca6d7 51 // read a packet from FIFO
pHysiX 1:43f8ac7ca6d7 52 imu.getFIFOBytes(fifoBuffer, packetSize);
pHysiX 1:43f8ac7ca6d7 53
pHysiX 1:43f8ac7ca6d7 54 // track FIFO count here in case there is > 1 packet available
pHysiX 1:43f8ac7ca6d7 55 // (this lets us immediately read more without waiting for an interrupt)
pHysiX 1:43f8ac7ca6d7 56 fifoCount -= packetSize;
pHysiX 1:43f8ac7ca6d7 57
pHysiX 1:43f8ac7ca6d7 58 // display YPR angles in degrees
pHysiX 1:43f8ac7ca6d7 59 imu.dmpGetQuaternion(&q, fifoBuffer);
pHysiX 1:43f8ac7ca6d7 60 imu.dmpGetGravity(&gravity, &q);
pHysiX 1:43f8ac7ca6d7 61 imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
pHysiX 12:953d25061417 62 ypr[0] *= 180/M_PI;
pHysiX 12:953d25061417 63 ypr[1] *= 180/M_PI;
pHysiX 12:953d25061417 64 ypr[2] *= 180/M_PI;
pHysiX 12:953d25061417 65
pHysiX 12:953d25061417 66 /*
pHysiX 12:953d25061417 67 if (compass.getDataReady()) {
pHysiX 12:953d25061417 68 // compass.getValues(&compass_x, &compass_y, &compass_z);
pHysiX 12:953d25061417 69 heading = compass.getHeadingXY() * 180/M_PI;
pHysiX 12:953d25061417 70 }
pHysiX 12:953d25061417 71
pHysiX 12:953d25061417 72 ypr[0] *= 0.98;
pHysiX 12:953d25061417 73 ypr[0] += 0.02*heading;
pHysiX 12:953d25061417 74 */
pHysiX 10:ef5fe86f67fe 75
pHysiX 3:605fbcb54e75 76 if (box_demo)
pHysiX 12:953d25061417 77 BT.printf("\nY%3.2f\nP%3.2f\nR%3.2f\n", ypr[0] - ypr_offset[0], ypr[1] - ypr_offset[1], ypr[2] - ypr_offset[2]);
pHysiX 1:43f8ac7ca6d7 78 }
pHysiX 1:43f8ac7ca6d7 79
pHysiX 2:ab967d7b4346 80 //LED[1] = !LED[1];
pHysiX 2:ab967d7b4346 81 }