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@1:2d2270d1a5f5, 2014-05-07 (annotated)

Committer:
JimCarver
Date:
Wed May 07 18:09:44 2014 +0000
Revision:
1:2d2270d1a5f5
Parent:
0:2126a25ea273 
Improved version which requires no modification to RTOS

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