Jim Carver / Mbed 2 deprecated K64F_eCompass_LCD

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 Jim Carver

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?

UserRevisionLine numberNew 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 }