This program uses the MMA8451 & MAG3110 on a KL-46 freedom board to implement a tilt compensated eCompass with the heading displayed on the LCD. This program uses the mbed RTOS to manage the hard real time requirements of the eCompass algorithm (Updated to mbed-os 5.x using eventqueues)
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib
Fork of KL46_eCompass by
main.cpp
- Committer:
- JimCarver
- Date:
- 2014-05-16
- Revision:
- 3:0770c275e6e8
- Parent:
- 2:e4ae1d748311
- Child:
- 4:ba1dbfb683fb
File content as of revision 3:0770c275e6e8:
#include "mbed.h" #include "eCompass_Lib.h" #include "MAG3110.h" #include "MMA8451Q.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); 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; MotionSensorDataCounts mag_raw; MotionSensorDataCounts acc_raw; Thread *(debugp); Thread *(calibrate); Thread *(lcdp); // // 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( MotionSensorDataCounts * acc_data, MotionSensorDataCounts * mag_data) { int16_t t; // swap and negate accelerometer x & y t = acc_data->y; acc_data->y = acc_data->x * -1; acc_data->x = t * -1; // negate magnetometer y mag_data->y *= -1; } // // This is the 50Hz thread where the magic happens // int l = 0; void compass_thread(void const *argument) { // get raw data from the sensors mag.getAxis(mag_raw); acc.getAxis(acc_raw); if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass if(!(l % 10)) lcdp->signal_set(0x04); if(l++ >= 50) { // take car of business once a second seconds++; calibrate->signal_set(0x2); debugp->signal_set(0x01); l = 0; green = !green; } tcount++; } void calibrate_thread(void const *argument) { while(true) { Thread::signal_wait(0x2); compass.calibrate(); } } void print_thread(void const *argument) { while (true) { // Signal flags that are reported as event are automatically cleared. Thread::signal_wait(0x1); debug_print(); } } void lcd_thread(void const *argument) { while (true) { // Signal flags that are reported as event are automatically cleared. Thread::signal_wait(0x4); slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD } } int main() { 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; Thread t1(print_thread); Thread t2(calibrate_thread); Thread t3(lcd_thread); debugp = &t1; calibrate = &t2; lcdp = &t3; mag.enable(); acc.enable(); // 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.whoAmI()); printf("MAG3110 ID= %X\r\n", mag.whoAmI()); mag.getAxis(mag_raw); // flush the magnetmeter RtosTimer compass_timer(compass_thread, osTimerPeriodic); /* * Thread Priorities * Compass Thread, High Priority * Compass calibration, Above Normal * LED Update, Normal * printf to console, Below Normal * main(), Normal */ debugp->set_priority(osPriorityBelowNormal); lcdp->set_priority(osPriorityLow); calibrate->set_priority(osPriorityAboveNormal); compass_timer.start(20); // Run the Compass every 20ms Thread::wait(osWaitForever); }