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:
- jun1x
- Date:
- 2018-02-05
- Revision:
- 6:95bced85b8e5
- Parent:
- 5:7b95c2f8e76e
File content as of revision 6:95bced85b8e5:
#include "mbed.h" #include "mbed_events.h" #include "eCompass_Lib.h" #include "MAG3110.h" #include "MMA8451Q.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 Priorities * Compass Thread, High Priority * Compass calibration, Above Normal * Console/LCD Update, Below Normal * main(), Normal */ Thread threadCalibrate(osPriorityAboveNormal); Thread threadCompass(osPriorityHigh); Thread threadDebug(osPriorityBelowNormal); EventQueue queueCalibrate; EventQueue queueCompass; EventQueue queueDebug; // 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() { // 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++ >= 50) { // take car of business once a second seconds++; l = 0; green = !green; } tcount++; } // // Recalibrate compass at 1Hz // void calibrate_thread() { red = !red; compass.calibrate(); } // // Print data values for debug at 1Hz // void debug_thread(void) { int h, m, s; h = seconds / 3600; m = (seconds % 3600) / 60; s = seconds % 60; // Some useful printf statements for debug printf("Runtime= %d:%02d:%02d\r\n", h, m, s); 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\n", axis6.q0, axis6.q1, axis6.q2, axis6.q3); } // // Update LCD at 5Hz // void lcd_thread() { slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD } int main() { slcd.clear(); red = 1; green = 1; seconds = 0; compass_type = NED_COMPASS; tcount = 0; cdebug = 0; // Set to 1 to disable eCompass in order to observe raw mag data. mag.enable(); acc.enable(); // Say hello to our sensors // 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 // Events on the queues are called periodically, interval in milliseconds queueCalibrate.call_every(1000, calibrate_thread); queueCompass.call_every(20, compass_thread); queueDebug.call_every(200, lcd_thread); queueDebug.call_every(1000, debug_thread); // Queues run in different threads with different priorities threadCalibrate.start(callback(&queueCalibrate, &EventQueue::dispatch_forever)); threadCompass.start(callback(&queueCompass, &EventQueue::dispatch_forever)); threadDebug.start(callback(&queueDebug, &EventQueue::dispatch_forever)); }