Diff: Compass.cpp

Revision:

12:12369ee344ab

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Compass.cpp	Sun Sep 27 11:04:54 2015 +0000
@@ -0,0 +1,115 @@
+#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);
+
+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;
+
+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    ", 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);
+    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); 
+}
+
+
+void compass_thread(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
+    if(l++ >= 50) { // take car of business once a second
+        seconds++;
+        sflag = 1;
+        compass.calibrate();
+        debug_print();
+        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 compass_main() {
+
+
+RtosTimer compass_timer(compass_thread, osTimerPeriodic);
+
+//cdebug = 1;  // uncomment to disable compass
+printf("\r\n\n\n\n\n\n\n");
+printf("Who AM I= %X\r\n", acc.whoAmI());
+acc.enable();
+
+
+acc.getAxis( acc_raw);
+mag.getAxis( mag_raw);
+
+compass_timer.start(20); // Run the Compass every 20ms
+    while(1) {
+        Thread::wait(osWaitForever);
+    }
+}