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@1:43f8ac7ca6d7, 2014-04-29 (annotated)

Committer:
pHysiX
Date:
Tue Apr 29 11:02:18 2014 +0000
Revision:
1:43f8ac7ca6d7
Child:
2:ab967d7b4346
Tidy file structure: threads in separate files for easy navigation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 1:43f8ac7ca6d7 1 /* Stabilisation */
pHysiX 1:43f8ac7ca6d7 2 #include "Task1.h"
pHysiX 1:43f8ac7ca6d7 3 #include "setup.h"
pHysiX 1:43f8ac7ca6d7 4
pHysiX 1:43f8ac7ca6d7 5 // MPU control/status vars
pHysiX 1:43f8ac7ca6d7 6 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
pHysiX 1:43f8ac7ca6d7 7 uint16_t fifoCount; // count of all bytes currently in FIFO
pHysiX 1:43f8ac7ca6d7 8 uint8_t fifoBuffer[64]; // FIFO storage buffer
pHysiX 1:43f8ac7ca6d7 9
pHysiX 1:43f8ac7ca6d7 10 // orientation/motion vars
pHysiX 1:43f8ac7ca6d7 11 Quaternion q; // [w, x, y, z] quaternion container
pHysiX 1:43f8ac7ca6d7 12 VectorFloat gravity; // [x, y, z] gravity vector
pHysiX 1:43f8ac7ca6d7 13 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
pHysiX 1:43f8ac7ca6d7 14
pHysiX 1:43f8ac7ca6d7 15
pHysiX 1:43f8ac7ca6d7 16 #ifndef M_PI
pHysiX 1:43f8ac7ca6d7 17 #define M_PI 3.1415
pHysiX 1:43f8ac7ca6d7 18 #endif
pHysiX 1:43f8ac7ca6d7 19
pHysiX 1:43f8ac7ca6d7 20 // ================================================================
pHysiX 1:43f8ac7ca6d7 21 // === INTERRUPT DETECTION ROUTINE ===
pHysiX 1:43f8ac7ca6d7 22 // ================================================================
pHysiX 1:43f8ac7ca6d7 23
pHysiX 1:43f8ac7ca6d7 24 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
pHysiX 1:43f8ac7ca6d7 25 void dmpDataReady()
pHysiX 1:43f8ac7ca6d7 26 {
pHysiX 1:43f8ac7ca6d7 27 mpuInterrupt = true;
pHysiX 1:43f8ac7ca6d7 28 }
pHysiX 1:43f8ac7ca6d7 29 void Task1(void const *argument)
pHysiX 1:43f8ac7ca6d7 30 {
pHysiX 1:43f8ac7ca6d7 31 // reset interrupt flag and get INT_STATUS byte
pHysiX 1:43f8ac7ca6d7 32 mpuInterrupt = false;
pHysiX 1:43f8ac7ca6d7 33 mpuIntStatus = imu.getIntStatus();
pHysiX 1:43f8ac7ca6d7 34
pHysiX 1:43f8ac7ca6d7 35 // get current FIFO count
pHysiX 1:43f8ac7ca6d7 36 fifoCount = imu.getFIFOCount();
pHysiX 1:43f8ac7ca6d7 37
pHysiX 1:43f8ac7ca6d7 38 // check for overflow (this should never happen unless our code is too inefficient)
pHysiX 1:43f8ac7ca6d7 39 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
pHysiX 1:43f8ac7ca6d7 40 // reset so we can continue cleanly
pHysiX 1:43f8ac7ca6d7 41 imu.resetFIFO();
pHysiX 1:43f8ac7ca6d7 42 imu.debugSerial.printf("FIFO overflow!");
pHysiX 1:43f8ac7ca6d7 43
pHysiX 1:43f8ac7ca6d7 44 // otherwise, check for DMP data ready interrupt (this should happen frequently)
pHysiX 1:43f8ac7ca6d7 45 } else if (mpuIntStatus & 0x02) {
pHysiX 1:43f8ac7ca6d7 46 // wait for correct available data length, should be a VERY short wait
pHysiX 1:43f8ac7ca6d7 47 while (fifoCount < packetSize) fifoCount = imu.getFIFOCount();
pHysiX 1:43f8ac7ca6d7 48
pHysiX 1:43f8ac7ca6d7 49 // read a packet from FIFO
pHysiX 1:43f8ac7ca6d7 50 imu.getFIFOBytes(fifoBuffer, packetSize);
pHysiX 1:43f8ac7ca6d7 51
pHysiX 1:43f8ac7ca6d7 52 // track FIFO count here in case there is > 1 packet available
pHysiX 1:43f8ac7ca6d7 53 // (this lets us immediately read more without waiting for an interrupt)
pHysiX 1:43f8ac7ca6d7 54 fifoCount -= packetSize;
pHysiX 1:43f8ac7ca6d7 55
pHysiX 1:43f8ac7ca6d7 56 // display YPR angles in degrees
pHysiX 1:43f8ac7ca6d7 57 imu.dmpGetQuaternion(&q, fifoBuffer);
pHysiX 1:43f8ac7ca6d7 58 imu.dmpGetGravity(&gravity, &q);
pHysiX 1:43f8ac7ca6d7 59 imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
pHysiX 1:43f8ac7ca6d7 60 ypr[0] *= 180/M_PI;
pHysiX 1:43f8ac7ca6d7 61 ypr[1] *= 180/M_PI;
pHysiX 1:43f8ac7ca6d7 62 ypr[2] *= 180/M_PI;
pHysiX 1:43f8ac7ca6d7 63
pHysiX 1:43f8ac7ca6d7 64 //BT.printf("YPR\t%3.2f\t%3.2f\t%3.2f\n", ypr[0]*180/M_PI, ypr[1]*180/M_PI, ypr[2]*180/M_PI);
pHysiX 1:43f8ac7ca6d7 65 BT.printf("Y%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 66 }
pHysiX 1:43f8ac7ca6d7 67
pHysiX 1:43f8ac7ca6d7 68 led1 = !led1;
pHysiX 1:43f8ac7ca6d7 69 }