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
Diff: main.cpp
- Revision:
- 2:51f3303cbefd
- Parent:
- 0:32e37c82ef4a
- Child:
- 3:d6404f10bd3b
--- a/main.cpp Mon Apr 14 17:17:00 2014 +0000 +++ b/main.cpp Fri May 09 16:23:09 2014 +0000 @@ -2,36 +2,46 @@ #include "FXOS8700Q.h" #include "eCompass_Lib.h" #include "rtos.h" +//#include "MotionSensorDtypes.h" - -FXOS8700Q combo1( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); +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); +//void compass_thread(void const *argument); + + + 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; -void hal_map( int16_t * acc, int16_t * mag) +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[0]; -acc[0] = acc[1] * -1; -acc[1] = t * -1; +t = acc_raw->x; +acc_raw->x = acc_raw->y * -1; +acc_raw->y = t * -1; // swap mag X & Y axis -t = mag[0]; -mag[0] = mag[1]; -mag[1] = t; +t = mag_raw->x; +mag_raw->x = mag_raw->y; +mag_raw->y = t; // negate mag Z axis -mag[2] *= -1; +mag_raw->z *= -1; } // @@ -48,46 +58,68 @@ void compass_thread(void const *argument) { - static int16_t acc_raw[3], mag_raw[3]; + // get raw data from the sensors - combo1.AccXYZraw( acc_raw); - combo1.MagXYZraw( mag_raw); + acc.getAxis( acc_raw); + mag.getAxis( mag_raw); if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass if(l++ >= 50) { // take car of business once a second seconds++; - debug_print(); - compass.calibrate(); // re-calibrate the eCompass every second + sflag = 1; + compass.calibrate(); l = 0; led = !led; } tcount++; } - + +/* +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 main() { -uint8_t d[2]; -int16_t acc_raw[3], mag_raw[3]; -d[0] = 0; + +//Thread calibrate(calibrate_thread); +Thread dprint(print_thread); +//RtosTimer compass_timer(compass_thread, osTimerPeriodic); + //cdebug = 1; // uncomment to disable compass printf("\r\n\n\n\n\n\n\n"); -combo1.enable(); -combo1.readRegs( FXOS8700Q_STATUS, d, 1); -printf("Status= %X\r\n", d[0]); -combo1.readRegs( FXOS8700Q_WHOAMI, d, 1); -printf("Who AM I= %X\r\n", d[0]); -combo1.readRegs( FXOS8700Q_XYZ_DATA_CFG, d, 1); -printf("XYZ Cfg= %X\r\n", d[0]); -combo1.readRegs( FXOS8700Q_CTRL_REG1, d, 1); -printf("CTRL REG1= %X\r\n", d[0]); -combo1.readRegs( FXOS8700Q_M_CTRL_REG1, d, 1); -printf("MAG CTRL REG1= %X\r\n", d[0]); -combo1.readRegs( FXOS8700Q_M_CTRL_REG2, d, 1); -printf("Mag CTRL REG2= %X\r\n\r\n", d[0]); +acc.enable(); +printf("Who AM I= %X\r\n", acc.whoAmI()); + +acc.getAxis( acc_raw); +mag.getAxis( mag_raw); + +dprint.set_priority(osPriorityLow); +//calibrate.set_priority(osPriorityBelowNormal); +//compass_timer.set_priority(osPriorityRealtime); -combo1.AccXYZraw( acc_raw); -combo1.MagXYZraw( mag_raw); - -RtosTimer compass_timer(compass_thread, osTimerPeriodic); -compass_timer.start(20); // Run the Compass every 20ms -Thread::wait(osWaitForever); +//compass_timer.start(20); // Run the Compass every 20ms +while(1) { + //while(!sflag); + sflag = 0; + //debug_print(); + printf("\r\nL\r\n"); + //calibrate.signal_set(0x1); + dprint.signal_set(0x1); + wait(1000); + } }