A basic eCompass using mbed-RTOS
Dependencies: C12832 FXOS8700Q eCompass_FPU_Lib mbed-rtos mbed
main.cpp
- Committer:
- JimCarver
- Date:
- 2014-05-05
- Revision:
- 0:2126a25ea273
- Child:
- 1:2d2270d1a5f5
File content as of revision 0:2126a25ea273:
#include "mbed.h" #include "FXOS8700Q.h" #include "eCompass_Lib.h" #include "rtos.h" #include "C12832.h" /* * * Changes which must be made to default rtos settings: * Default scheduler stack size changed to 2048 for target K64F (RTX_Conf_CM.h, line 66) * Default stack size changed to 1024 words for ARMCC Compiler (cmsis_os.h, line 121) * The Timer Thread must run at Real Time priority (RTX_Conf_CM.c, line 182) * */ extern Thread osTimerThread; FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); eCompass compass; DigitalOut red(D5); // RED LED on expansion shield DigitalOut green(D9); // GREEN LED on expansion shield DigitalOut blue(LED3); // BLUE LED on K64F Freedom board C12832 lcd(D11, D13, D12, D7, D10); // App shield LCD extern axis6_t axis6; extern uint32_t seconds; extern uint32_t compass_type; // optional, NED compass is default extern int32_t tcount; extern uint8_t cdebug; int l = 0; volatile int sflag = 0; MotionSensorDataCounts mag_raw; MotionSensorDataCounts acc_raw; Thread *(debugp); Thread *(calibrate); Thread *(lcdp); void hal_map( MotionSensorDataCounts * acc_raw, MotionSensorDataCounts * mag_raw) { int16_t t; // swap and negate X & Y axis t = acc_raw->x; acc_raw->x = acc_raw->y * -1; acc_raw->y = t * -1; // swap mag X & Y axis t = mag_raw->x; mag_raw->x = mag_raw->y; mag_raw->y = t; // negate mag Z axis mag_raw->z *= -1; } void compass_thread(void const *argument) { //osTimerThread.set_priority(osPriorityRealtime); // get raw data from the sensors green = 0; acc.getAxis( acc_raw); mag.getAxis( mag_raw); if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass if(!(l % 10)) lcdp->signal_set(0x04); if(l++ >= 49) { // take car of business once a second seconds++; calibrate->signal_set(0x2); debugp->signal_set(0x01); l = 0; sflag = 1; } tcount++; green = 1; //osTimerThread.set_priority(osPriorityNormal); } 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= %d Y= %d Z= %d ", acc_raw.x, acc_raw.y, acc_raw.z); //printf("Mag: X= %d Y= %d Z= %d\r\n", mag_raw.x, mag_raw.y, mag_raw.z); 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\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); printf("Tcount= %d\r\n", tcount); } void calibrate_thread(void const *argument) { while(true) { Thread::signal_wait(0x2); blue = 0; compass.calibrate(); blue = 1; } } 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) { int h, m, s; while (true) { // Signal flags that are reported as event are automatically cleared. Thread::signal_wait(0x4); red = 0; h = seconds / 3600; m = (seconds % 3600) / 60; s = seconds % 60; lcd.locate(0,1); lcd.printf("K64F eCompass\nRunTime %02d:%02d:%02d\nRoll: %03d Pitch: %02d Yaw: %03d ", h, m, s, axis6.roll, axis6.pitch, axis6.yaw); red = 1; } } int main (void) { red = 1; green = 1; blue = 1; Thread t1(print_thread); Thread t2(calibrate_thread); Thread t3(lcd_thread); debugp = &t1; calibrate = &t2; lcdp = &t3; acc.enable(); mag.enable(); lcd.cls(); lcd.locate(0,1); lcd.printf("K64F eCompass"); acc.getAxis( acc_raw); mag.getAxis( mag_raw); // The Timer Thread runs at Real Time priority, see RTX_Conf_CM.c, line 182 RtosTimer compass_timer(compass_thread, osTimerPeriodic); compass_timer.start(20); /* * Thread Priorities * Compass THread, Real Time (Highest) * Compass calibration, High Priority * LED Update, Above Normal * Pprintf to console, Normal * main(), Normal */ debugp->set_priority(osPriorityNormal); lcdp->set_priority(osPriorityAboveNormal); calibrate->set_priority(osPriorityHigh); green = 1; while (true) { Thread::wait(osWaitForever); } }