Revision 6:95bced85b8e5, committed 2018-02-05

Comitter:

jun1x

Date:

Mon Feb 05 16:26:22 2018 +0000

Parent:

5:7b95c2f8e76e

Commit message:

Change mbed 2.x threads and rtostimer to mbed 5.x eventqueue

Changed in this revision

diff -r 7b95c2f8e76e -r 95bced85b8e5 main.cpp
--- a/main.cpp	Sun Feb 04 13:54:44 2018 +0000
+++ b/main.cpp	Mon Feb 05 16:26:22 2018 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "mbed_events.h"
 #include "eCompass_Lib.h"
 #include "MAG3110.h"
 #include "MMA8451Q.h"
@@ -6,7 +7,6 @@
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
 
-
 eCompass compass;
 MAG3110  mag( PTE25, PTE24);
 MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS);
@@ -25,27 +25,22 @@
 MotionSensorDataCounts mag_raw;
 MotionSensorDataCounts acc_raw;
 
-
-Thread *(debugp);
-Thread *(calibrate);
-Thread *(lcdp);
+/*
+ * Thread Priorities
+ * Compass Thread,      High Priority
+ * Compass calibration, Above Normal
+ * Console/LCD Update,  Below Normal
+ * main(),              Normal
+ */
+ 
+Thread threadCalibrate(osPriorityAboveNormal);
+Thread threadCompass(osPriorityHigh);
+Thread threadDebug(osPriorityBelowNormal);
 
-//
-// Print data values for debug
-//
-void debug_print(void)
-{
-    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); 
-}
+EventQueue queueCalibrate;
+EventQueue queueCompass;
+EventQueue queueDebug;
+
 // HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors
 //
 // This routing move and negates data as needed the
@@ -62,73 +57,67 @@
 
     // 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 compass_thread() {
+    // 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++ >= 50) { // take car of business once a second
+        seconds++;
+        l = 0;
+        green = !green;
+    }
+    tcount++;
 }
 
+//
+// Recalibrate compass at 1Hz
+//
+void calibrate_thread() {
+    red = !red;
+    compass.calibrate();
+}
  
-void print_thread(void const *argument) {
-    while (true) {
-        // Signal flags that are reported as event are automatically cleared.
-        Thread::signal_wait(0x1);    
-        debug_print();
-    }
-}
-
+//
+// Print data values for debug at 1Hz
+//
+void debug_thread(void)
+{
+    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); 
+} 
 
-void lcd_thread(void const *argument) {
-    while (true) {
-        // Signal flags that are reported as event are automatically cleared.
-        Thread::signal_wait(0x4);  
-        slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD
-    }
+//
+// Update LCD at 5Hz 
+//
+void lcd_thread() {
+    slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD
 }
-                                 
+                               
 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;  
+
+    seconds = 0; 
+    compass_type = NED_COMPASS;  
+    tcount = 0;
+    cdebug = 0; // Set to 1 to disable eCompass in order to observe raw mag data. 
+
     mag.enable();
     acc.enable();
     // Say hello to our sensors
@@ -136,21 +125,15 @@
     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
+    // Events on the queues are called periodically, interval in milliseconds
+    queueCalibrate.call_every(1000, calibrate_thread); 
+    queueCompass.call_every(20, compass_thread); 
+    queueDebug.call_every(200, lcd_thread);  
+    queueDebug.call_every(1000, debug_thread); 
     
-    Thread::wait(osWaitForever);   
+    // Queues run in different threads with different priorities
+    threadCalibrate.start(callback(&queueCalibrate, &EventQueue::dispatch_forever));
+    threadCompass.start(callback(&queueCompass, &EventQueue::dispatch_forever));
+    threadDebug.start(callback(&queueDebug, &EventQueue::dispatch_forever));
 }