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/Task2_Master.cpp@36:d95e3d6f2fc4, 2014-05-13 (annotated)

Committer:
pHysiX
Date:
Tue May 13 04:05:34 2014 +0000
Revision:
36:d95e3d6f2fc4
Parent:
34:228d87c45151
Child:
38:ef65533cca32
Telemetry output completely changed; Code tidied

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pHysiX 27:18b6580eb0b1 1 /* File: Task2_Master.cpp
pHysiX 27:18b6580eb0b1 2 * Author: Trung Tin Ian HUA
pHysiX 27:18b6580eb0b1 3 * Date: May 2014
pHysiX 31:3dde2201e54d 4 * Purpose: Thread2M: Master PID control loop (attitude)
pHysiX 36:d95e3d6f2fc4 5 * Functions: AHRSSample: Read MPU6050 DMP and calculate YPR
pHysiX 36:d95e3d6f2fc4 6 * Settings: 200Hz
pHysiX 30:d9b988f8d84f 7 * Timing: 40us
pHysiX 27:18b6580eb0b1 8 */
pHysiX 27:18b6580eb0b1 9 #include "Task2_Slave.h"
pHysiX 27:18b6580eb0b1 10 #include "setup.h"
pHysiX 27:18b6580eb0b1 11 #include "PID.h"
pHysiX 27:18b6580eb0b1 12
pHysiX 36:d95e3d6f2fc4 13 /* MPU6050 control/status variables: */
pHysiX 36:d95e3d6f2fc4 14 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
pHysiX 36:d95e3d6f2fc4 15 uint16_t fifoCount; // count of all bytes currently in FIFO
pHysiX 36:d95e3d6f2fc4 16 uint8_t fifoBuffer[64]; // FIFO storage buffer
pHysiX 36:d95e3d6f2fc4 17
pHysiX 36:d95e3d6f2fc4 18 /* Orientation/motion variables: */
pHysiX 36:d95e3d6f2fc4 19 Quaternion q; // [w, x, y, z] quaternion container
pHysiX 36:d95e3d6f2fc4 20 VectorFloat gravity; // [x, y, z] gravity vector
pHysiX 36:d95e3d6f2fc4 21 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
pHysiX 36:d95e3d6f2fc4 22
pHysiX 36:d95e3d6f2fc4 23 float altitude, temperature;
pHysiX 36:d95e3d6f2fc4 24
pHysiX 36:d95e3d6f2fc4 25 #ifndef M_PI
pHysiX 36:d95e3d6f2fc4 26 #define M_PI 3.1415
pHysiX 36:d95e3d6f2fc4 27 #endif
pHysiX 36:d95e3d6f2fc4 28
pHysiX 36:d95e3d6f2fc4 29 #ifdef ENABLE_COMPASS
pHysiX 36:d95e3d6f2fc4 30 //int compassX, compassY, compassZ;
pHysiX 36:d95e3d6f2fc4 31 double heading = 0;
pHysiX 36:d95e3d6f2fc4 32 #endif
pHysiX 36:d95e3d6f2fc4 33
pHysiX 27:18b6580eb0b1 34 /* YPR Adjust */
pHysiX 27:18b6580eb0b1 35 volatile float adjust_attitude[3] = {0.0, 0.0, 0.0};
pHysiX 27:18b6580eb0b1 36
pHysiX 34:228d87c45151 37
pHysiX 34:228d87c45151 38
pHysiX 36:d95e3d6f2fc4 39
pHysiX 34:228d87c45151 40 // ===============================
pHysiX 34:228d87c45151 41 // === YPR SAMPLE & MASTER PID ===
pHysiX 34:228d87c45151 42 // ===============================
pHysiX 31:3dde2201e54d 43 //Timer
pHysiX 27:18b6580eb0b1 44 void Task2_Master(void const *argument)
pHysiX 27:18b6580eb0b1 45 {
pHysiX 31:3dde2201e54d 46 //Timer
pHysiX 36:d95e3d6f2fc4 47 AHRSSample();
pHysiX 36:d95e3d6f2fc4 48
pHysiX 30:d9b988f8d84f 49 if (armed) {
pHysiX 30:d9b988f8d84f 50 switch (mode) {
pHysiX 30:d9b988f8d84f 51 case RATE:
pHysiX 30:d9b988f8d84f 52 break;
pHysiX 28:aa72bd4ff103 53
pHysiX 30:d9b988f8d84f 54 case ATTITUDE:
pHysiX 30:d9b988f8d84f 55 default:
pHysiX 31:3dde2201e54d 56 //Timer
pHysiX 31:3dde2201e54d 57 pitchPIDstable.setProcessValue((int) (ypr[1] - ypr_offset[1]));
pHysiX 31:3dde2201e54d 58 rollPIDstable.setProcessValue((int) (ypr[2] - ypr_offset[2]));
pHysiX 27:18b6580eb0b1 59
pHysiX 30:d9b988f8d84f 60 pitchPIDstable.setSetPoint(inputYPR[1]);
pHysiX 30:d9b988f8d84f 61 rollPIDstable.setSetPoint(inputYPR[2]);
pHysiX 30:d9b988f8d84f 62
pHysiX 30:d9b988f8d84f 63 adjust_attitude[1] = pitchPIDstable.compute();
pHysiX 30:d9b988f8d84f 64 adjust_attitude[2] = rollPIDstable.compute();
pHysiX 30:d9b988f8d84f 65 adjust_attitude[2] *= -1;
pHysiX 27:18b6580eb0b1 66
pHysiX 30:d9b988f8d84f 67 //counterTask2Master = true;
pHysiX 31:3dde2201e54d 68 //Timer
pHysiX 30:d9b988f8d84f 69 break;
pHysiX 30:d9b988f8d84f 70 }
pHysiX 31:3dde2201e54d 71 //Timer
pHysiX 27:18b6580eb0b1 72 }
pHysiX 27:18b6580eb0b1 73 }
pHysiX 33:f88a6ee18103 74
pHysiX 36:d95e3d6f2fc4 75
pHysiX 36:d95e3d6f2fc4 76
pHysiX 36:d95e3d6f2fc4 77
pHysiX 34:228d87c45151 78 // ************************
pHysiX 34:228d87c45151 79 // *** Helper functions ***
pHysiX 34:228d87c45151 80 // ************************
pHysiX 36:d95e3d6f2fc4 81 void AHRSSample(void)
pHysiX 36:d95e3d6f2fc4 82 {
pHysiX 36:d95e3d6f2fc4 83 //Timer
pHysiX 36:d95e3d6f2fc4 84 // reset interrupt flag and get INT_STATUS byte
pHysiX 36:d95e3d6f2fc4 85 mpuIntStatus = imu.getIntStatus();
pHysiX 36:d95e3d6f2fc4 86
pHysiX 36:d95e3d6f2fc4 87 // get current FIFO count
pHysiX 36:d95e3d6f2fc4 88 fifoCount = imu.getFIFOCount();
pHysiX 36:d95e3d6f2fc4 89 //imu.debugSerial.printf("FIFO Count: %d\n", fifoCount);
pHysiX 36:d95e3d6f2fc4 90
pHysiX 36:d95e3d6f2fc4 91 // check for overflow
pHysiX 36:d95e3d6f2fc4 92 // Only keep a max of 2 packets in buffer.
pHysiX 36:d95e3d6f2fc4 93 if ((mpuIntStatus & 0x10) || fifoCount > 84) {
pHysiX 36:d95e3d6f2fc4 94 // reset so we can continue cleanly
pHysiX 36:d95e3d6f2fc4 95 imu.resetFIFO();
pHysiX 36:d95e3d6f2fc4 96 imu.debugSerial.printf("FIFO overflow!");
pHysiX 36:d95e3d6f2fc4 97
pHysiX 36:d95e3d6f2fc4 98 // otherwise, check for DMP data ready interrupt (this should happen frequently)
pHysiX 36:d95e3d6f2fc4 99 } else if (mpuIntStatus & 0x02) {
pHysiX 36:d95e3d6f2fc4 100 // wait for correct available data length, should be a VERY short wait
pHysiX 36:d95e3d6f2fc4 101 while (fifoCount < packetSize) fifoCount = imu.getFIFOCount();
pHysiX 36:d95e3d6f2fc4 102
pHysiX 36:d95e3d6f2fc4 103 while (fifoCount > 41) {
pHysiX 36:d95e3d6f2fc4 104 // read a packet from FIFO
pHysiX 36:d95e3d6f2fc4 105 imu.getFIFOBytes(fifoBuffer, packetSize);
pHysiX 36:d95e3d6f2fc4 106
pHysiX 36:d95e3d6f2fc4 107 // track FIFO count here in case there is > 1 packet available
pHysiX 36:d95e3d6f2fc4 108 // (this lets us immediately read more without waiting for an interrupt)
pHysiX 36:d95e3d6f2fc4 109 fifoCount -= packetSize;
pHysiX 36:d95e3d6f2fc4 110 }
pHysiX 36:d95e3d6f2fc4 111
pHysiX 36:d95e3d6f2fc4 112 // display YPR angles in degrees
pHysiX 36:d95e3d6f2fc4 113 imu.dmpGetQuaternion(&q, fifoBuffer);
pHysiX 36:d95e3d6f2fc4 114 imu.dmpGetGravity(&gravity, &q);
pHysiX 36:d95e3d6f2fc4 115 imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
pHysiX 36:d95e3d6f2fc4 116
pHysiX 36:d95e3d6f2fc4 117 ypr[0] = ypr[0] * 180/M_PI;
pHysiX 36:d95e3d6f2fc4 118 ypr[1] = ypr[1] * 180/M_PI;
pHysiX 36:d95e3d6f2fc4 119 ypr[2] = ypr[2] * 180/M_PI;
pHysiX 36:d95e3d6f2fc4 120
pHysiX 36:d95e3d6f2fc4 121 /*
pHysiX 36:d95e3d6f2fc4 122 if (compass.getDataReady()) {
pHysiX 36:d95e3d6f2fc4 123 // compass.getValues(&compass_x, &compass_y, &compass_z);
pHysiX 36:d95e3d6f2fc4 124 heading = compass.getHeadingXY() * 180/M_PI;
pHysiX 36:d95e3d6f2fc4 125 }
pHysiX 36:d95e3d6f2fc4 126
pHysiX 36:d95e3d6f2fc4 127 ypr[0] *= 0.98;
pHysiX 36:d95e3d6f2fc4 128 ypr[0] += 0.02*heading;
pHysiX 36:d95e3d6f2fc4 129 */
pHysiX 36:d95e3d6f2fc4 130 }
pHysiX 36:d95e3d6f2fc4 131 //Timer
pHysiX 36:d95e3d6f2fc4 132 }