Jim Carver / Mbed 2 deprecated rtos_compass Featured

A basic eCompass using mbed-RTOS

Dependencies:   C12832 FXOS8700Q eCompass_FPU_Lib mbed-rtos mbed

main.cpp@0:2126a25ea273, 2014-05-05 (annotated)

Committer:
JimCarver
Date:
Mon May 05 17:46:15 2014 +0000
Revision:
0:2126a25ea273
Child:
1:2d2270d1a5f5
A basic eCompass using mbed-RTOS to manage all tasks

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JimCarver 0:2126a25ea273 1 #include "mbed.h"
JimCarver 0:2126a25ea273 2 #include "FXOS8700Q.h"
JimCarver 0:2126a25ea273 3 #include "eCompass_Lib.h"
JimCarver 0:2126a25ea273 4 #include "rtos.h"
JimCarver 0:2126a25ea273 5 #include "C12832.h"
JimCarver 0:2126a25ea273 6
JimCarver 0:2126a25ea273 7 /*
JimCarver 0:2126a25ea273 8 *
JimCarver 0:2126a25ea273 9 * Changes which must be made to default rtos settings:
JimCarver 0:2126a25ea273 10 * Default scheduler stack size changed to 2048 for target K64F (RTX_Conf_CM.h, line 66)
JimCarver 0:2126a25ea273 11 * Default stack size changed to 1024 words for ARMCC Compiler (cmsis_os.h, line 121)
JimCarver 0:2126a25ea273 12 * The Timer Thread must run at Real Time priority (RTX_Conf_CM.c, line 182)
JimCarver 0:2126a25ea273 13 *
JimCarver 0:2126a25ea273 14 */
JimCarver 0:2126a25ea273 15
JimCarver 0:2126a25ea273 16 extern Thread osTimerThread;
JimCarver 0:2126a25ea273 17 FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
JimCarver 0:2126a25ea273 18 FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
JimCarver 0:2126a25ea273 19 eCompass compass;
JimCarver 0:2126a25ea273 20 DigitalOut red(D5); // RED LED on expansion shield
JimCarver 0:2126a25ea273 21 DigitalOut green(D9); // GREEN LED on expansion shield
JimCarver 0:2126a25ea273 22 DigitalOut blue(LED3); // BLUE LED on K64F Freedom board
JimCarver 0:2126a25ea273 23 C12832 lcd(D11, D13, D12, D7, D10); // App shield LCD
JimCarver 0:2126a25ea273 24
JimCarver 0:2126a25ea273 25
JimCarver 0:2126a25ea273 26 extern axis6_t axis6;
JimCarver 0:2126a25ea273 27 extern uint32_t seconds;
JimCarver 0:2126a25ea273 28 extern uint32_t compass_type; // optional, NED compass is default
JimCarver 0:2126a25ea273 29 extern int32_t tcount;
JimCarver 0:2126a25ea273 30 extern uint8_t cdebug;
JimCarver 0:2126a25ea273 31 int l = 0;
JimCarver 0:2126a25ea273 32 volatile int sflag = 0;
JimCarver 0:2126a25ea273 33
JimCarver 0:2126a25ea273 34 MotionSensorDataCounts mag_raw;
JimCarver 0:2126a25ea273 35 MotionSensorDataCounts acc_raw;
JimCarver 0:2126a25ea273 36
JimCarver 0:2126a25ea273 37
JimCarver 0:2126a25ea273 38 Thread *(debugp);
JimCarver 0:2126a25ea273 39 Thread *(calibrate);
JimCarver 0:2126a25ea273 40 Thread *(lcdp);
JimCarver 0:2126a25ea273 41
JimCarver 0:2126a25ea273 42 void hal_map( MotionSensorDataCounts * acc_raw, MotionSensorDataCounts * mag_raw)
JimCarver 0:2126a25ea273 43 {
JimCarver 0:2126a25ea273 44 int16_t t;
JimCarver 0:2126a25ea273 45 // swap and negate X & Y axis
JimCarver 0:2126a25ea273 46 t = acc_raw->x;
JimCarver 0:2126a25ea273 47 acc_raw->x = acc_raw->y * -1;
JimCarver 0:2126a25ea273 48 acc_raw->y = t * -1;
JimCarver 0:2126a25ea273 49 // swap mag X & Y axis
JimCarver 0:2126a25ea273 50 t = mag_raw->x;
JimCarver 0:2126a25ea273 51 mag_raw->x = mag_raw->y;
JimCarver 0:2126a25ea273 52 mag_raw->y = t;
JimCarver 0:2126a25ea273 53 // negate mag Z axis
JimCarver 0:2126a25ea273 54 mag_raw->z *= -1;
JimCarver 0:2126a25ea273 55 }
JimCarver 0:2126a25ea273 56
JimCarver 0:2126a25ea273 57
JimCarver 0:2126a25ea273 58 void compass_thread(void const *argument) {
JimCarver 0:2126a25ea273 59 //osTimerThread.set_priority(osPriorityRealtime);
JimCarver 0:2126a25ea273 60 // get raw data from the sensors
JimCarver 0:2126a25ea273 61 green = 0;
JimCarver 0:2126a25ea273 62 acc.getAxis( acc_raw);
JimCarver 0:2126a25ea273 63 mag.getAxis( mag_raw);
JimCarver 0:2126a25ea273 64 if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
JimCarver 0:2126a25ea273 65 if(!(l % 10)) lcdp->signal_set(0x04);
JimCarver 0:2126a25ea273 66 if(l++ >= 49) { // take car of business once a second
JimCarver 0:2126a25ea273 67 seconds++;
JimCarver 0:2126a25ea273 68 calibrate->signal_set(0x2);
JimCarver 0:2126a25ea273 69 debugp->signal_set(0x01);
JimCarver 0:2126a25ea273 70 l = 0;
JimCarver 0:2126a25ea273 71 sflag = 1;
JimCarver 0:2126a25ea273 72 }
JimCarver 0:2126a25ea273 73 tcount++;
JimCarver 0:2126a25ea273 74 green = 1;
JimCarver 0:2126a25ea273 75 //osTimerThread.set_priority(osPriorityNormal);
JimCarver 0:2126a25ea273 76 }
JimCarver 0:2126a25ea273 77
JimCarver 0:2126a25ea273 78 void debug_print(void)
JimCarver 0:2126a25ea273 79 {
JimCarver 0:2126a25ea273 80 // Some useful printf statements for debug
JimCarver 0:2126a25ea273 81 printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
JimCarver 0:2126a25ea273 82 //printf("Acc: X= %d Y= %d Z= %d ", acc_raw.x, acc_raw.y, acc_raw.z);
JimCarver 0:2126a25ea273 83 //printf("Mag: X= %d Y= %d Z= %d\r\n", mag_raw.x, mag_raw.y, mag_raw.z);
JimCarver 0:2126a25ea273 84 printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz);
JimCarver 0:2126a25ea273 85 printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
JimCarver 0:2126a25ea273 86 printf("Quaternion: Q0= %1.4f Q1= %1.4f Q2= %1.4f Q3= %1.4f\r\n\r\n", axis6.q0, axis6.q1, axis6.q2, axis6.q3);
JimCarver 0:2126a25ea273 87 printf("Tcount= %d\r\n", tcount);
JimCarver 0:2126a25ea273 88 }
JimCarver 0:2126a25ea273 89
JimCarver 0:2126a25ea273 90 void calibrate_thread(void const *argument) {
JimCarver 0:2126a25ea273 91 while(true) {
JimCarver 0:2126a25ea273 92 Thread::signal_wait(0x2);
JimCarver 0:2126a25ea273 93 blue = 0;
JimCarver 0:2126a25ea273 94 compass.calibrate();
JimCarver 0:2126a25ea273 95 blue = 1;
JimCarver 0:2126a25ea273 96 }
JimCarver 0:2126a25ea273 97 }
JimCarver 0:2126a25ea273 98
JimCarver 0:2126a25ea273 99
JimCarver 0:2126a25ea273 100 void print_thread(void const *argument) {
JimCarver 0:2126a25ea273 101 while (true) {
JimCarver 0:2126a25ea273 102 // Signal flags that are reported as event are automatically cleared.
JimCarver 0:2126a25ea273 103 Thread::signal_wait(0x1);
JimCarver 0:2126a25ea273 104 debug_print();
JimCarver 0:2126a25ea273 105 }
JimCarver 0:2126a25ea273 106 }
JimCarver 0:2126a25ea273 107
JimCarver 0:2126a25ea273 108
JimCarver 0:2126a25ea273 109 void lcd_thread(void const *argument) {
JimCarver 0:2126a25ea273 110 int h, m, s;
JimCarver 0:2126a25ea273 111 while (true) {
JimCarver 0:2126a25ea273 112 // Signal flags that are reported as event are automatically cleared.
JimCarver 0:2126a25ea273 113 Thread::signal_wait(0x4);
JimCarver 0:2126a25ea273 114 red = 0;
JimCarver 0:2126a25ea273 115 h = seconds / 3600;
JimCarver 0:2126a25ea273 116 m = (seconds % 3600) / 60;
JimCarver 0:2126a25ea273 117 s = seconds % 60;
JimCarver 0:2126a25ea273 118 lcd.locate(0,1);
JimCarver 0:2126a25ea273 119 lcd.printf("K64F eCompass\nRunTime %02d:%02d:%02d\nRoll: %03d Pitch: %02d Yaw: %03d ", h, m, s, axis6.roll, axis6.pitch, axis6.yaw);
JimCarver 0:2126a25ea273 120 red = 1;
JimCarver 0:2126a25ea273 121 }
JimCarver 0:2126a25ea273 122 }
JimCarver 0:2126a25ea273 123
JimCarver 0:2126a25ea273 124
JimCarver 0:2126a25ea273 125 int main (void) {
JimCarver 0:2126a25ea273 126 red = 1;
JimCarver 0:2126a25ea273 127 green = 1;
JimCarver 0:2126a25ea273 128 blue = 1;
JimCarver 0:2126a25ea273 129 Thread t1(print_thread);
JimCarver 0:2126a25ea273 130 Thread t2(calibrate_thread);
JimCarver 0:2126a25ea273 131 Thread t3(lcd_thread);
JimCarver 0:2126a25ea273 132 debugp = &t1;
JimCarver 0:2126a25ea273 133 calibrate = &t2;
JimCarver 0:2126a25ea273 134 lcdp = &t3;
JimCarver 0:2126a25ea273 135 acc.enable();
JimCarver 0:2126a25ea273 136 mag.enable();
JimCarver 0:2126a25ea273 137 lcd.cls();
JimCarver 0:2126a25ea273 138 lcd.locate(0,1);
JimCarver 0:2126a25ea273 139 lcd.printf("K64F eCompass");
JimCarver 0:2126a25ea273 140 acc.getAxis( acc_raw);
JimCarver 0:2126a25ea273 141 mag.getAxis( mag_raw);
JimCarver 0:2126a25ea273 142 // The Timer Thread runs at Real Time priority, see RTX_Conf_CM.c, line 182
JimCarver 0:2126a25ea273 143 RtosTimer compass_timer(compass_thread, osTimerPeriodic);
JimCarver 0:2126a25ea273 144 compass_timer.start(20);
JimCarver 0:2126a25ea273 145 /*
JimCarver 0:2126a25ea273 146 * Thread Priorities
JimCarver 0:2126a25ea273 147 * Compass THread, Real Time (Highest)
JimCarver 0:2126a25ea273 148 * Compass calibration, High Priority
JimCarver 0:2126a25ea273 149 * LED Update, Above Normal
JimCarver 0:2126a25ea273 150 * Pprintf to console, Normal
JimCarver 0:2126a25ea273 151 * main(), Normal
JimCarver 0:2126a25ea273 152 */
JimCarver 0:2126a25ea273 153 debugp->set_priority(osPriorityNormal);
JimCarver 0:2126a25ea273 154 lcdp->set_priority(osPriorityAboveNormal);
JimCarver 0:2126a25ea273 155 calibrate->set_priority(osPriorityHigh);
JimCarver 0:2126a25ea273 156 green = 1;
JimCarver 0:2126a25ea273 157 while (true) {
JimCarver 0:2126a25ea273 158 Thread::wait(osWaitForever);
JimCarver 0:2126a25ea273 159 }
JimCarver 0:2126a25ea273 160 }