main.cpp

00001 #include "mbed.h"
00002 #include "FXOS8700Q.h"
00003 #include "eCompass_Lib.h"
00004 #include "rtos.h"
00005 //#include "MotionSensorDtypes.h"
00006 
00007 
00008 FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
00009 FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
00010 Serial pc(USBTX, USBRX);
00011 DigitalOut gpo(D0);
00012 DigitalOut led(LED_RED);
00013 eCompass compass;
00014 
00015 //void calibrate_thread(void const *argument);
00016 //void print_thread(void const *argument);
00017 //void compass_thread(void const *argument);
00018 
00019 
00020 
00021 extern axis6_t axis6;
00022 extern uint32_t seconds;
00023 extern uint32_t compass_type; // optional, NED compass is default
00024 extern int32_t tcount;
00025 extern uint8_t cdebug;
00026 int  l = 0;
00027 volatile int sflag = 0;
00028 
00029 MotionSensorDataCounts mag_raw;
00030 MotionSensorDataCounts acc_raw;
00031 
00032 void hal_map( MotionSensorDataCounts * acc_raw, MotionSensorDataCounts * mag_raw)
00033 {
00034 int16_t t;
00035 // swap and negate X & Y axis
00036 t = acc_raw->x;
00037 acc_raw->x = acc_raw->y * -1;
00038 acc_raw->y = t * -1;
00039 // swap mag X & Y axis
00040 t = mag_raw->x;
00041 mag_raw->x = mag_raw->y;
00042 mag_raw->y = t;
00043 // negate mag Z axis
00044 mag_raw->z *= -1;
00045 }
00046 
00047 //
00048 // Print data values for debug
00049 //
00050 void debug_print(void)
00051 {
00052     // Some useful printf statements for debug
00053     printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
00054     printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f    ", axis6.fGax, axis6.fGay, axis6.fGaz);
00055     printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
00056     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); 
00057 }
00058 
00059 
00060 void compass_thread(void const *argument) {
00061 
00062     // get raw data from the sensors
00063     acc.getAxis( acc_raw);
00064     mag.getAxis( mag_raw);
00065     if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
00066     if(l++ >= 50) { // take car of business once a second
00067         seconds++;
00068         sflag = 1;
00069         compass.calibrate();
00070         debug_print();
00071         l = 0;
00072         led = !led;
00073         }
00074     tcount++;
00075 }
00076  
00077 /*  
00078 void calibrate_thread(void const *argument) {
00079     while (true) {
00080         // Signal flags that are reported as event are automatically cleared.
00081         Thread::signal_wait(0x1);
00082         compass.calibrate(); // re-calibrate the eCompass every second
00083     }
00084 }
00085  
00086 
00087   
00088 void print_thread(void const *argument) {
00089     while (true) {
00090         // Signal flags that are reported as event are automatically cleared.
00091         Thread::signal_wait(0x1);
00092         debug_print(); // re-calibrate the eCompass every second
00093     }
00094 }
00095  */            
00096 
00097 int main() {
00098 
00099 
00100 RtosTimer compass_timer(compass_thread, osTimerPeriodic);
00101 
00102 //cdebug = 1;  // uncomment to disable compass
00103 printf("\r\n\n\n\n\n\n\n");
00104 printf("Who AM I= %X\r\n", acc.whoAmI());
00105 acc.enable();
00106 
00107 
00108 acc.getAxis( acc_raw);
00109 mag.getAxis( mag_raw);
00110 
00111 compass_timer.start(20); // Run the Compass every 20ms
00112 while(1) {
00113     Thread::wait(osWaitForever);
00114     }  
00115 }