Jim Carver / Mbed 2 deprecated K64F_eCompass Featured

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.

Dependencies:   FXOS8700Q eCompass_FPU_Lib mbed-rtos mbed

main.cpp@2:51f3303cbefd, 2014-05-09 (annotated)

Committer:
JimCarver
Date:
Fri May 09 16:23:09 2014 +0000
Revision:
2:51f3303cbefd
Parent:
0:32e37c82ef4a
Child:
3:d6404f10bd3b
Updated to use latest FXOS8700 library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JimCarver 0:32e37c82ef4a 1 #include "mbed.h"
JimCarver 0:32e37c82ef4a 2 #include "FXOS8700Q.h"
JimCarver 0:32e37c82ef4a 3 #include "eCompass_Lib.h"
JimCarver 0:32e37c82ef4a 4 #include "rtos.h"
JimCarver 2:51f3303cbefd 5 //#include "MotionSensorDtypes.h"
JimCarver 0:32e37c82ef4a 6
JimCarver 0:32e37c82ef4a 7
JimCarver 2:51f3303cbefd 8 FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
JimCarver 2:51f3303cbefd 9 FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
JimCarver 0:32e37c82ef4a 10 Serial pc(USBTX, USBRX);
JimCarver 0:32e37c82ef4a 11 DigitalOut gpo(D0);
JimCarver 0:32e37c82ef4a 12 DigitalOut led(LED_RED);
JimCarver 0:32e37c82ef4a 13 eCompass compass;
JimCarver 0:32e37c82ef4a 14
JimCarver 2:51f3303cbefd 15 //void calibrate_thread(void const *argument);
JimCarver 2:51f3303cbefd 16 //void print_thread(void const *argument);
JimCarver 2:51f3303cbefd 17 //void compass_thread(void const *argument);
JimCarver 2:51f3303cbefd 18
JimCarver 2:51f3303cbefd 19
JimCarver 2:51f3303cbefd 20
JimCarver 0:32e37c82ef4a 21 extern axis6_t axis6;
JimCarver 0:32e37c82ef4a 22 extern uint32_t seconds;
JimCarver 0:32e37c82ef4a 23 extern uint32_t compass_type; // optional, NED compass is default
JimCarver 0:32e37c82ef4a 24 extern int32_t tcount;
JimCarver 0:32e37c82ef4a 25 extern uint8_t cdebug;
JimCarver 0:32e37c82ef4a 26 int l = 0;
JimCarver 2:51f3303cbefd 27 volatile int sflag = 0;
JimCarver 0:32e37c82ef4a 28
JimCarver 2:51f3303cbefd 29 MotionSensorDataCounts mag_raw;
JimCarver 2:51f3303cbefd 30 MotionSensorDataCounts acc_raw;
JimCarver 2:51f3303cbefd 31
JimCarver 2:51f3303cbefd 32 void hal_map( MotionSensorDataCounts * acc_raw, MotionSensorDataCounts * mag_raw)
JimCarver 0:32e37c82ef4a 33 {
JimCarver 0:32e37c82ef4a 34 int16_t t;
JimCarver 0:32e37c82ef4a 35 // swap and negate X & Y axis
JimCarver 2:51f3303cbefd 36 t = acc_raw->x;
JimCarver 2:51f3303cbefd 37 acc_raw->x = acc_raw->y * -1;
JimCarver 2:51f3303cbefd 38 acc_raw->y = t * -1;
JimCarver 0:32e37c82ef4a 39 // swap mag X & Y axis
JimCarver 2:51f3303cbefd 40 t = mag_raw->x;
JimCarver 2:51f3303cbefd 41 mag_raw->x = mag_raw->y;
JimCarver 2:51f3303cbefd 42 mag_raw->y = t;
JimCarver 0:32e37c82ef4a 43 // negate mag Z axis
JimCarver 2:51f3303cbefd 44 mag_raw->z *= -1;
JimCarver 0:32e37c82ef4a 45 }
JimCarver 0:32e37c82ef4a 46
JimCarver 0:32e37c82ef4a 47 //
JimCarver 0:32e37c82ef4a 48 // Print data values for debug
JimCarver 0:32e37c82ef4a 49 //
JimCarver 0:32e37c82ef4a 50 void debug_print(void)
JimCarver 0:32e37c82ef4a 51 {
JimCarver 0:32e37c82ef4a 52 // Some useful printf statements for debug
JimCarver 0:32e37c82ef4a 53 printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
JimCarver 0:32e37c82ef4a 54 printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz);
JimCarver 0:32e37c82ef4a 55 printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
JimCarver 0:32e37c82ef4a 56 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);
JimCarver 0:32e37c82ef4a 57 }
JimCarver 0:32e37c82ef4a 58
JimCarver 0:32e37c82ef4a 59
JimCarver 0:32e37c82ef4a 60 void compass_thread(void const *argument) {
JimCarver 2:51f3303cbefd 61
JimCarver 0:32e37c82ef4a 62 // get raw data from the sensors
JimCarver 2:51f3303cbefd 63 acc.getAxis( acc_raw);
JimCarver 2:51f3303cbefd 64 mag.getAxis( mag_raw);
JimCarver 0:32e37c82ef4a 65 if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
JimCarver 0:32e37c82ef4a 66 if(l++ >= 50) { // take car of business once a second
JimCarver 0:32e37c82ef4a 67 seconds++;
JimCarver 2:51f3303cbefd 68 sflag = 1;
JimCarver 2:51f3303cbefd 69 compass.calibrate();
JimCarver 0:32e37c82ef4a 70 l = 0;
JimCarver 0:32e37c82ef4a 71 led = !led;
JimCarver 0:32e37c82ef4a 72 }
JimCarver 0:32e37c82ef4a 73 tcount++;
JimCarver 0:32e37c82ef4a 74 }
JimCarver 2:51f3303cbefd 75
JimCarver 2:51f3303cbefd 76 /*
JimCarver 2:51f3303cbefd 77 void calibrate_thread(void const *argument) {
JimCarver 2:51f3303cbefd 78 while (true) {
JimCarver 2:51f3303cbefd 79 // Signal flags that are reported as event are automatically cleared.
JimCarver 2:51f3303cbefd 80 Thread::signal_wait(0x1);
JimCarver 2:51f3303cbefd 81 compass.calibrate(); // re-calibrate the eCompass every second
JimCarver 2:51f3303cbefd 82 }
JimCarver 2:51f3303cbefd 83 }
JimCarver 2:51f3303cbefd 84
JimCarver 2:51f3303cbefd 85 */
JimCarver 2:51f3303cbefd 86
JimCarver 2:51f3303cbefd 87 void print_thread(void const *argument) {
JimCarver 2:51f3303cbefd 88 while (true) {
JimCarver 2:51f3303cbefd 89 // Signal flags that are reported as event are automatically cleared.
JimCarver 2:51f3303cbefd 90 Thread::signal_wait(0x1);
JimCarver 2:51f3303cbefd 91 debug_print(); // re-calibrate the eCompass every second
JimCarver 2:51f3303cbefd 92 }
JimCarver 2:51f3303cbefd 93 }
JimCarver 2:51f3303cbefd 94
JimCarver 2:51f3303cbefd 95
JimCarver 0:32e37c82ef4a 96 int main() {
JimCarver 0:32e37c82ef4a 97
JimCarver 2:51f3303cbefd 98
JimCarver 2:51f3303cbefd 99 //Thread calibrate(calibrate_thread);
JimCarver 2:51f3303cbefd 100 Thread dprint(print_thread);
JimCarver 2:51f3303cbefd 101 //RtosTimer compass_timer(compass_thread, osTimerPeriodic);
JimCarver 2:51f3303cbefd 102
JimCarver 0:32e37c82ef4a 103 //cdebug = 1; // uncomment to disable compass
JimCarver 0:32e37c82ef4a 104 printf("\r\n\n\n\n\n\n\n");
JimCarver 2:51f3303cbefd 105 acc.enable();
JimCarver 2:51f3303cbefd 106 printf("Who AM I= %X\r\n", acc.whoAmI());
JimCarver 2:51f3303cbefd 107
JimCarver 2:51f3303cbefd 108 acc.getAxis( acc_raw);
JimCarver 2:51f3303cbefd 109 mag.getAxis( mag_raw);
JimCarver 2:51f3303cbefd 110
JimCarver 2:51f3303cbefd 111 dprint.set_priority(osPriorityLow);
JimCarver 2:51f3303cbefd 112 //calibrate.set_priority(osPriorityBelowNormal);
JimCarver 2:51f3303cbefd 113 //compass_timer.set_priority(osPriorityRealtime);
JimCarver 0:32e37c82ef4a 114
JimCarver 2:51f3303cbefd 115 //compass_timer.start(20); // Run the Compass every 20ms
JimCarver 2:51f3303cbefd 116 while(1) {
JimCarver 2:51f3303cbefd 117 //while(!sflag);
JimCarver 2:51f3303cbefd 118 sflag = 0;
JimCarver 2:51f3303cbefd 119 //debug_print();
JimCarver 2:51f3303cbefd 120 printf("\r\nL\r\n");
JimCarver 2:51f3303cbefd 121 //calibrate.signal_set(0x1);
JimCarver 2:51f3303cbefd 122 dprint.signal_set(0x1);
JimCarver 2:51f3303cbefd 123 wait(1000);
JimCarver 2:51f3303cbefd 124 }
JimCarver 0:32e37c82ef4a 125 }