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?
User | Revision | Line number | New 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 | } |