main.cpp

00001 #include "mbed.h"
00002 #include "FXOS8700Q.h"
00003 #include "eCompass_Lib.h"
00004 #include "rtos.h"
00005 #include "C12832.h"
00006 
00007 // Using Arduino pin notation
00008 C12832 lcd(D11, D13, D12, D7, D10); // App shield LCD
00009 FXOS8700Q combo1( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
00010 Serial pc(USBTX, USBRX);
00011 DigitalOut gpo(D0);
00012 DigitalOut red(LED_RED);
00013 DigitalOut led(D5);
00014 DigitalOut green(LED_GREEN);
00015 DigitalOut blue(LED_BLUE);
00016 eCompass compass;
00017 
00018 extern axis6_t axis6;
00019 extern uint32_t seconds;
00020 extern uint32_t compass_type; // optional, NED compass is default
00021 extern int32_t tcount;
00022 extern uint8_t cdebug;
00023 int  l = 0;
00024 
00025 
00026 void hal_map( int16_t * acc, int16_t * mag)
00027 {
00028 int16_t t;
00029 // swap and negate X & Y axis
00030 t = acc[0];
00031 acc[0] = acc[1] * -1;
00032 acc[1] = t * -1;
00033 // swap mag X & Y axis
00034 t = mag[0];
00035 mag[0] = mag[1];
00036 mag[1] = t;
00037 // negate mag Z axis
00038 mag[2] *= -1;
00039 }
00040 
00041 //
00042 // Print data values for debug
00043 //
00044 void debug_print(void)
00045 {
00046     // Some useful printf statements for debug
00047     printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
00048     printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f    ", axis6.fGax, axis6.fGay, axis6.fGaz);
00049     printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
00050     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); 
00051 }
00052 
00053 void doRGB(void)
00054 {
00055     int16_t h;
00056     h = axis6.yaw;
00057     if((h >= 358) || (h <= 2)) {
00058         red = 1;
00059         green = 0;
00060         blue = 1;
00061         return;
00062         }
00063     if((h >= 353) || (h <= 7)) {
00064         red = 1;
00065         green = 0;
00066         blue = 0;
00067         return;
00068         }
00069     if((h >=  85) && (h <=  95)) {
00070         red = 0;
00071         green = 1;
00072         blue = 1;
00073         return;
00074         }
00075     if((h >= 265) && (h <= 275)) {
00076         red = 0;
00077         green = 1;
00078         blue = 1;
00079         return;
00080         }
00081     if((h >= 175) && (h <= 185)) {
00082         red = 0;
00083         green = 1;
00084         blue = 0;
00085         return;
00086         }
00087     red = 1;
00088     green = 1;
00089     blue = 1;
00090 }
00091     
00092 
00093 void compass_thread(void const *argument) {
00094     static int16_t acc_raw[3], mag_raw[3];
00095     // get raw data from the sensors
00096     combo1.AccXYZraw( acc_raw);
00097     combo1.MagXYZraw( mag_raw);
00098     if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
00099     if(!l)      led = 0;
00100         else    led = 1;
00101     doRGB();
00102     if(l++ >= 50) { // take car of business once a second
00103         seconds++;
00104         debug_print();
00105         compass.calibrate(); // re-calibrate the eCompass every second
00106         l = 0;
00107         }
00108     tcount++;
00109 }
00110          
00111 int main() {
00112 
00113 uint8_t d[2];
00114 int16_t acc_raw[3], mag_raw[3];
00115 d[0] = 0;
00116 red = 1;
00117 green = 1;
00118 blue = 1;
00119 //cdebug = 1;  // uncomment to disable compass
00120 lcd.cls();
00121 lcd.locate(0,1);
00122 lcd.printf("K64F eCompass");
00123 printf("\r\n\n\n\n\n\n\n");
00124 combo1.enable();
00125 combo1.readRegs( FXOS8700Q_STATUS, d, 1);
00126 printf("Status= %X\r\n", d[0]);
00127 combo1.readRegs( FXOS8700Q_WHOAMI, d, 1);
00128 printf("Who AM I= %X\r\n", d[0]);
00129 combo1.readRegs( FXOS8700Q_XYZ_DATA_CFG, d, 1);
00130 printf("XYZ Cfg= %X\r\n", d[0]);
00131 combo1.readRegs( FXOS8700Q_CTRL_REG1, d, 1);
00132 printf("CTRL REG1= %X\r\n", d[0]);
00133 combo1.readRegs( FXOS8700Q_M_CTRL_REG1, d, 1);
00134 printf("MAG CTRL REG1= %X\r\n", d[0]);
00135 combo1.readRegs( FXOS8700Q_M_CTRL_REG2, d, 1);
00136 printf("Mag CTRL REG2= %X\r\n\r\n", d[0]);
00137 
00138 combo1.AccXYZraw( acc_raw);
00139 combo1.MagXYZraw( mag_raw);
00140 
00141 RtosTimer compass_timer(compass_thread, osTimerPeriodic);
00142 compass_timer.start(20); // Run the Compass every 20ms
00143 while(1) {
00144     lcd.locate(0,11);
00145     lcd.printf("Seconds: %04d Heading: %03d", seconds, axis6.yaw);
00146     lcd.locate(0,22);
00147     lcd.printf("Roll   : %03d  Pitch: %02d", axis6.roll, axis6.pitch);
00148     wait(0.1);
00149     }
00150 }