Ragnarok
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed
Fork of KL46_eCompass by
main.cpp
00001 #include "mbed.h" 00002 #include "eCompass_Lib.h" 00003 #include "MAG3110.h" 00004 #include "MMA8451Q.h" 00005 #include "rtos.h" 00006 #include "SLCD.h" 00007 00008 #define MMA8451_I2C_ADDRESS (0x1d<<1) 00009 00010 00011 eCompass compass; 00012 MAG3110 mag( PTE25, PTE24); 00013 MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS); 00014 DigitalOut red(LED_RED); 00015 DigitalOut green(LED_GREEN); 00016 Serial pc(USBTX, USBRX); 00017 SLCD slcd; 00018 00019 extern axis6_t axis6; 00020 extern uint32_t seconds; 00021 extern uint32_t compass_type; 00022 extern int32_t tcount; 00023 extern uint8_t cdebug; 00024 00025 MotionSensorDataCounts mag_raw; 00026 MotionSensorDataCounts acc_raw; 00027 00028 Thread *(debugp); 00029 Thread *(calibrate); 00030 Thread *(lcdp); 00031 00032 // 00033 // Print data values for debug 00034 // 00035 void debug_print(void) 00036 { 00037 int h, m, s; 00038 h = seconds / 3600; 00039 m = (seconds % 3600) / 60; 00040 s = seconds % 60; 00041 // Some useful printf statements for debug 00042 printf("Runtime= %d:%02d:%02d\r\n", h, m, s); 00043 printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw); 00044 printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz); 00045 printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz); 00046 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); 00047 } 00048 // HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors 00049 // 00050 // This routing move and negates data as needed the 00051 // properly align the sensor axis for our desired compass (NED) 00052 // For more information see Freescale appication note AN4696 00053 // 00054 void hal_map( MotionSensorDataCounts * acc_data, MotionSensorDataCounts * mag_data) 00055 { 00056 int16_t t; 00057 // swap and negate accelerometer x & y 00058 t = acc_data->y; 00059 acc_data->y = acc_data->x * -1; 00060 acc_data->x = t * -1; 00061 00062 // negate magnetometer y 00063 mag_data->y *= -1; 00064 00065 } 00066 00067 // 00068 // This is the 50Hz thread where the magic happens 00069 // 00070 int l = 0; 00071 00072 void compass_thread(void const *argument) { 00073 // get raw data from the sensors 00074 mag.getAxis(mag_raw); 00075 acc.getAxis(acc_raw); 00076 if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass 00077 if(!(l % 10)) lcdp->signal_set(0x04); 00078 if(l++ >= 50) { // take car of business once a second 00079 seconds++; 00080 calibrate->signal_set(0x2); 00081 debugp->signal_set(0x01); 00082 l = 0; 00083 green = !green; 00084 } 00085 tcount++; 00086 } 00087 00088 00089 void calibrate_thread(void const *argument) { 00090 while(true) { 00091 Thread::signal_wait(0x2); 00092 red = 0; 00093 compass.calibrate(); 00094 red = 1; 00095 } 00096 } 00097 00098 00099 void print_thread(void const *argument) { 00100 while (true) { 00101 // Signal flags that are reported as event are automatically cleared. 00102 Thread::signal_wait(0x1); 00103 debug_print(); 00104 } 00105 } 00106 00107 00108 void lcd_thread(void const *argument) { 00109 while (true) { 00110 // Signal flags that are reported as event are automatically cleared. 00111 Thread::signal_wait(0x4); 00112 slcd.printf("%04d", axis6.roll); // print the heading (NED compass) to the LCD 00113 } 00114 } 00115 00116 int main() { 00117 00118 slcd.clear(); 00119 tcount = 0; 00120 red = 1; 00121 green = 1; 00122 //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data. 00123 compass_type = NED_COMPASS; 00124 seconds = 0; 00125 Thread t1(print_thread); 00126 Thread t2(calibrate_thread); 00127 Thread t3(lcd_thread); 00128 debugp = &t1; 00129 calibrate = &t2; 00130 lcdp = &t3; 00131 mag.enable(); 00132 acc.enable(); 00133 // Say hello to our sensors 00134 // Native KL46-FRDM sensors 00135 printf("\r\nMMA8451Q ID= %X\r\n", acc.whoAmI()); 00136 printf("MAG3110 ID= %X\r\n", mag.whoAmI()); 00137 mag.getAxis(mag_raw); // flush the magnetmeter 00138 RtosTimer compass_timer(compass_thread, osTimerPeriodic); 00139 00140 /* 00141 * Thread Priorities 00142 * Compass Thread, High Priority 00143 * Compass calibration, Above Normal 00144 * LED Update, Normal 00145 * printf to console, Below Normal 00146 * main(), Normal 00147 */ 00148 00149 debugp->set_priority(osPriorityBelowNormal); 00150 lcdp->set_priority(osPriorityLow); 00151 calibrate->set_priority(osPriorityAboveNormal); 00152 compass_timer.start(20); // Run the Compass every 20ms 00153 00154 Thread::wait(osWaitForever); 00155 }
Generated on Thu Jul 14 2022 12:50:04 by 1.7.2