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@51:04c6af4319e1, 2014-05-19 (annotated)

Committer:
pHysiX
Date:
Mon May 19 15:30:05 2014 +0000
Revision:
51:04c6af4319e1
Parent:
50:8a0accb23007 
Code rearranged into threads. Timer reserved for time critical routines only. Implemented working semaphore and mutex

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 38:ef65533cca32 7 * Timing:
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 50:8a0accb23007 13 Semaphore sem_Task2_Master(1);
pHysiX 50:8a0accb23007 14
pHysiX 36:d95e3d6f2fc4 15 /* MPU6050 control/status variables: */
pHysiX 36:d95e3d6f2fc4 16 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
pHysiX 36:d95e3d6f2fc4 17 uint16_t fifoCount; // count of all bytes currently in FIFO
pHysiX 36:d95e3d6f2fc4 18 uint8_t fifoBuffer[64]; // FIFO storage buffer
pHysiX 36:d95e3d6f2fc4 19
pHysiX 36:d95e3d6f2fc4 20 /* Orientation/motion variables: */
pHysiX 36:d95e3d6f2fc4 21 Quaternion q; // [w, x, y, z] quaternion container
pHysiX 36:d95e3d6f2fc4 22 VectorFloat gravity; // [x, y, z] gravity vector
pHysiX 38:ef65533cca32 23 float ypr_rad[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
pHysiX 50:8a0accb23007 24 volatile float ypr[3];
pHysiX 36:d95e3d6f2fc4 25
pHysiX 36:d95e3d6f2fc4 26 #ifndef M_PI
pHysiX 36:d95e3d6f2fc4 27 #define M_PI 3.1415
pHysiX 36:d95e3d6f2fc4 28 #endif
pHysiX 36:d95e3d6f2fc4 29
pHysiX 36:d95e3d6f2fc4 30 #ifdef ENABLE_COMPASS
pHysiX 36:d95e3d6f2fc4 31 //int compassX, compassY, compassZ;
pHysiX 36:d95e3d6f2fc4 32 double heading = 0;
pHysiX 36:d95e3d6f2fc4 33 #endif
pHysiX 36:d95e3d6f2fc4 34
pHysiX 51:04c6af4319e1 35 float altitude, temperature;
pHysiX 51:04c6af4319e1 36
pHysiX 27:18b6580eb0b1 37 /* YPR Adjust */
pHysiX 27:18b6580eb0b1 38 volatile float adjust_attitude[3] = {0.0, 0.0, 0.0};
pHysiX 27:18b6580eb0b1 39
pHysiX 34:228d87c45151 40
pHysiX 34:228d87c45151 41
pHysiX 36:d95e3d6f2fc4 42
pHysiX 34:228d87c45151 43 // ===============================
pHysiX 34:228d87c45151 44 // === YPR SAMPLE & MASTER PID ===
pHysiX 34:228d87c45151 45 // ===============================
pHysiX 27:18b6580eb0b1 46 void Task2_Master(void const *argument)
pHysiX 27:18b6580eb0b1 47 {
pHysiX 50:8a0accb23007 48 while(1) {
pHysiX 51:04c6af4319e1 49 //PC.printf("T2M\n");
pHysiX 50:8a0accb23007 50 sem_Task2_Master.wait();
pHysiX 51:04c6af4319e1 51 //PC.printf("T2M Sem\n");
pHysiX 51:04c6af4319e1 52
pHysiX 51:04c6af4319e1 53 mutex_i2c.lock();
pHysiX 50:8a0accb23007 54 AHRSSample();
pHysiX 51:04c6af4319e1 55 altitude = altimeter.Altitude_m();
pHysiX 51:04c6af4319e1 56 altimeter.Temp_C();
pHysiX 51:04c6af4319e1 57 mutex_i2c.unlock();
pHysiX 36:d95e3d6f2fc4 58
pHysiX 50:8a0accb23007 59 if (armed) {
pHysiX 50:8a0accb23007 60 switch (mode) {
pHysiX 50:8a0accb23007 61 case RATE:
pHysiX 50:8a0accb23007 62 break;
pHysiX 28:aa72bd4ff103 63
pHysiX 50:8a0accb23007 64 case ATTITUDE:
pHysiX 50:8a0accb23007 65 default:
pHysiX 50:8a0accb23007 66 pitchPIDattitude.setProcessValue((int) (ypr[1] - ypr_offset[1]));
pHysiX 50:8a0accb23007 67 rollPIDattitude.setProcessValue((int) (ypr[2] - ypr_offset[2]));
pHysiX 50:8a0accb23007 68
pHysiX 50:8a0accb23007 69 pitchPIDattitude.setSetPoint(inputYPR[1]);
pHysiX 50:8a0accb23007 70 rollPIDattitude.setSetPoint(inputYPR[2]);
pHysiX 27:18b6580eb0b1 71
pHysiX 50:8a0accb23007 72 adjust_attitude[1] = pitchPIDattitude.compute();
pHysiX 50:8a0accb23007 73 adjust_attitude[2] = rollPIDattitude.compute();
pHysiX 50:8a0accb23007 74 adjust_attitude[2] *= -1;
pHysiX 30:d9b988f8d84f 75
pHysiX 50:8a0accb23007 76 sem_Task2_Slave.release();
pHysiX 50:8a0accb23007 77 break;
pHysiX 50:8a0accb23007 78 }
pHysiX 30:d9b988f8d84f 79 }
pHysiX 51:04c6af4319e1 80 Thread::wait(TASK2_MASTER_PERIOD);
pHysiX 27:18b6580eb0b1 81 }
pHysiX 27:18b6580eb0b1 82 }
pHysiX 33:f88a6ee18103 83
pHysiX 36:d95e3d6f2fc4 84
pHysiX 36:d95e3d6f2fc4 85
pHysiX 36:d95e3d6f2fc4 86
pHysiX 34:228d87c45151 87 // ************************
pHysiX 34:228d87c45151 88 // *** Helper functions ***
pHysiX 34:228d87c45151 89 // ************************
pHysiX 50:8a0accb23007 90 void Task2_Master_ISR(void const *argument)
pHysiX 50:8a0accb23007 91 {
pHysiX 51:04c6af4319e1 92 //PC.printf("T2M ISR\n");
pHysiX 50:8a0accb23007 93 sem_Task2_Master.release();
pHysiX 50:8a0accb23007 94 }
pHysiX 50:8a0accb23007 95
pHysiX 36:d95e3d6f2fc4 96 void AHRSSample(void)
pHysiX 36:d95e3d6f2fc4 97 {
pHysiX 36:d95e3d6f2fc4 98 //Timer
pHysiX 36:d95e3d6f2fc4 99 // reset interrupt flag and get INT_STATUS byte
pHysiX 48:9dbdc4144f00 100 mpuIntStatus = imu_nb.getIntStatus();
pHysiX 36:d95e3d6f2fc4 101
pHysiX 36:d95e3d6f2fc4 102 // get current FIFO count
pHysiX 48:9dbdc4144f00 103 fifoCount = imu_nb.getFIFOCount();
pHysiX 36:d95e3d6f2fc4 104 //imu.debugSerial.printf("FIFO Count: %d\n", fifoCount);
pHysiX 36:d95e3d6f2fc4 105
pHysiX 36:d95e3d6f2fc4 106 // check for overflow
pHysiX 36:d95e3d6f2fc4 107 // Only keep a max of 2 packets in buffer.
pHysiX 36:d95e3d6f2fc4 108 if ((mpuIntStatus & 0x10) || fifoCount > 84) {
pHysiX 36:d95e3d6f2fc4 109 // reset so we can continue cleanly
pHysiX 48:9dbdc4144f00 110 imu_nb.resetFIFO();
pHysiX 38:ef65533cca32 111 //imu.debugSerial.printf("FIFO overflow!");
pHysiX 38:ef65533cca32 112 //BT.printf("FIFO overflow!\n");
pHysiX 36:d95e3d6f2fc4 113
pHysiX 36:d95e3d6f2fc4 114 // otherwise, check for DMP data ready interrupt (this should happen frequently)
pHysiX 36:d95e3d6f2fc4 115 } else if (mpuIntStatus & 0x02) {
pHysiX 36:d95e3d6f2fc4 116 // wait for correct available data length, should be a VERY short wait
pHysiX 48:9dbdc4144f00 117 while (fifoCount < packetSize) fifoCount = imu_nb.getFIFOCount();
pHysiX 36:d95e3d6f2fc4 118
pHysiX 36:d95e3d6f2fc4 119 while (fifoCount > 41) {
pHysiX 36:d95e3d6f2fc4 120 // read a packet from FIFO
pHysiX 48:9dbdc4144f00 121 imu_nb.getFIFOBytes(fifoBuffer, packetSize);
pHysiX 36:d95e3d6f2fc4 122
pHysiX 36:d95e3d6f2fc4 123 // track FIFO count here in case there is > 1 packet available
pHysiX 36:d95e3d6f2fc4 124 // (this lets us immediately read more without waiting for an interrupt)
pHysiX 36:d95e3d6f2fc4 125 fifoCount -= packetSize;
pHysiX 36:d95e3d6f2fc4 126 }
pHysiX 36:d95e3d6f2fc4 127
pHysiX 36:d95e3d6f2fc4 128 // display YPR angles in degrees
pHysiX 48:9dbdc4144f00 129 imu_nb.dmpGetQuaternion(&q, fifoBuffer);
pHysiX 48:9dbdc4144f00 130 imu_nb.dmpGetGravity(&gravity, &q);
pHysiX 48:9dbdc4144f00 131 imu_nb.dmpGetYawPitchRoll(ypr_rad, &q, &gravity);
pHysiX 36:d95e3d6f2fc4 132
pHysiX 38:ef65533cca32 133 for (int i = 0; i < 3; i++)
pHysiX 38:ef65533cca32 134 ypr[i] = ypr_rad[i] * 180/M_PI;
pHysiX 36:d95e3d6f2fc4 135 }
pHysiX 36:d95e3d6f2fc4 136 //Timer
pHysiX 36:d95e3d6f2fc4 137 }