Robert Linn-Moran / Mbed 2 deprecated GPS_6Axis_DataLogger_SD_UDP

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?

UserRevisionLine numberNew 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 }