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
- Committer:
- rlinnmoran
- Date:
- 2015-04-02
- Revision:
- 2:bcd60a69583f
- Parent:
- 0:77857a36b4ff
File content as of revision 2:bcd60a69583f:
#include "mbed.h" #include "FXOS8700Q.h" #include "eCompass_Lib.h" #include "rtos.h" #include "debug.h" //#include "MotionSensorDtypes.h" FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); //Serial pc(USBTX, USBRX); DigitalOut gpo(D0); //DigitalOut led(LED_RED); eCompass compass; //void calibrate_thread(void const *argument); //void print_thread(void const *argument); int captureCompass(void const *argument); int eCompass_init(void); 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; volatile int sflag = 0; MotionSensorDataCounts mag_raw; MotionSensorDataCounts acc_raw; void hal_map( MotionSensorDataCounts * acc_raw, MotionSensorDataCounts * mag_raw) { int16_t t; // swap and negate X & Y axis t = acc_raw->x; acc_raw->x = acc_raw->y * -1; acc_raw->y = t * -1; // swap mag X & Y axis t = mag_raw->x; mag_raw->x = mag_raw->y; mag_raw->y = t; // negate mag Z axis mag_raw->z *= -1; } // // Print data values for debug // void debug_print(void) { // Some useful printf statements for debug D(printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw)); D(printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f \r\n", axis6.fGax, axis6.fGay, axis6.fGaz)); D(printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz)); 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)); } int captureCompass(void const *argument) { // get raw data from the sensors acc.getAxis( acc_raw); mag.getAxis( mag_raw); if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass debug_print(); if(l++ >= 50) { // take car of business once a second seconds++; sflag = 1; compass.calibrate(); l = 0; //led = !led; } tcount++; return(0); } /* void calibrate_thread(void const *argument) { while (true) { // Signal flags that are reported as event are automatically cleared. Thread::signal_wait(0x1); compass.calibrate(); // re-calibrate the eCompass every second } } void print_thread(void const *argument) { while (true) { // Signal flags that are reported as event are automatically cleared. Thread::signal_wait(0x1); debug_print(); // re-calibrate the eCompass every second } } */ int eCompass_init() { acc.enable(); return 0; }