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@27:18b6580eb0b1, 2014-05-10 (annotated)

Committer:
pHysiX
Date:
Sat May 10 01:39:02 2014 +0000
Revision:
27:18b6580eb0b1
Parent:
24:54a8cdf17378
Child:
30:d9b988f8d84f
Alternative1: Run Master only when YPR is updated

Who changed what in which revision?

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