default eCompass example, but with desired serial communication
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed
Fork of KL46_eCompass by
main.cpp
- Committer:
- Kosinkadink
- Date:
- 2016-04-08
- Revision:
- 7:d932835d9613
- Parent:
- 6:f68052364f92
File content as of revision 7:d932835d9613:
#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) { //pc.printf("0,%d$",axis6.yaw); pc.printf("%4.2f,%4.2f$",axis6.fUTmx, axis6.fUTmy); //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); } // 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); //red = 0; //compass.calibrate(); //red = 1; } } void print_thread(void const *argument) { while (true) { // Signal flags that are reported as event are automatically cleared. //Thread::signal_wait(0x1); if (pc.getc()) { debug_print(); } } } void lcd_thread(void const *argument) { while (true) { // Signal flags that are reported as event are automatically cleared. Thread::signal_wait(0x04); slcd.printf("%04d", axis6.yaw); } } 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(); // 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 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); }