Ragnarok
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed
Fork of KL46_eCompass by
main.cpp
- Committer:
- JimCarver
- Date:
- 2014-04-13
- Revision:
- 2:e4ae1d748311
- Parent:
- 0:4e1d43dc608f
- Child:
- 3:0770c275e6e8
File content as of revision 2:e4ae1d748311:
#include "mbed.h" #include "eCompass_Lib.h" #include "MAG3110.h" #include "MMA8451Q.h" #include "FXOS8700Q.h" #include "rtos.h" #include "SLCD.h" #define MMA8451_I2C_ADDRESS (0x1d<<1) eCompass compass; MAG3110 mag( PTE25, PTE24); MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS); //FXOS8700Q combo0( A4, A5, FXOS8700CQ_SLAVE_ADDR0); DigitalOut red(LED_RED); DigitalOut green(LED_GREEN); Serial pc(USBTX, USBRX); SLCD slcd; extern axis6_t axis6; extern uint32_t seconds; extern uint32_t compass_type; extern int32_t tcount; extern uint8_t cdebug; // // 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", 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); } // HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors // // This routing move and negates data as needed the // properly align the sensor axis for our desired compass (NED) // For more information see Freescale appication note AN4696 // void hal_map(int16_t * acc, int16_t * mag) { int16_t t; // swap accelerometer x & y t = acc[1]; acc[1] = acc[0]; acc[0] = t; // negate accelerometer x & y acc[0] *= -1; acc[1] *= -1; // negate magnetometer y mag[1] *= -1; } /* // HAL Map for Multi-Sensor board with FXOS8700 sensor // // This routing move and negates data as needed the // properly align the sensor axis for our desired compass (NED) // For more information see Freescale appication note AN4696 // void hal_map( int16_t * acc, int16_t * mag) { int16_t t; // Insert your HAL map here t = acc[0]; acc[0] = acc[1] * -1; acc[1] = t * -1; t = mag[1]; mag[1] = mag[0]; mag[0] = t; mag[2] *= -1; } */ // // This is the 50Hz thread where the magic happens // int l = 0; void compass_thread(void const *argument) { static int16_t acc_raw[3], mag_raw[3]; // get raw data from the sensors mag.ReadXYZraw(mag_raw); acc.getAccXYZraw(acc_raw); //combo0.AccXYZraw( acc_raw); //combo0.MagXYZraw( mag_raw); if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD if(l++ >= 50) { // take car of business once a second seconds++; debug_print(); compass.calibrate(); // re-calibrate the eCompass every second l = 0; green = !green; } tcount++; } int main() { // Thread thread(compass_thread); int16_t mag_data[3]; slcd.clear(); tcount = 0; red = 1; green = 1; //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data. compass_type = NED_COMPASS; seconds = 0; // make sure our sensors are talking to us // Only available when multi-sensor shield is installed //printf("\r\nFXOS8700 ID= %X\r\n", combo0.getWhoAmI()); // NAtive KL46-FRDM sensors printf("\r\nMMA8451Q ID= %X\r\n", acc.getWhoAmI()); printf("MAG3110 ID= %X\r\n", mag.readReg(MAG_WHO_AM_I)); mag.ReadXYZraw(mag_data); // flush the magnetmeter RtosTimer compass_timer(compass_thread, osTimerPeriodic); // //RtosTimer b_timer(blink_green, osTimerPeriodic); compass_timer.start(20); // Run the Compass every 20ms //b_timer.start(250); //blink the green LED at 2Hz Thread::wait(osWaitForever); }