File content as of revision 16:0ada6cbd41e2:

#include "core.h"

namespace mailData{
    Mail<mail_t, 100> mailBox;
}

namespace enqueueMail{
    //Send speed, accelerometer and brake values to a 100 element MAIL queue
    Thread thread;
    const float freq = 0.2; //hz
    
    //I/O
    void runTask(){
        
        
        Timer executionTimer,sleepTimer;
        executionTimer.reset();
        sleepTimer.reset();
        
        const int const_delay = int((1000.0f/freq)+0.5f);
        int dynamic_delay = const_delay;
        int tick = 0;
        
        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();
            
            runTimeParams::liveAccess.lock();
            
            mail->speed = runTimeParams::avgSpeed;
            mail->accel = runTimeParams::accelForce;
            mail->brake = runTimeParams::brakeForce;
            
            runTimeParams::liveAccess.unlock();
            
            mailBox.put(mail);
            //End of Core loop
            
            tick++;
            executionTimer.stop();
            int exec_time = executionTimer.read_ms();
            
            #if DEBUG_MODE
            const int debug_log_interval = int(freq/dequeueMail::freq);
            if (!(tick%debug_log_interval)){
                runTimeParams::liveAccess.lock();
                runTimeParams::debugLog += "Enqueue Mail," + to_string(exec_time) + ","
                            + to_string(sleepTime) + ","
                            + to_string(drift) + "\n\r";
                runTimeParams::liveAccess.unlock();
                tick = 0;
            }
            #endif
            
            executionTimer.reset();
            sleepTimer.reset();
            sleepTimer.start();
            
            dynamic_delay = const_delay - (exec_time + drift);
            Thread::wait(dynamic_delay);
        }
        
    }
}

namespace dequeueMail{
    //Dump contents of feature_7 MAIL queue to the serial connection to the PC
    Thread thread;
    #if DEBUG_MODE
    const float freq = 0.2; //hz
    #else
    const float freq = 0.05; //hz
    #endif
    
    
    Serial pc(USBTX, USBRX); // tx, rx
    
    
    void runTask(){
        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");
        
        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);
            }
            
            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);
            
        }   
    }
}