Gershom Agim / Mbed 2 deprecated RTOS_EMBEDDED

GIU\ZF

Dependencies:   MCP23017 WattBob_TextLCD mbed-rtos mbed

Fork of rtos_basic by mbed official

tasks/MailTasks.cpp@20:202e0046527e, 2018-03-28 (annotated)

Committer:
ihexx
Date:
Wed Mar 28 14:34:34 2018 +0000
Revision:
20:202e0046527e
Parent:
19:2044bb5d7f29 
LAST MINUTE CHJECKS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ihexx 12:90b5d8eae5ec 1 #include "core.h"
ihexx 12:90b5d8eae5ec 2
ihexx 12:90b5d8eae5ec 3 namespace mailData{
ihexx 12:90b5d8eae5ec 4 Mail<mail_t, 100> mailBox;
ihexx 12:90b5d8eae5ec 5 }
ihexx 12:90b5d8eae5ec 6
ihexx 13:ab52f46c98ab 7 namespace enqueueMail{
ihexx 13:ab52f46c98ab 8 //Send speed, accelerometer and brake values to a 100 element MAIL queue
ihexx 12:90b5d8eae5ec 9 Thread thread;
ihexx 13:ab52f46c98ab 10 const float freq = 0.2; //hz
ihexx 12:90b5d8eae5ec 11
ihexx 12:90b5d8eae5ec 12 void runTask(){
ihexx 14:8a6c20435523 13
ihexx 13:ab52f46c98ab 14 Timer executionTimer,sleepTimer;
ihexx 13:ab52f46c98ab 15 executionTimer.reset();
ihexx 13:ab52f46c98ab 16 sleepTimer.reset();
ihexx 13:ab52f46c98ab 17
ihexx 13:ab52f46c98ab 18 const int const_delay = int((1000.0f/freq)+0.5f);
ihexx 13:ab52f46c98ab 19 int dynamic_delay = const_delay;
ihexx 14:8a6c20435523 20 int tick = 0;
ihexx 13:ab52f46c98ab 21
ihexx 12:90b5d8eae5ec 22 while(1){
ihexx 19:2044bb5d7f29 23 //Determine scheduling compensators:
ihexx 19:2044bb5d7f29 24 //1: release time drift
ihexx 19:2044bb5d7f29 25 //2: execution time
ihexx 14:8a6c20435523 26 sleepTimer.stop();
ihexx 13:ab52f46c98ab 27 executionTimer.start();
ihexx 13:ab52f46c98ab 28 int sleepTime = sleepTimer.read_ms();
ihexx 13:ab52f46c98ab 29 const int drift = ((sleepTime - dynamic_delay) > 0)?
ihexx 13:ab52f46c98ab 30 (sleepTime - dynamic_delay) : 0;
ihexx 19:2044bb5d7f29 31 //Core Task-----------------
ihexx 12:90b5d8eae5ec 32 using namespace mailData;
ihexx 12:90b5d8eae5ec 33 mail_t *mail = mailBox.alloc();
ihexx 12:90b5d8eae5ec 34
ihexx 12:90b5d8eae5ec 35 runTimeParams::liveAccess.lock();
ihexx 12:90b5d8eae5ec 36
ihexx 12:90b5d8eae5ec 37 mail->speed = runTimeParams::avgSpeed;
ihexx 12:90b5d8eae5ec 38 mail->accel = runTimeParams::accelForce;
ihexx 12:90b5d8eae5ec 39 mail->brake = runTimeParams::brakeForce;
ihexx 12:90b5d8eae5ec 40
ihexx 12:90b5d8eae5ec 41 runTimeParams::liveAccess.unlock();
ihexx 12:90b5d8eae5ec 42
ihexx 12:90b5d8eae5ec 43 mailBox.put(mail);
ihexx 19:2044bb5d7f29 44 //End of Core task
ihexx 12:90b5d8eae5ec 45
ihexx 14:8a6c20435523 46 tick++;
ihexx 13:ab52f46c98ab 47 executionTimer.stop();
ihexx 13:ab52f46c98ab 48 int exec_time = executionTimer.read_ms();
ihexx 13:ab52f46c98ab 49
ihexx 13:ab52f46c98ab 50 #if DEBUG_MODE
ihexx 19:2044bb5d7f29 51 //Debug Logs (once per dequeue call to avoid memory issues)
ihexx 14:8a6c20435523 52 const int debug_log_interval = int(freq/dequeueMail::freq);
ihexx 14:8a6c20435523 53 if (!(tick%debug_log_interval)){
ihexx 17:a29ce6fc667c 54 runTimeParams::debugAccess.lock();
ihexx 17:a29ce6fc667c 55 *runTimeParams::debugLog += "Enqueue Mail," + to_string(exec_time) + ","
ihexx 14:8a6c20435523 56 + to_string(sleepTime) + ","
ihexx 14:8a6c20435523 57 + to_string(drift) + "\n\r";
ihexx 17:a29ce6fc667c 58 runTimeParams::debugAccess.unlock();
ihexx 14:8a6c20435523 59 tick = 0;
ihexx 14:8a6c20435523 60 }
ihexx 13:ab52f46c98ab 61 #endif
ihexx 13:ab52f46c98ab 62
ihexx 19:2044bb5d7f29 63
ihexx 13:ab52f46c98ab 64 executionTimer.reset();
ihexx 13:ab52f46c98ab 65 sleepTimer.reset();
ihexx 13:ab52f46c98ab 66 sleepTimer.start();
ihexx 14:8a6c20435523 67
ihexx 19:2044bb5d7f29 68 //compensate for delays
ihexx 13:ab52f46c98ab 69 dynamic_delay = const_delay - (exec_time + drift);
ihexx 13:ab52f46c98ab 70 Thread::wait(dynamic_delay);
ihexx 12:90b5d8eae5ec 71 }
ihexx 12:90b5d8eae5ec 72
ihexx 12:90b5d8eae5ec 73 }
ihexx 12:90b5d8eae5ec 74 }
ihexx 12:90b5d8eae5ec 75
ihexx 13:ab52f46c98ab 76 namespace dequeueMail{
ihexx 13:ab52f46c98ab 77 //Dump contents of feature_7 MAIL queue to the serial connection to the PC
ihexx 12:90b5d8eae5ec 78 Thread thread;
ihexx 16:0ada6cbd41e2 79 const float freq = 0.05; //hz
ihexx 17:a29ce6fc667c 80
ihexx 12:90b5d8eae5ec 81 Serial pc(USBTX, USBRX); // tx, rx
ihexx 13:ab52f46c98ab 82
ihexx 12:90b5d8eae5ec 83 void runTask(){
ihexx 19:2044bb5d7f29 84 //init
ihexx 13:ab52f46c98ab 85 Timer executionTimer,sleepTimer;
ihexx 13:ab52f46c98ab 86 executionTimer.reset();
ihexx 13:ab52f46c98ab 87 sleepTimer.reset();
ihexx 13:ab52f46c98ab 88
ihexx 13:ab52f46c98ab 89 const int const_delay = int((1000.0f/freq)+0.5f);
ihexx 13:ab52f46c98ab 90 int dynamic_delay = const_delay;
ihexx 13:ab52f46c98ab 91
ihexx 13:ab52f46c98ab 92 pc.printf("speed,acceleration,brake\n\r");
ihexx 12:90b5d8eae5ec 93
ihexx 13:ab52f46c98ab 94 while(true){
ihexx 19:2044bb5d7f29 95 //Determine scheduling compensators:
ihexx 19:2044bb5d7f29 96 //1: release time drift
ihexx 19:2044bb5d7f29 97 //2: execution time
ihexx 13:ab52f46c98ab 98 sleepTimer.stop();
ihexx 13:ab52f46c98ab 99 executionTimer.start();
ihexx 13:ab52f46c98ab 100 int sleepTime = sleepTimer.read_ms();
ihexx 13:ab52f46c98ab 101 const int drift = ((sleepTime - dynamic_delay) > 0)?
ihexx 13:ab52f46c98ab 102 (sleepTime - dynamic_delay) : 0;
ihexx 13:ab52f46c98ab 103
ihexx 19:2044bb5d7f29 104 // Mail Dump------------
ihexx 13:ab52f46c98ab 105 using namespace mailData;
ihexx 13:ab52f46c98ab 106 osEvent evt = mailBox.get(1);
ihexx 13:ab52f46c98ab 107 while (evt.status == osEventMail){
ihexx 13:ab52f46c98ab 108 mail_t * mail = (mail_t*)evt.value.p;
ihexx 13:ab52f46c98ab 109 pc.printf("%.2f,%.2f,%.2f\n\r",mail->speed,mail->accel,mail->brake);
ihexx 13:ab52f46c98ab 110 mailBox.free(mail);
ihexx 13:ab52f46c98ab 111 evt = mailBox.get(1);
ihexx 13:ab52f46c98ab 112 }
ihexx 19:2044bb5d7f29 113 //----------------------
ihexx 12:90b5d8eae5ec 114
ihexx 13:ab52f46c98ab 115 executionTimer.stop();
ihexx 13:ab52f46c98ab 116 int exec_time = executionTimer.read_ms();
ihexx 12:90b5d8eae5ec 117
ihexx 13:ab52f46c98ab 118 #if DEBUG_MODE
ihexx 19:2044bb5d7f29 119 //Debug Logs (once per dequeue call to avoid memory issues)
ihexx 17:a29ce6fc667c 120 runTimeParams::debugAccess.lock();
ihexx 17:a29ce6fc667c 121 *runTimeParams::debugLog += "Dequeue Mail," + to_string(exec_time) + ","
ihexx 13:ab52f46c98ab 122 + to_string(sleepTime) + ","
ihexx 13:ab52f46c98ab 123 + to_string(drift) + "\n\r";
ihexx 19:2044bb5d7f29 124
ihexx 19:2044bb5d7f29 125 //Buffer Swap so other tasks can prempt this one and keep logging
ihexx 17:a29ce6fc667c 126 string * message;
ihexx 17:a29ce6fc667c 127 if (runTimeParams::debugLog == & runTimeParams::debugLogBuffer1){
ihexx 17:a29ce6fc667c 128 runTimeParams::debugLog = & runTimeParams::debugLogBuffer2;
ihexx 17:a29ce6fc667c 129 message = &runTimeParams::debugLogBuffer1;
ihexx 17:a29ce6fc667c 130 }
ihexx 17:a29ce6fc667c 131 else{
ihexx 17:a29ce6fc667c 132 runTimeParams::debugLog = & runTimeParams::debugLogBuffer1;
ihexx 17:a29ce6fc667c 133 message = &runTimeParams::debugLogBuffer2;
ihexx 17:a29ce6fc667c 134 }
ihexx 17:a29ce6fc667c 135 *runTimeParams::debugLog = "";
ihexx 17:a29ce6fc667c 136 runTimeParams::debugAccess.unlock();
ihexx 17:a29ce6fc667c 137
ihexx 17:a29ce6fc667c 138 pc.printf(message->c_str());
ihexx 13:ab52f46c98ab 139 #endif
ihexx 13:ab52f46c98ab 140
ihexx 13:ab52f46c98ab 141 executionTimer.reset();
ihexx 13:ab52f46c98ab 142 sleepTimer.reset();
ihexx 13:ab52f46c98ab 143 sleepTimer.start();
ihexx 13:ab52f46c98ab 144 dynamic_delay = const_delay - (exec_time + drift);
ihexx 13:ab52f46c98ab 145 Thread::wait(dynamic_delay);
ihexx 13:ab52f46c98ab 146
ihexx 13:ab52f46c98ab 147 }
ihexx 12:90b5d8eae5ec 148 }
ihexx 12:90b5d8eae5ec 149 }
ihexx 12:90b5d8eae5ec 150
ihexx 12:90b5d8eae5ec 151