K64F based data logger for GPS (ublox MAX M8Q) & 6 Axis Sensor (FXOS8700Q) - Outputs to SD + UDP - Uses FRDM K64F + ublox "Cellular and positioning shield" (3G version)
Dependencies: MAX_M8Q_Capture EthernetInterface FXOS8700Q SDFileSystem eCompass_FPU_Lib mbed-rtos mbed
compass.cpp@3:6085916c9d74, 2015-05-20 (annotated)
- Committer:
- rlinnmoran
- Date:
- Wed May 20 07:14:22 2015 +0000
- Revision:
- 3:6085916c9d74
- Parent:
- 2:bcd60a69583f
First release V1_0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rlinnmoran | 0:77857a36b4ff | 1 | #include "mbed.h" |
rlinnmoran | 0:77857a36b4ff | 2 | #include "FXOS8700Q.h" |
rlinnmoran | 0:77857a36b4ff | 3 | #include "eCompass_Lib.h" |
rlinnmoran | 0:77857a36b4ff | 4 | #include "rtos.h" |
rlinnmoran | 2:bcd60a69583f | 5 | #include "debug.h" |
rlinnmoran | 0:77857a36b4ff | 6 | //#include "MotionSensorDtypes.h" |
rlinnmoran | 0:77857a36b4ff | 7 | |
rlinnmoran | 0:77857a36b4ff | 8 | FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); |
rlinnmoran | 0:77857a36b4ff | 9 | FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); |
rlinnmoran | 0:77857a36b4ff | 10 | //Serial pc(USBTX, USBRX); |
rlinnmoran | 0:77857a36b4ff | 11 | DigitalOut gpo(D0); |
rlinnmoran | 0:77857a36b4ff | 12 | //DigitalOut led(LED_RED); |
rlinnmoran | 0:77857a36b4ff | 13 | eCompass compass; |
rlinnmoran | 0:77857a36b4ff | 14 | |
rlinnmoran | 0:77857a36b4ff | 15 | //void calibrate_thread(void const *argument); |
rlinnmoran | 0:77857a36b4ff | 16 | //void print_thread(void const *argument); |
rlinnmoran | 0:77857a36b4ff | 17 | int captureCompass(void const *argument); |
rlinnmoran | 0:77857a36b4ff | 18 | int eCompass_init(void); |
rlinnmoran | 0:77857a36b4ff | 19 | |
rlinnmoran | 0:77857a36b4ff | 20 | |
rlinnmoran | 0:77857a36b4ff | 21 | extern axis6_t axis6; |
rlinnmoran | 0:77857a36b4ff | 22 | extern uint32_t seconds; |
rlinnmoran | 0:77857a36b4ff | 23 | extern uint32_t compass_type; // optional, NED compass is default |
rlinnmoran | 0:77857a36b4ff | 24 | extern int32_t tcount; |
rlinnmoran | 0:77857a36b4ff | 25 | extern uint8_t cdebug; |
rlinnmoran | 0:77857a36b4ff | 26 | int l = 0; |
rlinnmoran | 0:77857a36b4ff | 27 | volatile int sflag = 0; |
rlinnmoran | 0:77857a36b4ff | 28 | |
rlinnmoran | 0:77857a36b4ff | 29 | MotionSensorDataCounts mag_raw; |
rlinnmoran | 0:77857a36b4ff | 30 | MotionSensorDataCounts acc_raw; |
rlinnmoran | 0:77857a36b4ff | 31 | |
rlinnmoran | 0:77857a36b4ff | 32 | void hal_map( MotionSensorDataCounts * acc_raw, MotionSensorDataCounts * mag_raw) |
rlinnmoran | 0:77857a36b4ff | 33 | { |
rlinnmoran | 0:77857a36b4ff | 34 | int16_t t; |
rlinnmoran | 0:77857a36b4ff | 35 | // swap and negate X & Y axis |
rlinnmoran | 0:77857a36b4ff | 36 | t = acc_raw->x; |
rlinnmoran | 0:77857a36b4ff | 37 | acc_raw->x = acc_raw->y * -1; |
rlinnmoran | 0:77857a36b4ff | 38 | acc_raw->y = t * -1; |
rlinnmoran | 0:77857a36b4ff | 39 | // swap mag X & Y axis |
rlinnmoran | 0:77857a36b4ff | 40 | t = mag_raw->x; |
rlinnmoran | 0:77857a36b4ff | 41 | mag_raw->x = mag_raw->y; |
rlinnmoran | 0:77857a36b4ff | 42 | mag_raw->y = t; |
rlinnmoran | 0:77857a36b4ff | 43 | // negate mag Z axis |
rlinnmoran | 0:77857a36b4ff | 44 | mag_raw->z *= -1; |
rlinnmoran | 0:77857a36b4ff | 45 | } |
rlinnmoran | 0:77857a36b4ff | 46 | |
rlinnmoran | 0:77857a36b4ff | 47 | // |
rlinnmoran | 0:77857a36b4ff | 48 | // Print data values for debug |
rlinnmoran | 0:77857a36b4ff | 49 | // |
rlinnmoran | 0:77857a36b4ff | 50 | void debug_print(void) |
rlinnmoran | 0:77857a36b4ff | 51 | { |
rlinnmoran | 0:77857a36b4ff | 52 | // Some useful printf statements for debug |
rlinnmoran | 2:bcd60a69583f | 53 | D(printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw)); |
rlinnmoran | 2:bcd60a69583f | 54 | D(printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f \r\n", axis6.fGax, axis6.fGay, axis6.fGaz)); |
rlinnmoran | 2:bcd60a69583f | 55 | D(printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz)); |
rlinnmoran | 2:bcd60a69583f | 56 | D(printf("Quaternion: Q0= %1.4f Q1= %1.4f Q2= %1.4f Q3= %1.4f\r\n", axis6.q0, axis6.q1, axis6.q2, axis6.q3)); |
rlinnmoran | 0:77857a36b4ff | 57 | |
rlinnmoran | 0:77857a36b4ff | 58 | |
rlinnmoran | 0:77857a36b4ff | 59 | } |
rlinnmoran | 0:77857a36b4ff | 60 | |
rlinnmoran | 0:77857a36b4ff | 61 | |
rlinnmoran | 0:77857a36b4ff | 62 | int captureCompass(void const *argument) { |
rlinnmoran | 0:77857a36b4ff | 63 | |
rlinnmoran | 0:77857a36b4ff | 64 | // get raw data from the sensors |
rlinnmoran | 0:77857a36b4ff | 65 | acc.getAxis( acc_raw); |
rlinnmoran | 0:77857a36b4ff | 66 | mag.getAxis( mag_raw); |
rlinnmoran | 0:77857a36b4ff | 67 | if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass |
rlinnmoran | 0:77857a36b4ff | 68 | debug_print(); |
rlinnmoran | 0:77857a36b4ff | 69 | if(l++ >= 50) { // take car of business once a second |
rlinnmoran | 0:77857a36b4ff | 70 | seconds++; |
rlinnmoran | 0:77857a36b4ff | 71 | sflag = 1; |
rlinnmoran | 0:77857a36b4ff | 72 | compass.calibrate(); |
rlinnmoran | 0:77857a36b4ff | 73 | l = 0; |
rlinnmoran | 0:77857a36b4ff | 74 | //led = !led; |
rlinnmoran | 0:77857a36b4ff | 75 | } |
rlinnmoran | 0:77857a36b4ff | 76 | tcount++; |
rlinnmoran | 0:77857a36b4ff | 77 | return(0); |
rlinnmoran | 0:77857a36b4ff | 78 | } |
rlinnmoran | 0:77857a36b4ff | 79 | |
rlinnmoran | 0:77857a36b4ff | 80 | /* |
rlinnmoran | 0:77857a36b4ff | 81 | void calibrate_thread(void const *argument) { |
rlinnmoran | 0:77857a36b4ff | 82 | while (true) { |
rlinnmoran | 0:77857a36b4ff | 83 | // Signal flags that are reported as event are automatically cleared. |
rlinnmoran | 0:77857a36b4ff | 84 | Thread::signal_wait(0x1); |
rlinnmoran | 0:77857a36b4ff | 85 | compass.calibrate(); // re-calibrate the eCompass every second |
rlinnmoran | 0:77857a36b4ff | 86 | } |
rlinnmoran | 0:77857a36b4ff | 87 | } |
rlinnmoran | 0:77857a36b4ff | 88 | |
rlinnmoran | 0:77857a36b4ff | 89 | |
rlinnmoran | 0:77857a36b4ff | 90 | |
rlinnmoran | 0:77857a36b4ff | 91 | void print_thread(void const *argument) { |
rlinnmoran | 0:77857a36b4ff | 92 | while (true) { |
rlinnmoran | 0:77857a36b4ff | 93 | // Signal flags that are reported as event are automatically cleared. |
rlinnmoran | 0:77857a36b4ff | 94 | Thread::signal_wait(0x1); |
rlinnmoran | 0:77857a36b4ff | 95 | debug_print(); // re-calibrate the eCompass every second |
rlinnmoran | 0:77857a36b4ff | 96 | } |
rlinnmoran | 0:77857a36b4ff | 97 | } |
rlinnmoran | 0:77857a36b4ff | 98 | */ |
rlinnmoran | 0:77857a36b4ff | 99 | |
rlinnmoran | 0:77857a36b4ff | 100 | int eCompass_init() { |
rlinnmoran | 0:77857a36b4ff | 101 | |
rlinnmoran | 0:77857a36b4ff | 102 | acc.enable(); |
rlinnmoran | 0:77857a36b4ff | 103 | |
rlinnmoran | 0:77857a36b4ff | 104 | return 0; |
rlinnmoran | 0:77857a36b4ff | 105 | } |