main.cpp

00001 #include "mbed.h"
00002 #include "eCompass_Lib.h"
00003 #include "MAG3110.h"
00004 #include "MMA8451Q.h"
00005 #include "rtos.h"
00006 #include "SLCD.h"
00007 
00008 #define MMA8451_I2C_ADDRESS (0x1d<<1)
00009 
00010 
00011 eCompass compass;
00012 MAG3110  mag( PTE25, PTE24);
00013 MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS);
00014 DigitalOut red(LED_RED);
00015 DigitalOut green(LED_GREEN);
00016 Serial pc(USBTX, USBRX);
00017 SLCD slcd;
00018 
00019 extern axis6_t axis6;
00020 extern uint32_t seconds;
00021 extern uint32_t compass_type;
00022 extern int32_t tcount;
00023 extern uint8_t cdebug; 
00024 
00025 
00026 MotionSensorDataCounts mag_raw;
00027 MotionSensorDataCounts acc_raw;
00028 
00029 
00030 Thread *(debugp);
00031 Thread *(calibrate);
00032 Thread *(lcdp);
00033 
00034 //
00035 // Print data values for debug
00036 //
00037 void debug_print(void)
00038 {
00039     //pc.printf("0,%d$",axis6.yaw);
00040     pc.printf("%4.2f,%4.2f$",axis6.fUTmx, axis6.fUTmy);
00041     //int h, m, s;
00042     //h = seconds / 3600;
00043     //m = (seconds % 3600) / 60;
00044     //s = seconds % 60;
00045     // Some useful printf statements for debug
00046     //printf("Runtime= %d:%02d:%02d\r\n", h, m, s);
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", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
00050     //printf("Quaternion: Q0= %1.4f Q1= %1.4f Q2= %1.4f Q3= %1.4f\r\n\n", axis6.q0, axis6.q1, axis6.q2, axis6.q3); 
00051 }
00052 // HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors
00053 //
00054 // This routing move and negates data as needed the
00055 // properly align the sensor axis for our desired compass (NED)
00056 // For more information see Freescale appication note AN4696
00057 //
00058 void hal_map( MotionSensorDataCounts * acc_data, MotionSensorDataCounts * mag_data)
00059 {
00060     //int16_t t;
00061     // swap and negate accelerometer x & y
00062     //t = acc_data->y;
00063     //acc_data->y = acc_data->x * -1;
00064     //acc_data->x = t * -1;
00065 
00066     // negate magnetometer y
00067     //mag_data->y *= -1;
00068     
00069 }
00070 
00071 //
00072 // This is the 50Hz thread where the magic happens
00073 //
00074 int  l = 0;
00075 
00076 void compass_thread(void const *argument) {
00077         // get raw data from the sensors
00078         mag.getAxis(mag_raw);
00079         acc.getAxis(acc_raw);
00080         if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
00081         if(!(l % 10)) lcdp->signal_set(0x04);
00082         if(l++ >= 50) { // take car of business once a second
00083             seconds++;
00084             //calibrate->signal_set(0x2);
00085             //debugp->signal_set(0x01);
00086             l = 0;
00087             green = !green;
00088             }
00089         tcount++;
00090 }
00091 
00092 
00093 void calibrate_thread(void const *argument) {
00094     while(true) {
00095         Thread::signal_wait(0x2); 
00096         //red = 0;
00097         //compass.calibrate();
00098         //red = 1;
00099         }
00100 }
00101 
00102  
00103 void print_thread(void const *argument) {
00104     while (true) {
00105         // Signal flags that are reported as event are automatically cleared.
00106         //Thread::signal_wait(0x1); 
00107         if (pc.getc())
00108         {   
00109             debug_print();
00110         }
00111     }
00112 }
00113 
00114 
00115 void lcd_thread(void const *argument) {
00116     while (true) {
00117         // Signal flags that are reported as event are automatically cleared.
00118         Thread::signal_wait(0x04);
00119         slcd.printf("%04d", axis6.yaw);
00120     }
00121 }
00122                                  
00123 int main() {
00124 
00125     slcd.clear();
00126     tcount = 0;
00127     red = 1;
00128     green = 1;
00129     //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data.
00130     compass_type = NED_COMPASS;
00131     seconds = 0;
00132     Thread t1(print_thread);  
00133     Thread t2(calibrate_thread);
00134     Thread t3(lcd_thread);
00135     debugp = &t1;
00136     calibrate = &t2;
00137     lcdp = &t3;  
00138     mag.enable();
00139     acc.enable();
00140     // Say hello to our sensors
00141     // Native KL46-FRDM sensors
00142     //printf("\r\nMMA8451Q ID= %X\r\n", acc.whoAmI());
00143     //printf("MAG3110 ID= %X\r\n", mag.whoAmI());
00144     mag.getAxis(mag_raw); // flush the magnetmeter
00145     RtosTimer compass_timer(compass_thread, osTimerPeriodic);
00146     
00147     /*
00148      * Thread Priorities
00149      * Compass Thread,      High Priority
00150      * Compass calibration, Above Normal
00151      * LED Update,          Normal
00152      * printf to console,   Below Normal
00153      * main(),              Normal
00154      */
00155 
00156     debugp->set_priority(osPriorityBelowNormal);
00157     lcdp->set_priority(osPriorityLow);
00158     calibrate->set_priority(osPriorityAboveNormal);
00159     compass_timer.start(20); // Run the Compass every 20ms
00160     
00161     Thread::wait(osWaitForever);   
00162 }