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@3:d6404f10bd3b, 2014-05-09 (annotated)
- Committer:
- JimCarver
- Date:
- Fri May 09 16:30:47 2014 +0000
- Revision:
- 3:d6404f10bd3b
- Parent:
- 2:51f3303cbefd
Bug fix
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:d6404f10bd3b | 70 | debug_print(); |
JimCarver | 0:32e37c82ef4a | 71 | l = 0; |
JimCarver | 0:32e37c82ef4a | 72 | led = !led; |
JimCarver | 0:32e37c82ef4a | 73 | } |
JimCarver | 0:32e37c82ef4a | 74 | tcount++; |
JimCarver | 0:32e37c82ef4a | 75 | } |
JimCarver | 2:51f3303cbefd | 76 | |
JimCarver | 2:51f3303cbefd | 77 | /* |
JimCarver | 2:51f3303cbefd | 78 | void calibrate_thread(void const *argument) { |
JimCarver | 2:51f3303cbefd | 79 | while (true) { |
JimCarver | 2:51f3303cbefd | 80 | // Signal flags that are reported as event are automatically cleared. |
JimCarver | 2:51f3303cbefd | 81 | Thread::signal_wait(0x1); |
JimCarver | 2:51f3303cbefd | 82 | compass.calibrate(); // re-calibrate the eCompass every second |
JimCarver | 2:51f3303cbefd | 83 | } |
JimCarver | 2:51f3303cbefd | 84 | } |
JimCarver | 2:51f3303cbefd | 85 | |
JimCarver | 3:d6404f10bd3b | 86 | |
JimCarver | 2:51f3303cbefd | 87 | |
JimCarver | 2:51f3303cbefd | 88 | void print_thread(void const *argument) { |
JimCarver | 2:51f3303cbefd | 89 | while (true) { |
JimCarver | 2:51f3303cbefd | 90 | // Signal flags that are reported as event are automatically cleared. |
JimCarver | 2:51f3303cbefd | 91 | Thread::signal_wait(0x1); |
JimCarver | 2:51f3303cbefd | 92 | debug_print(); // re-calibrate the eCompass every second |
JimCarver | 2:51f3303cbefd | 93 | } |
JimCarver | 2:51f3303cbefd | 94 | } |
JimCarver | 3:d6404f10bd3b | 95 | */ |
JimCarver | 2:51f3303cbefd | 96 | |
JimCarver | 0:32e37c82ef4a | 97 | int main() { |
JimCarver | 0:32e37c82ef4a | 98 | |
JimCarver | 2:51f3303cbefd | 99 | |
JimCarver | 3:d6404f10bd3b | 100 | RtosTimer compass_timer(compass_thread, osTimerPeriodic); |
JimCarver | 2:51f3303cbefd | 101 | |
JimCarver | 0:32e37c82ef4a | 102 | //cdebug = 1; // uncomment to disable compass |
JimCarver | 0:32e37c82ef4a | 103 | printf("\r\n\n\n\n\n\n\n"); |
JimCarver | 3:d6404f10bd3b | 104 | printf("Who AM I= %X\r\n", acc.whoAmI()); |
JimCarver | 2:51f3303cbefd | 105 | acc.enable(); |
JimCarver | 3:d6404f10bd3b | 106 | |
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 | 3:d6404f10bd3b | 111 | compass_timer.start(20); // Run the Compass every 20ms |
JimCarver | 2:51f3303cbefd | 112 | while(1) { |
JimCarver | 3:d6404f10bd3b | 113 | Thread::wait(osWaitForever); |
JimCarver | 2:51f3303cbefd | 114 | } |
JimCarver | 0:32e37c82ef4a | 115 | } |