GIU\ZF
Dependencies: MCP23017 WattBob_TextLCD mbed-rtos mbed
Fork of rtos_basic by
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
Generated on Mon Jul 25 2022 05:11:11 by 1.7.2