MailTasks.cpp

00001 #include "core.h"
00002 
00003 namespace mailData{
00004     Mail<mail_t, 100> mailBox;
00005 }
00006 
00007 namespace enqueueMail{
00008     //Send speed, accelerometer and brake values to a 100 element MAIL queue
00009     Thread thread;
00010     const float freq = 0.2; //hz
00011     
00012     void runTask(){
00013         
00014         Timer executionTimer,sleepTimer;
00015         executionTimer.reset();
00016         sleepTimer.reset();
00017         
00018         const int const_delay = int((1000.0f/freq)+0.5f);
00019         int dynamic_delay = const_delay;
00020         int tick = 0;
00021         
00022         while(1){
00023             //Determine scheduling compensators:
00024                 //1: release time drift 
00025                 //2: execution time
00026             sleepTimer.stop();
00027             executionTimer.start();
00028             int sleepTime = sleepTimer.read_ms();
00029             const int drift = ((sleepTime - dynamic_delay) > 0)?
00030                                         (sleepTime - dynamic_delay) : 0;
00031             //Core Task-----------------
00032             using namespace mailData;
00033             mail_t *mail = mailBox.alloc();
00034             
00035             runTimeParams::liveAccess.lock();
00036             
00037             mail->speed = runTimeParams::avgSpeed;
00038             mail->accel = runTimeParams::accelForce;
00039             mail->brake = runTimeParams::brakeForce;
00040             
00041             runTimeParams::liveAccess.unlock();
00042             
00043             mailBox.put(mail);
00044             //End of Core task
00045             
00046             tick++;
00047             executionTimer.stop();
00048             int exec_time = executionTimer.read_ms();
00049             
00050             #if DEBUG_MODE
00051             //Debug Logs (once per dequeue call to avoid memory issues)
00052             const int debug_log_interval = int(freq/dequeueMail::freq);
00053             if (!(tick%debug_log_interval)){
00054                 runTimeParams::debugAccess.lock();
00055                 *runTimeParams::debugLog += "Enqueue Mail," + to_string(exec_time) + ","
00056                             + to_string(sleepTime) + ","
00057                             + to_string(drift) + "\n\r";
00058                 runTimeParams::debugAccess.unlock();
00059                 tick = 0;
00060             }
00061             #endif
00062             
00063             
00064             executionTimer.reset();
00065             sleepTimer.reset();
00066             sleepTimer.start();
00067             
00068             //compensate for delays
00069             dynamic_delay = const_delay - (exec_time + drift);
00070             Thread::wait(dynamic_delay);
00071         }
00072         
00073     }
00074 }
00075 
00076 namespace dequeueMail{
00077     //Dump contents of feature_7 MAIL queue to the serial connection to the PC
00078     Thread thread;
00079     const float freq = 0.05; //hz
00080     
00081     Serial pc(USBTX, USBRX); // tx, rx
00082     
00083     void runTask(){
00084         //init
00085         Timer executionTimer,sleepTimer;
00086         executionTimer.reset();
00087         sleepTimer.reset();
00088         
00089         const int const_delay = int((1000.0f/freq)+0.5f);
00090         int dynamic_delay = const_delay;
00091         
00092         pc.printf("speed,acceleration,brake\n\r");
00093         
00094         while(true){
00095             //Determine scheduling compensators:
00096                 //1: release time drift 
00097                 //2: execution time
00098             sleepTimer.stop();
00099             executionTimer.start();
00100             int sleepTime = sleepTimer.read_ms();
00101             const int drift = ((sleepTime - dynamic_delay) > 0)?
00102                                         (sleepTime - dynamic_delay) : 0;
00103             
00104             // Mail Dump------------
00105             using namespace mailData;
00106             osEvent evt = mailBox.get(1);
00107             while (evt.status == osEventMail){
00108                 mail_t * mail = (mail_t*)evt.value.p;
00109                 pc.printf("%.2f,%.2f,%.2f\n\r",mail->speed,mail->accel,mail->brake);
00110                 mailBox.free(mail);
00111                 evt = mailBox.get(1);
00112             }
00113             //----------------------
00114             
00115             executionTimer.stop();
00116             int exec_time = executionTimer.read_ms();
00117             
00118             #if DEBUG_MODE
00119             //Debug Logs (once per dequeue call to avoid memory issues)
00120             runTimeParams::debugAccess.lock();
00121             *runTimeParams::debugLog += "Dequeue Mail," + to_string(exec_time) + ","
00122                         + to_string(sleepTime) + ","
00123                         + to_string(drift) + "\n\r";
00124             
00125             //Buffer Swap so other tasks can prempt this one and keep logging
00126             string * message;
00127             if (runTimeParams::debugLog == & runTimeParams::debugLogBuffer1){
00128             runTimeParams::debugLog = & runTimeParams::debugLogBuffer2;
00129             message = &runTimeParams::debugLogBuffer1;
00130             }
00131             else{
00132             runTimeParams::debugLog = & runTimeParams::debugLogBuffer1;
00133             message = &runTimeParams::debugLogBuffer2;
00134             }
00135             *runTimeParams::debugLog = "";
00136             runTimeParams::debugAccess.unlock();
00137             
00138             pc.printf(message->c_str());
00139             #endif
00140             
00141             executionTimer.reset();
00142             sleepTimer.reset();
00143             sleepTimer.start();
00144             dynamic_delay = const_delay - (exec_time + drift);
00145             Thread::wait(dynamic_delay);
00146             
00147         }   
00148     }
00149 }
00150         
00151