Diff: main.cpp

Revision:

0:2126a25ea273

Child:

1:2d2270d1a5f5

--- /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