This is an example of a tilt compensated eCompass running on a K64F Freedom Platform using the on board FXOS8700 6 axis sensor and Freescale's eCompass software library in a linkable object library compiled for a FPU enabled Cortex M4. This version also used the LCD in an App Shield to display navigation data.
Dependencies: C12832 FXOS8700Q eCompass_FPU_Lib mbed-rtos mbed
Fork of K64F_eCompass by
main.cpp@2:770f77c92bab, 2014-04-21 (annotated)
- Committer:
- JimCarver
- Date:
- Mon Apr 21 23:13:22 2014 +0000
- Revision:
- 2:770f77c92bab
- Parent:
- 0:32e37c82ef4a
Another K64F eCompass that used the LCD on an app shield to display heading data
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JimCarver | 0:32e37c82ef4a | 1 | #include "mbed.h" |
JimCarver | 0:32e37c82ef4a | 2 | #include "FXOS8700Q.h" |
JimCarver | 0:32e37c82ef4a | 3 | #include "eCompass_Lib.h" |
JimCarver | 0:32e37c82ef4a | 4 | #include "rtos.h" |
JimCarver | 2:770f77c92bab | 5 | #include "C12832.h" |
JimCarver | 0:32e37c82ef4a | 6 | |
JimCarver | 2:770f77c92bab | 7 | // Using Arduino pin notation |
JimCarver | 2:770f77c92bab | 8 | C12832 lcd(D11, D13, D12, D7, D10); // App shield LCD |
JimCarver | 0:32e37c82ef4a | 9 | FXOS8700Q combo1( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); |
JimCarver | 0:32e37c82ef4a | 10 | Serial pc(USBTX, USBRX); |
JimCarver | 0:32e37c82ef4a | 11 | DigitalOut gpo(D0); |
JimCarver | 2:770f77c92bab | 12 | DigitalOut red(LED_RED); |
JimCarver | 2:770f77c92bab | 13 | DigitalOut led(D5); |
JimCarver | 2:770f77c92bab | 14 | DigitalOut green(LED_GREEN); |
JimCarver | 2:770f77c92bab | 15 | DigitalOut blue(LED_BLUE); |
JimCarver | 0:32e37c82ef4a | 16 | eCompass compass; |
JimCarver | 0:32e37c82ef4a | 17 | |
JimCarver | 0:32e37c82ef4a | 18 | extern axis6_t axis6; |
JimCarver | 0:32e37c82ef4a | 19 | extern uint32_t seconds; |
JimCarver | 0:32e37c82ef4a | 20 | extern uint32_t compass_type; // optional, NED compass is default |
JimCarver | 0:32e37c82ef4a | 21 | extern int32_t tcount; |
JimCarver | 0:32e37c82ef4a | 22 | extern uint8_t cdebug; |
JimCarver | 0:32e37c82ef4a | 23 | int l = 0; |
JimCarver | 0:32e37c82ef4a | 24 | |
JimCarver | 0:32e37c82ef4a | 25 | |
JimCarver | 0:32e37c82ef4a | 26 | void hal_map( int16_t * acc, int16_t * mag) |
JimCarver | 0:32e37c82ef4a | 27 | { |
JimCarver | 0:32e37c82ef4a | 28 | int16_t t; |
JimCarver | 0:32e37c82ef4a | 29 | // swap and negate X & Y axis |
JimCarver | 0:32e37c82ef4a | 30 | t = acc[0]; |
JimCarver | 0:32e37c82ef4a | 31 | acc[0] = acc[1] * -1; |
JimCarver | 0:32e37c82ef4a | 32 | acc[1] = t * -1; |
JimCarver | 0:32e37c82ef4a | 33 | // swap mag X & Y axis |
JimCarver | 0:32e37c82ef4a | 34 | t = mag[0]; |
JimCarver | 0:32e37c82ef4a | 35 | mag[0] = mag[1]; |
JimCarver | 0:32e37c82ef4a | 36 | mag[1] = t; |
JimCarver | 0:32e37c82ef4a | 37 | // negate mag Z axis |
JimCarver | 0:32e37c82ef4a | 38 | mag[2] *= -1; |
JimCarver | 0:32e37c82ef4a | 39 | } |
JimCarver | 0:32e37c82ef4a | 40 | |
JimCarver | 0:32e37c82ef4a | 41 | // |
JimCarver | 0:32e37c82ef4a | 42 | // Print data values for debug |
JimCarver | 0:32e37c82ef4a | 43 | // |
JimCarver | 0:32e37c82ef4a | 44 | void debug_print(void) |
JimCarver | 0:32e37c82ef4a | 45 | { |
JimCarver | 0:32e37c82ef4a | 46 | // Some useful printf statements for debug |
JimCarver | 0:32e37c82ef4a | 47 | printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw); |
JimCarver | 0:32e37c82ef4a | 48 | printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz); |
JimCarver | 0:32e37c82ef4a | 49 | printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz); |
JimCarver | 0:32e37c82ef4a | 50 | 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:32e37c82ef4a | 51 | } |
JimCarver | 0:32e37c82ef4a | 52 | |
JimCarver | 2:770f77c92bab | 53 | void doRGB(void) |
JimCarver | 2:770f77c92bab | 54 | { |
JimCarver | 2:770f77c92bab | 55 | int16_t h; |
JimCarver | 2:770f77c92bab | 56 | h = axis6.yaw; |
JimCarver | 2:770f77c92bab | 57 | if((h >= 358) || (h <= 2)) { |
JimCarver | 2:770f77c92bab | 58 | red = 1; |
JimCarver | 2:770f77c92bab | 59 | green = 0; |
JimCarver | 2:770f77c92bab | 60 | blue = 1; |
JimCarver | 2:770f77c92bab | 61 | return; |
JimCarver | 2:770f77c92bab | 62 | } |
JimCarver | 2:770f77c92bab | 63 | if((h >= 353) || (h <= 7)) { |
JimCarver | 2:770f77c92bab | 64 | red = 1; |
JimCarver | 2:770f77c92bab | 65 | green = 0; |
JimCarver | 2:770f77c92bab | 66 | blue = 0; |
JimCarver | 2:770f77c92bab | 67 | return; |
JimCarver | 2:770f77c92bab | 68 | } |
JimCarver | 2:770f77c92bab | 69 | if((h >= 85) && (h <= 95)) { |
JimCarver | 2:770f77c92bab | 70 | red = 0; |
JimCarver | 2:770f77c92bab | 71 | green = 1; |
JimCarver | 2:770f77c92bab | 72 | blue = 1; |
JimCarver | 2:770f77c92bab | 73 | return; |
JimCarver | 2:770f77c92bab | 74 | } |
JimCarver | 2:770f77c92bab | 75 | if((h >= 265) && (h <= 275)) { |
JimCarver | 2:770f77c92bab | 76 | red = 0; |
JimCarver | 2:770f77c92bab | 77 | green = 1; |
JimCarver | 2:770f77c92bab | 78 | blue = 1; |
JimCarver | 2:770f77c92bab | 79 | return; |
JimCarver | 2:770f77c92bab | 80 | } |
JimCarver | 2:770f77c92bab | 81 | if((h >= 175) && (h <= 185)) { |
JimCarver | 2:770f77c92bab | 82 | red = 0; |
JimCarver | 2:770f77c92bab | 83 | green = 1; |
JimCarver | 2:770f77c92bab | 84 | blue = 0; |
JimCarver | 2:770f77c92bab | 85 | return; |
JimCarver | 2:770f77c92bab | 86 | } |
JimCarver | 2:770f77c92bab | 87 | red = 1; |
JimCarver | 2:770f77c92bab | 88 | green = 1; |
JimCarver | 2:770f77c92bab | 89 | blue = 1; |
JimCarver | 2:770f77c92bab | 90 | } |
JimCarver | 2:770f77c92bab | 91 | |
JimCarver | 0:32e37c82ef4a | 92 | |
JimCarver | 0:32e37c82ef4a | 93 | void compass_thread(void const *argument) { |
JimCarver | 0:32e37c82ef4a | 94 | static int16_t acc_raw[3], mag_raw[3]; |
JimCarver | 0:32e37c82ef4a | 95 | // get raw data from the sensors |
JimCarver | 0:32e37c82ef4a | 96 | combo1.AccXYZraw( acc_raw); |
JimCarver | 0:32e37c82ef4a | 97 | combo1.MagXYZraw( mag_raw); |
JimCarver | 0:32e37c82ef4a | 98 | if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass |
JimCarver | 2:770f77c92bab | 99 | if(!l) led = 0; |
JimCarver | 2:770f77c92bab | 100 | else led = 1; |
JimCarver | 2:770f77c92bab | 101 | doRGB(); |
JimCarver | 0:32e37c82ef4a | 102 | if(l++ >= 50) { // take car of business once a second |
JimCarver | 0:32e37c82ef4a | 103 | seconds++; |
JimCarver | 0:32e37c82ef4a | 104 | debug_print(); |
JimCarver | 0:32e37c82ef4a | 105 | compass.calibrate(); // re-calibrate the eCompass every second |
JimCarver | 0:32e37c82ef4a | 106 | l = 0; |
JimCarver | 0:32e37c82ef4a | 107 | } |
JimCarver | 0:32e37c82ef4a | 108 | tcount++; |
JimCarver | 0:32e37c82ef4a | 109 | } |
JimCarver | 0:32e37c82ef4a | 110 | |
JimCarver | 0:32e37c82ef4a | 111 | int main() { |
JimCarver | 0:32e37c82ef4a | 112 | |
JimCarver | 0:32e37c82ef4a | 113 | uint8_t d[2]; |
JimCarver | 0:32e37c82ef4a | 114 | int16_t acc_raw[3], mag_raw[3]; |
JimCarver | 0:32e37c82ef4a | 115 | d[0] = 0; |
JimCarver | 2:770f77c92bab | 116 | red = 1; |
JimCarver | 2:770f77c92bab | 117 | green = 1; |
JimCarver | 2:770f77c92bab | 118 | blue = 1; |
JimCarver | 0:32e37c82ef4a | 119 | //cdebug = 1; // uncomment to disable compass |
JimCarver | 2:770f77c92bab | 120 | lcd.cls(); |
JimCarver | 2:770f77c92bab | 121 | lcd.locate(0,1); |
JimCarver | 2:770f77c92bab | 122 | lcd.printf("K64F eCompass"); |
JimCarver | 0:32e37c82ef4a | 123 | printf("\r\n\n\n\n\n\n\n"); |
JimCarver | 0:32e37c82ef4a | 124 | combo1.enable(); |
JimCarver | 0:32e37c82ef4a | 125 | combo1.readRegs( FXOS8700Q_STATUS, d, 1); |
JimCarver | 0:32e37c82ef4a | 126 | printf("Status= %X\r\n", d[0]); |
JimCarver | 0:32e37c82ef4a | 127 | combo1.readRegs( FXOS8700Q_WHOAMI, d, 1); |
JimCarver | 0:32e37c82ef4a | 128 | printf("Who AM I= %X\r\n", d[0]); |
JimCarver | 0:32e37c82ef4a | 129 | combo1.readRegs( FXOS8700Q_XYZ_DATA_CFG, d, 1); |
JimCarver | 0:32e37c82ef4a | 130 | printf("XYZ Cfg= %X\r\n", d[0]); |
JimCarver | 0:32e37c82ef4a | 131 | combo1.readRegs( FXOS8700Q_CTRL_REG1, d, 1); |
JimCarver | 0:32e37c82ef4a | 132 | printf("CTRL REG1= %X\r\n", d[0]); |
JimCarver | 0:32e37c82ef4a | 133 | combo1.readRegs( FXOS8700Q_M_CTRL_REG1, d, 1); |
JimCarver | 0:32e37c82ef4a | 134 | printf("MAG CTRL REG1= %X\r\n", d[0]); |
JimCarver | 0:32e37c82ef4a | 135 | combo1.readRegs( FXOS8700Q_M_CTRL_REG2, d, 1); |
JimCarver | 0:32e37c82ef4a | 136 | printf("Mag CTRL REG2= %X\r\n\r\n", d[0]); |
JimCarver | 0:32e37c82ef4a | 137 | |
JimCarver | 0:32e37c82ef4a | 138 | combo1.AccXYZraw( acc_raw); |
JimCarver | 0:32e37c82ef4a | 139 | combo1.MagXYZraw( mag_raw); |
JimCarver | 0:32e37c82ef4a | 140 | |
JimCarver | 0:32e37c82ef4a | 141 | RtosTimer compass_timer(compass_thread, osTimerPeriodic); |
JimCarver | 0:32e37c82ef4a | 142 | compass_timer.start(20); // Run the Compass every 20ms |
JimCarver | 2:770f77c92bab | 143 | while(1) { |
JimCarver | 2:770f77c92bab | 144 | lcd.locate(0,11); |
JimCarver | 2:770f77c92bab | 145 | lcd.printf("Seconds: %04d Heading: %03d", seconds, axis6.yaw); |
JimCarver | 2:770f77c92bab | 146 | lcd.locate(0,22); |
JimCarver | 2:770f77c92bab | 147 | lcd.printf("Roll : %03d Pitch: %02d", axis6.roll, axis6.pitch); |
JimCarver | 2:770f77c92bab | 148 | wait(0.1); |
JimCarver | 2:770f77c92bab | 149 | } |
JimCarver | 0:32e37c82ef4a | 150 | } |