A basic eCompass using mbed-RTOS
Dependencies: C12832 FXOS8700Q eCompass_FPU_Lib mbed-rtos mbed
Diff: main.cpp
- Revision:
- 0:2126a25ea273
- Child:
- 1:2d2270d1a5f5
diff -r 000000000000 -r 2126a25ea273 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon May 05 17:46:15 2014 +0000 @@ -0,0 +1,160 @@ +#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); + } +} \ No newline at end of file