Diff: main.cpp

Revision:

3:0770c275e6e8

Parent:

2:e4ae1d748311

Child:

4:ba1dbfb683fb

--- a/main.cpp	Sun Apr 13 21:38:03 2014 +0000
+++ b/main.cpp	Fri May 16 18:23:32 2014 +0000
@@ -2,7 +2,6 @@
 #include "eCompass_Lib.h"
 #include "MAG3110.h"
 #include "MMA8451Q.h"
-#include "FXOS8700Q.h"
 #include "rtos.h"
 #include "SLCD.h"
 
@@ -12,7 +11,6 @@
 eCompass compass;
 MAG3110  mag( PTE25, PTE24);
 MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS);
-//FXOS8700Q combo0( A4, A5, FXOS8700CQ_SLAVE_ADDR0);
 DigitalOut red(LED_RED);
 DigitalOut green(LED_GREEN);
 Serial pc(USBTX, USBRX);
@@ -23,6 +21,16 @@
 extern uint32_t compass_type;
 extern int32_t tcount;
 extern uint8_t cdebug; 
+
+
+MotionSensorDataCounts mag_raw;
+MotionSensorDataCounts acc_raw;
+
+
+Thread *(debugp);
+Thread *(calibrate);
+Thread *(lcdp);
+
 //
 // Print data values for debug
 //
@@ -40,72 +48,68 @@
 // properly align the sensor axis for our desired compass (NED)
 // For more information see Freescale appication note AN4696
 //
-void hal_map(int16_t * acc, int16_t * mag)
+void hal_map( MotionSensorDataCounts * acc_data, MotionSensorDataCounts * mag_data)
 {
     int16_t t;
-    // swap accelerometer x & y
-    t = acc[1];
-    acc[1] = acc[0];
-    acc[0] = t;
-    // negate accelerometer x & y
-    acc[0] *= -1;
-    acc[1] *= -1;
+    // swap and negate accelerometer x & y
+    t = acc_data->y;
+    acc_data->y = acc_data->x * -1;
+    acc_data->x = t * -1;
+
     // negate magnetometer y
-    mag[1] *= -1;
+    mag_data->y *= -1;
     
 }
 
-/*
-// HAL Map for Multi-Sensor board with FXOS8700 sensor
-//
-// This routing move and negates data as needed the
-// properly align the sensor axis for our desired compass (NED)
-// For more information see Freescale appication note AN4696
-//
-void hal_map( int16_t * acc, int16_t * mag)
-{
-    int16_t t;
-    // Insert your HAL map here
-    t = acc[0];
-    acc[0] = acc[1] * -1;
-    acc[1] = t * -1;
-    
-    t = mag[1];
-    mag[1] = mag[0];
-    mag[0] = t;
-    mag[2] *= -1;
-
-}
-*/
-
-
 //
 // This is the 50Hz thread where the magic happens
 //
 int  l = 0;
 
 void compass_thread(void const *argument) {
-    static int16_t acc_raw[3], mag_raw[3];
         // get raw data from the sensors
-        mag.ReadXYZraw(mag_raw);
-        acc.getAccXYZraw(acc_raw);
-        //combo0.AccXYZraw( acc_raw);
-        //combo0.MagXYZraw( mag_raw);
+        mag.getAxis(mag_raw);
+        acc.getAxis(acc_raw);
         if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
-        slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD
+        if(!(l % 10)) lcdp->signal_set(0x04);
         if(l++ >= 50) { // take car of business once a second
             seconds++;
-            debug_print();
-            compass.calibrate(); // re-calibrate the eCompass every second
+            calibrate->signal_set(0x2);
+            debugp->signal_set(0x01);
             l = 0;
             green = !green;
             }
         tcount++;
 }
-                                   
+
+
+void calibrate_thread(void const *argument) {
+    while(true) {
+        Thread::signal_wait(0x2); 
+        compass.calibrate();
+        }
+}
+
+ 
+void print_thread(void const *argument) {
+    while (true) {
+        // Signal flags that are reported as event are automatically cleared.
+        Thread::signal_wait(0x1);    
+        debug_print();
+    }
+}
+
+
+void lcd_thread(void const *argument) {
+    while (true) {
+        // Signal flags that are reported as event are automatically cleared.
+        Thread::signal_wait(0x4);  
+        slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD
+    }
+}
+                                 
 int main() {
-//    Thread thread(compass_thread);
-    int16_t mag_data[3]; 
+
     slcd.clear();
     tcount = 0;
     red = 1;
@@ -113,16 +117,36 @@
     //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data.
     compass_type = NED_COMPASS;
     seconds = 0;
+    Thread t1(print_thread);  
+    Thread t2(calibrate_thread);
+    Thread t3(lcd_thread);
+    debugp = &t1;
+    calibrate = &t2;
+    lcdp = &t3;  
+    mag.enable();
+    acc.enable();
     // make sure our sensors are talking to us
     // Only available when multi-sensor shield is installed
     //printf("\r\nFXOS8700 ID= %X\r\n", combo0.getWhoAmI()); 
-    // NAtive KL46-FRDM sensors
-    printf("\r\nMMA8451Q ID= %X\r\n", acc.getWhoAmI());
-    printf("MAG3110 ID= %X\r\n", mag.readReg(MAG_WHO_AM_I));
-    mag.ReadXYZraw(mag_data); // flush the magnetmeter
+    // Native KL46-FRDM sensors
+    printf("\r\nMMA8451Q ID= %X\r\n", acc.whoAmI());
+    printf("MAG3110 ID= %X\r\n", mag.whoAmI());
+    mag.getAxis(mag_raw); // flush the magnetmeter
     RtosTimer compass_timer(compass_thread, osTimerPeriodic);
-//    //RtosTimer b_timer(blink_green, osTimerPeriodic);
+    
+    /*
+     * Thread Priorities
+     * Compass Thread,      High Priority
+     * Compass calibration, Above Normal
+     * LED Update,          Normal
+     * printf to console,   Below Normal
+     * main(),              Normal
+     */
+
+    debugp->set_priority(osPriorityBelowNormal);
+    lcdp->set_priority(osPriorityLow);
+    calibrate->set_priority(osPriorityAboveNormal);
+
     compass_timer.start(20); // Run the Compass every 20ms
-    //b_timer.start(250); //blink the green LED at 2Hz
     Thread::wait(osWaitForever);   
 }