Diff: compass.cpp

Revision:

0:77857a36b4ff

Child:

2:bcd60a69583f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/compass.cpp	Fri Mar 27 08:35:29 2015 +0000
@@ -0,0 +1,104 @@
+#include "mbed.h"
+#include "FXOS8700Q.h"
+#include "eCompass_Lib.h"
+#include "rtos.h"
+//#include "MotionSensorDtypes.h"
+
+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);
+int captureCompass(void const *argument);
+int eCompass_init(void);
+
+
+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;
+
+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_raw->x;
+acc_raw->x = acc_raw->y * -1;
+acc_raw->y = t * -1;
+// swap mag X & Y axis
+t = mag_raw->x;
+mag_raw->x = mag_raw->y;
+mag_raw->y = t;
+// negate mag Z axis
+mag_raw->z *= -1;
+}
+
+//
+// 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    \r\n", 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", axis6.q0, axis6.q1, axis6.q2, axis6.q3);
+    
+     
+}
+
+
+int captureCompass(void const *argument) {
+
+    // get raw data from the sensors
+    acc.getAxis( acc_raw);
+    mag.getAxis( mag_raw);
+    if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
+    debug_print();
+    if(l++ >= 50) { // take car of business once a second
+        seconds++;
+        sflag = 1;
+        compass.calibrate();
+        l = 0;
+        //led = !led;
+        }
+    tcount++;
+    return(0);
+}
+ 
+/*  
+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 eCompass_init() {
+
+    acc.enable();
+ 
+    return 0;
+}