Diff: main.cpp

Revision:

2:e4ae1d748311

Parent:

0:4e1d43dc608f

Child:

3:0770c275e6e8

--- a/main.cpp	Mon Apr 07 21:07:04 2014 +0000
+++ b/main.cpp	Sun Apr 13 21:38:03 2014 +0000
@@ -1,27 +1,44 @@
 #include "mbed.h"
-#include "eCompass_Lib_V3.h"
+#include "eCompass_Lib.h"
 #include "MAG3110.h"
 #include "MMA8451Q.h"
+#include "FXOS8700Q.h"
 #include "rtos.h"
 #include "SLCD.h"
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
 
 
+eCompass compass;
 MAG3110  mag( PTE25, PTE24);
 MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS);
-I2C      i2c( PTE25, PTE24);
-DigitalOut led(LED_RED);
+//FXOS8700Q combo0( A4, A5, FXOS8700CQ_SLAVE_ADDR0);
+DigitalOut red(LED_RED);
+DigitalOut green(LED_GREEN);
 Serial pc(USBTX, USBRX);
 SLCD slcd;
 
-axis6_t axis6;
-unsigned int seconds;
-unsigned int compass_type;
-int tcount;
-
+extern axis6_t axis6;
+extern uint32_t seconds;
+extern uint32_t compass_type;
+extern int32_t tcount;
+extern uint8_t cdebug; 
+//
+// Print data values for debug
+//
+void debug_print(void)
+{
+    // Some useful printf statements for debug
+    printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
+    printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f    ", axis6.fGax, axis6.fGay, axis6.fGaz);
+    printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
+    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); 
+}
+// HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors
+//
 // 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)
 {
@@ -37,87 +54,75 @@
     mag[1] *= -1;
     
 }
-    
 
-void manage_axis6(int16_t * acc_raw, int16_t * mag_raw)
+/*
+// 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)
 {
-    axis6.timestamp = tcount;
-    hal_map(acc_raw, mag_raw);
-    //
-    // raw data
-    axis6.acc_x = acc_raw[0];
-    axis6.acc_y = acc_raw[1];
-    axis6.acc_z = acc_raw[2];
-    axis6.mag_x = mag_raw[0];
-    axis6.mag_y = mag_raw[1];
-    axis6.mag_z = mag_raw[2];
-    //
-    // raw data converted to floating ouing
-    axis6.fax = (float) acc_raw[0];
-    axis6.fay = (float) acc_raw[1];
-    axis6.faz = (float) acc_raw[2];
-    axis6.fmx = (float) mag_raw[0];
-    axis6.fmy = (float) mag_raw[1];
-    axis6.fmz = (float) mag_raw[2];
-    //
-    // Accelerometer data converted to Gs
-    axis6.fGax = ((float) acc_raw[0]) / 4096.0;
-    axis6.fGay = ((float) acc_raw[1]) / 4096.0;
-    axis6.fGaz = ((float) acc_raw[2]) / 4096.0;
-    //
-    // Magnetometer data converted to microteslas
-    axis6.fUTmx = ((float) mag_raw[0]) / 10.0;
-    axis6.fUTmy = ((float) mag_raw[1]) / 10.0;
-    axis6.fUTmz = ((float) mag_raw[2]) / 10.0; 
-    }
+    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];
-    static int l = 0;
-    while (true) {
-        // Signal flags that are reported as event are automatically cleared.
-        Thread::signal_wait(0x1);
         // get raw data from the sensors
         mag.ReadXYZraw(mag_raw);
         acc.getAccXYZraw(acc_raw);
-        
-        manage_axis6( acc_raw, mag_raw); // copy needed data into the axis6 structure for use by the eCompass
-        if(tcount) run_eCompass(); // calculate the eCompass
+        //combo0.AccXYZraw( acc_raw);
+        //combo0.MagXYZraw( mag_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++ >= 50) { // take car of business once a second
             seconds++;
-            calibrate_eCompass(); // re-calibrate the eCompass every second
+            debug_print();
+            compass.calibrate(); // re-calibrate the eCompass every second
             l = 0;
-            /* Some usful printf statements for debug
-            printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
-            printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f    ", axis6.fGax, axis6.fGay, axis6.fGaz);
-            printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
-            */
-            led = !led;
+            green = !green;
             }
         tcount++;
-    }
 }
                                    
 int main() {
-    Thread thread(compass_thread);
-    float mag_data[3];
-    
+//    Thread thread(compass_thread);
+    int16_t mag_data[3]; 
     slcd.clear();
     tcount = 0;
-
-    init_eCompass();
+    red = 1;
+    green = 1;
+    //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data.
     compass_type = NED_COMPASS;
     seconds = 0;
     // make sure our sensors are talking to us
-    printf("\r\n%X\r\n", acc.getWhoAmI());
-    printf("%X\r\n", mag.readReg(MAG_WHO_AM_I));
-    mag.ReadXYZ(mag_data); // flush the magnetmeter
-    while (1) {
-        Thread::wait(20); // run out thread every 20ms for a 50Hz rate
-        thread.signal_set(0x1);    
-        }
+    // 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
+    RtosTimer compass_timer(compass_thread, osTimerPeriodic);
+//    //RtosTimer b_timer(blink_green, osTimerPeriodic);
+    compass_timer.start(20); // Run the Compass every 20ms
+    //b_timer.start(250); //blink the green LED at 2Hz
+    Thread::wait(osWaitForever);   
 }