k64f_OneNET
Dependencies: EthernetInterface FXOS8700Q eCompass_FPU_Lib mbed-rtos mbed mbed
Fork of K64F_eCompass by
Diff: main.cpp
- Revision:
- 0:32e37c82ef4a
- Child:
- 2:51f3303cbefd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Apr 14 17:15:09 2014 +0000 @@ -0,0 +1,93 @@ +#include "mbed.h" +#include "FXOS8700Q.h" +#include "eCompass_Lib.h" +#include "rtos.h" + + + +FXOS8700Q combo1( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); +Serial pc(USBTX, USBRX); +DigitalOut gpo(D0); +DigitalOut led(LED_RED); +eCompass compass; + +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; + + +void hal_map( int16_t * acc, int16_t * mag) +{ +int16_t t; +// swap and negate X & Y axis +t = acc[0]; +acc[0] = acc[1] * -1; +acc[1] = t * -1; +// swap mag X & Y axis +t = mag[0]; +mag[0] = mag[1]; +mag[1] = t; +// negate mag Z axis +mag[2] *= -1; +} + +// +// Print data values for debug +// +void debug_print(void) +{ + // Some useful printf statements for debug + printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw); + printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz); + printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz); + 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); +} + + +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); + 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 + l = 0; + led = !led; + } + tcount++; +} + +int main() { + +uint8_t d[2]; +int16_t acc_raw[3], mag_raw[3]; +d[0] = 0; +//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]); + +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); +}