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@34:228d87c45151, 2014-05-12 (annotated)

Committer:
pHysiX
Date:
Mon May 12 13:55:34 2014 +0000
Revision:
34:228d87c45151
Parent:
33:f88a6ee18103
Child:
36:d95e3d6f2fc4
Code tidied

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