File content as of revision 20:202e0046527e:

#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
    
    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){
            //Determine scheduling compensators:
                //1: release time drift 
                //2: execution time
            sleepTimer.stop();
            executionTimer.start();
            int sleepTime = sleepTimer.read_ms();
            const int drift = ((sleepTime - dynamic_delay) > 0)?
                                        (sleepTime - dynamic_delay) : 0;
            //Core Task-----------------
            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 task
            
            tick++;
            executionTimer.stop();
            int exec_time = executionTimer.read_ms();
            
            #if DEBUG_MODE
            //Debug Logs (once per dequeue call to avoid memory issues)
            const int debug_log_interval = int(freq/dequeueMail::freq);
            if (!(tick%debug_log_interval)){
                runTimeParams::debugAccess.lock();
                *runTimeParams::debugLog += "Enqueue Mail," + to_string(exec_time) + ","
                            + to_string(sleepTime) + ","
                            + to_string(drift) + "\n\r";
                runTimeParams::debugAccess.unlock();
                tick = 0;
            }
            #endif
            
            
            executionTimer.reset();
            sleepTimer.reset();
            sleepTimer.start();
            
            //compensate for delays
            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;
    const float freq = 0.05; //hz
    
    Serial pc(USBTX, USBRX); // tx, rx
    
    void runTask(){
        //init
        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){
            //Determine scheduling compensators:
                //1: release time drift 
                //2: execution time
            sleepTimer.stop();
            executionTimer.start();
            int sleepTime = sleepTimer.read_ms();
            const int drift = ((sleepTime - dynamic_delay) > 0)?
                                        (sleepTime - dynamic_delay) : 0;
            
            // Mail Dump------------
            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
            //Debug Logs (once per dequeue call to avoid memory issues)
            runTimeParams::debugAccess.lock();
            *runTimeParams::debugLog += "Dequeue Mail," + to_string(exec_time) + ","
                        + to_string(sleepTime) + ","
                        + to_string(drift) + "\n\r";
            
            //Buffer Swap so other tasks can prempt this one and keep logging
            string * message;
            if (runTimeParams::debugLog == & runTimeParams::debugLogBuffer1){
            runTimeParams::debugLog = & runTimeParams::debugLogBuffer2;
            message = &runTimeParams::debugLogBuffer1;
            }
            else{
            runTimeParams::debugLog = & runTimeParams::debugLogBuffer1;
            message = &runTimeParams::debugLogBuffer2;
            }
            *runTimeParams::debugLog = "";
            runTimeParams::debugAccess.unlock();
            
            pc.printf(message->c_str());
            #endif
            
            executionTimer.reset();
            sleepTimer.reset();
            sleepTimer.start();
            dynamic_delay = const_delay - (exec_time + drift);
            Thread::wait(dynamic_delay);
            
        }   
    }
}