Diff: tasks/sendMail.cpp

Revision:

13:ab52f46c98ab

Parent:

12:90b5d8eae5ec

Child:

14:8a6c20435523

diff -r 90b5d8eae5ec -r ab52f46c98ab tasks/sendMail.cpp
--- a/tasks/sendMail.cpp	Tue Mar 27 14:35:09 2018 +0000
+++ b/tasks/sendMail.cpp	Tue Mar 27 15:56:27 2018 +0000
@@ -6,18 +6,28 @@
 
 
 
-namespace loadMail{
-    
+namespace enqueueMail{
+    //Send speed, accelerometer and brake values to a 100 element MAIL queue
     Thread thread;
-    static const float freq = 0.2; //hz
+    const float freq = 0.2; //hz
     
     //I/O
     
     void runTask(){
-        //Send speed, accelerometer and brake values to a 100 element
-        //MAIL queue
-        //• A MAIL queue is a structure available in the MBED RTOS
+        Timer executionTimer,sleepTimer;
+        executionTimer.reset();
+        sleepTimer.reset();
+        
+        const int const_delay = int((1000.0f/freq)+0.5f);
+        int dynamic_delay = const_delay;
+        
         while(1){
+             sleepTimer.stop();
+            executionTimer.start();
+            int sleepTime = sleepTimer.read_ms();
+            const int drift = ((sleepTime - dynamic_delay) > 0)?
+                                        (sleepTime - dynamic_delay) : 0;
+            //Core Loop:
             using namespace mailData;
             mail_t *mail = mailBox.alloc();
             
@@ -30,45 +40,86 @@
             runTimeParams::liveAccess.unlock();
             
             mailBox.put(mail);
+            //End of Core loop
             
-            wait(1.0f/freq);
+            executionTimer.stop();
+            int exec_time = executionTimer.read_ms();
+            
+            #if DEBUG_MODE
+            runTimeParams::liveAccess.lock();
+            //runTimeParams::debugLog += "Enqueue Mail," + to_string(exec_time) + ","
+//                        + to_string(sleepTime) + ","
+//                        + to_string(drift) + "\n";
+            runTimeParams::liveAccess.unlock();
+            #endif
+            
+            executionTimer.reset();
+            sleepTimer.reset();
+            sleepTimer.start();
+            dynamic_delay = const_delay - (exec_time + drift);
+            Thread::wait(dynamic_delay);
         }
         
     }
 }
 
-namespace sendMail{
-    
+namespace dequeueMail{
+    //Dump contents of feature_7 MAIL queue to the serial connection to the PC
     Thread thread;
     const float freq = 0.05;
-    const float executionTime = 0;
+    
     Serial pc(USBTX, USBRX); // tx, rx
-    bool init = true;
+    
     
     void runTask(){
-        //Dump contents of feature_7 MAIL queue to the serial
-        //connection to the PC. (Data will be passed through the MBED
-        //USB connection). See later technical note
+        Timer executionTimer,sleepTimer;
+        executionTimer.reset();
+        sleepTimer.reset();
+        
+        const int const_delay = int((1000.0f/freq)+0.5f);
+        int dynamic_delay = const_delay;
+        
+        pc.printf("speed,acceleration,brake\n\r");
         
-        /* Mail */
-        if (init){
-             pc.printf("speed,acceleration,brake\n\r");
-             init = false;
-             } //?
-        using namespace mailData;
-        osEvent evt = mailBox.get();
-        if (evt.status == osEventMail){
-            mail_t * mail = (mail_t*)evt.value.p;
-            pc.printf("%.2f,%.2f,%.2f\n\r",mail->speed,mail->accel,mail->brake);
+        while(true){
+            
+            sleepTimer.stop();
+            executionTimer.start();
+            int sleepTime = sleepTimer.read_ms();
+            const int drift = ((sleepTime - dynamic_delay) > 0)?
+                                        (sleepTime - dynamic_delay) : 0;
+            
+            /* Mail */
+            using namespace mailData;
+            osEvent evt = mailBox.get(1);
+            while (evt.status == osEventMail){
+                mail_t * mail = (mail_t*)evt.value.p;
+                pc.printf("%.2f,%.2f,%.2f\n\r",mail->speed,mail->accel,mail->brake);
+                mailBox.free(mail);
+                evt = mailBox.get(1);
+            }
             
-            mailBox.free(mail);
-            wait((1.0/freq)-executionTime);
-        }
+            executionTimer.stop();
+            int exec_time = executionTimer.read_ms();
             
+            #if DEBUG_MODE
+            runTimeParams::liveAccess.lock();
+            runTimeParams::debugLog += "Dequeue Mail," + to_string(exec_time) + ","
+                        + to_string(sleepTime) + ","
+                        + to_string(drift) + "\n\r";
+            pc.printf(runTimeParams::debugLog.c_str());
+            runTimeParams::debugLog = "";
+            runTimeParams::liveAccess.unlock();
+            #endif
+            
+            executionTimer.reset();
+            sleepTimer.reset();
+            sleepTimer.start();
+            dynamic_delay = const_delay - (exec_time + drift);
+            Thread::wait(dynamic_delay);
+            
+        }   
     }
 }
-
-
-
         
     
\ No newline at end of file