Diff: main.cpp

Revision:

2:51f3303cbefd

Parent:

0:32e37c82ef4a

Child:

3:d6404f10bd3b

--- a/main.cpp	Mon Apr 14 17:17:00 2014 +0000
+++ b/main.cpp	Fri May 09 16:23:09 2014 +0000
@@ -2,36 +2,46 @@
 #include "FXOS8700Q.h"
 #include "eCompass_Lib.h"
 #include "rtos.h"
+//#include "MotionSensorDtypes.h"
 
 
-
-FXOS8700Q combo1( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
+FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
+FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
 Serial pc(USBTX, USBRX);
 DigitalOut gpo(D0);
 DigitalOut led(LED_RED);
 eCompass compass;
 
+//void calibrate_thread(void const *argument);
+//void print_thread(void const *argument);
+//void compass_thread(void const *argument);
+
+
+
 extern axis6_t axis6;
 extern uint32_t seconds;
 extern uint32_t compass_type; // optional, NED compass is default
 extern int32_t tcount;
 extern uint8_t cdebug;
 int  l = 0;
-
+volatile int sflag = 0;
 
-void hal_map( int16_t * acc, int16_t * mag)
+MotionSensorDataCounts mag_raw;
+MotionSensorDataCounts acc_raw;
+
+void hal_map( MotionSensorDataCounts * acc_raw, MotionSensorDataCounts * mag_raw)
 {
 int16_t t;
 // swap and negate X & Y axis
-t = acc[0];
-acc[0] = acc[1] * -1;
-acc[1] = t * -1;
+t = acc_raw->x;
+acc_raw->x = acc_raw->y * -1;
+acc_raw->y = t * -1;
 // swap mag X & Y axis
-t = mag[0];
-mag[0] = mag[1];
-mag[1] = t;
+t = mag_raw->x;
+mag_raw->x = mag_raw->y;
+mag_raw->y = t;
 // negate mag Z axis
-mag[2] *= -1;
+mag_raw->z *= -1;
 }
 
 //
@@ -48,46 +58,68 @@
 
 
 void compass_thread(void const *argument) {
-    static int16_t acc_raw[3], mag_raw[3];
+
     // get raw data from the sensors
-    combo1.AccXYZraw( acc_raw);
-    combo1.MagXYZraw( mag_raw);
+    acc.getAxis( acc_raw);
+    mag.getAxis( mag_raw);
     if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
     if(l++ >= 50) { // take car of business once a second
         seconds++;
-        debug_print();
-        compass.calibrate(); // re-calibrate the eCompass every second
+        sflag = 1;
+        compass.calibrate();
         l = 0;
         led = !led;
         }
     tcount++;
 }
-         
+ 
+/*  
+void calibrate_thread(void const *argument) {
+    while (true) {
+        // Signal flags that are reported as event are automatically cleared.
+        Thread::signal_wait(0x1);
+        compass.calibrate(); // re-calibrate the eCompass every second
+    }
+}
+ 
+*/
+  
+void print_thread(void const *argument) {
+    while (true) {
+        // Signal flags that are reported as event are automatically cleared.
+        Thread::signal_wait(0x1);
+        debug_print(); // re-calibrate the eCompass every second
+    }
+}
+             
+
 int main() {
 
-uint8_t d[2];
-int16_t acc_raw[3], mag_raw[3];
-d[0] = 0;
+
+//Thread calibrate(calibrate_thread);
+Thread dprint(print_thread);
+//RtosTimer compass_timer(compass_thread, osTimerPeriodic);
+
 //cdebug = 1;  // uncomment to disable compass
 printf("\r\n\n\n\n\n\n\n");
-combo1.enable();
-combo1.readRegs( FXOS8700Q_STATUS, d, 1);
-printf("Status= %X\r\n", d[0]);
-combo1.readRegs( FXOS8700Q_WHOAMI, d, 1);
-printf("Who AM I= %X\r\n", d[0]);
-combo1.readRegs( FXOS8700Q_XYZ_DATA_CFG, d, 1);
-printf("XYZ Cfg= %X\r\n", d[0]);
-combo1.readRegs( FXOS8700Q_CTRL_REG1, d, 1);
-printf("CTRL REG1= %X\r\n", d[0]);
-combo1.readRegs( FXOS8700Q_M_CTRL_REG1, d, 1);
-printf("MAG CTRL REG1= %X\r\n", d[0]);
-combo1.readRegs( FXOS8700Q_M_CTRL_REG2, d, 1);
-printf("Mag CTRL REG2= %X\r\n\r\n", d[0]);
+acc.enable();
+printf("Who AM I= %X\r\n", acc.whoAmI());
+
+acc.getAxis( acc_raw);
+mag.getAxis( mag_raw);
+
+dprint.set_priority(osPriorityLow);
+//calibrate.set_priority(osPriorityBelowNormal);
+//compass_timer.set_priority(osPriorityRealtime);
 
-combo1.AccXYZraw( acc_raw);
-combo1.MagXYZraw( mag_raw);
-
-RtosTimer compass_timer(compass_thread, osTimerPeriodic);
-compass_timer.start(20); // Run the Compass every 20ms
-Thread::wait(osWaitForever);  
+//compass_timer.start(20); // Run the Compass every 20ms
+while(1) {
+    //while(!sflag);
+    sflag = 0;
+    //debug_print();
+    printf("\r\nL\r\n");
+    //calibrate.signal_set(0x1);
+    dprint.signal_set(0x1);
+    wait(1000);
+    }  
 }