Jim Carver
This is an example of a tilt compensated eCompass running on a K64F Freedom Platform using the on board FXOS8700 6 axis sensor and Freescale's eCompass software library in a linkable object library compiled for a FPU enabled Cortex M4. This version also used the LCD in an App Shield to display navigation data.
Dependencies: C12832 FXOS8700Q eCompass_FPU_Lib mbed-rtos mbed
Fork of
K64F_eCompass
by 
Jim Carver
Diff: main.cpp
- Revision:
- 0:32e37c82ef4a
- Child:
- 2:770f77c92bab
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Apr 14 17:15:09 2014 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "FXOS8700Q.h"
+#include "eCompass_Lib.h"
+#include "rtos.h"
+
+
+
+FXOS8700Q combo1( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
+Serial pc(USBTX, USBRX);
+DigitalOut gpo(D0);
+DigitalOut led(LED_RED);
+eCompass compass;
+
+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;
+
+
+void hal_map( int16_t * acc, int16_t * mag)
+{
+int16_t t;
+// swap and negate X & Y axis
+t = acc[0];
+acc[0] = acc[1] * -1;
+acc[1] = t * -1;
+// swap mag X & Y axis
+t = mag[0];
+mag[0] = mag[1];
+mag[1] = t;
+// negate mag Z axis
+mag[2] *= -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) {
+ static int16_t acc_raw[3], mag_raw[3];
+ // get raw data from the sensors
+ combo1.AccXYZraw( acc_raw);
+ combo1.MagXYZraw( 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
+ l = 0;
+ led = !led;
+ }
+ tcount++;
+}
+
+int main() {
+
+uint8_t d[2];
+int16_t acc_raw[3], mag_raw[3];
+d[0] = 0;
+//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]);
+
+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);
+}