Gershom Agim / Mbed 2 deprecated RTOS_EMBEDDED

GIU\ZF

Dependencies:   MCP23017 WattBob_TextLCD mbed-rtos mbed

Fork of rtos_basic by mbed official

tasks/task_group2.cpp@17:a29ce6fc667c, 2018-03-28 (annotated)

Committer:
ihexx
Date:
Wed Mar 28 00:26:55 2018 +0000
Revision:
17:a29ce6fc667c
Parent:
16:0ada6cbd41e2
Child:
19:2044bb5d7f29
Fixed deadlock bug on debug framework

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ihexx 15:524de2b2ef8e 1 #include "core.h"
ihexx 15:524de2b2ef8e 2 namespace getControls{
ihexx 15:524de2b2ef8e 3 //I/O
ihexx 15:524de2b2ef8e 4 AnalogIn brake(PORT_BRAKE);
ihexx 15:524de2b2ef8e 5 AnalogIn accel(PORT_ACCEL);
ihexx 15:524de2b2ef8e 6
ihexx 15:524de2b2ef8e 7 const float freq = 10.0f;
ihexx 15:524de2b2ef8e 8
ihexx 15:524de2b2ef8e 9 static inline void hotLoop(){
ihexx 15:524de2b2ef8e 10 runTimeParams::liveAccess.lock();
ihexx 15:524de2b2ef8e 11 runTimeParams::brakeForce = brake.read();
ihexx 15:524de2b2ef8e 12 runTimeParams::accelForce = brake.read();
ihexx 15:524de2b2ef8e 13 runTimeParams::liveAccess.unlock();
ihexx 15:524de2b2ef8e 14 }
ihexx 15:524de2b2ef8e 15 }
ihexx 16:0ada6cbd41e2 16 namespace getIgnition{
ihexx 16:0ada6cbd41e2 17 //Read engine on/off switch and show current state on a LED
ihexx 16:0ada6cbd41e2 18 static const float freq = 2; //hz
ihexx 16:0ada6cbd41e2 19
ihexx 16:0ada6cbd41e2 20 //I/O
ihexx 16:0ada6cbd41e2 21 DigitalIn ignition(PORT_IGNITION);
ihexx 16:0ada6cbd41e2 22 DigitalOut led1(IGNITION_LED);
ihexx 16:0ada6cbd41e2 23
ihexx 16:0ada6cbd41e2 24 static inline void hotLoop(){
ihexx 16:0ada6cbd41e2 25 led1 = ignition.read();
ihexx 16:0ada6cbd41e2 26 }
ihexx 16:0ada6cbd41e2 27 }
ihexx 15:524de2b2ef8e 28
ihexx 15:524de2b2ef8e 29 namespace carSimulator{
ihexx 15:524de2b2ef8e 30 Thread thread;
ihexx 15:524de2b2ef8e 31 const float freq = 20.0f;
ihexx 15:524de2b2ef8e 32
ihexx 15:524de2b2ef8e 33 static inline void hotLoop(){
ihexx 16:0ada6cbd41e2 34
ihexx 17:a29ce6fc667c 35 static int i = 0;
ihexx 17:a29ce6fc667c 36 const int i_prev = i;
ihexx 17:a29ce6fc667c 37 i = (i>=3)? 0: i+1;
ihexx 17:a29ce6fc667c 38
ihexx 16:0ada6cbd41e2 39 const float friction = 0.1f;
ihexx 17:a29ce6fc667c 40
ihexx 17:a29ce6fc667c 41 runTimeParams::liveAccess.lock();
ihexx 16:0ada6cbd41e2 42
ihexx 17:a29ce6fc667c 43 //v2 = at+v1
ihexx 17:a29ce6fc667c 44 float accel;
ihexx 17:a29ce6fc667c 45 if (getIgnition::ignition.read()){
ihexx 17:a29ce6fc667c 46 accel = runTimeParams::accelForce -
ihexx 17:a29ce6fc667c 47 (runTimeParams::brakeForce+friction);
ihexx 17:a29ce6fc667c 48 }
ihexx 17:a29ce6fc667c 49 else{
ihexx 17:a29ce6fc667c 50 accel= -(runTimeParams::brakeForce+friction);
ihexx 17:a29ce6fc667c 51 }
ihexx 17:a29ce6fc667c 52
ihexx 17:a29ce6fc667c 53 float tmpSpeed = accel * + runTimeParams::speed[i_prev];
ihexx 17:a29ce6fc667c 54 runTimeParams::speed[i] = (tmpSpeed>0)?tmpSpeed:0;
ihexx 15:524de2b2ef8e 55 runTimeParams::liveAccess.unlock();
ihexx 17:a29ce6fc667c 56
ihexx 15:524de2b2ef8e 57 }
ihexx 15:524de2b2ef8e 58 }
ihexx 16:0ada6cbd41e2 59 namespace filterSpeed{
ihexx 17:a29ce6fc667c 60 //Filter speed with averaging filter
ihexx 16:0ada6cbd41e2 61 static const float freq = 5; //hz
ihexx 17:a29ce6fc667c 62 static inline void hotLoop(){
ihexx 16:0ada6cbd41e2 63 runTimeParams::liveAccess.lock();
ihexx 17:a29ce6fc667c 64 runTimeParams::avgSpeed = (runTimeParams::speed[0] +
ihexx 17:a29ce6fc667c 65 runTimeParams::speed[1] +
ihexx 17:a29ce6fc667c 66 runTimeParams::speed[2])/3;
ihexx 16:0ada6cbd41e2 67 runTimeParams::liveAccess.unlock();
ihexx 16:0ada6cbd41e2 68
ihexx 17:a29ce6fc667c 69 }
ihexx 16:0ada6cbd41e2 70 }
ihexx 15:524de2b2ef8e 71
ihexx 17:a29ce6fc667c 72
ihexx 15:524de2b2ef8e 73 namespace task_group_2{
ihexx 15:524de2b2ef8e 74 Thread thread;
ihexx 15:524de2b2ef8e 75 const float freq = 20.0f; //hz
ihexx 16:0ada6cbd41e2 76 DigitalOut led2(LED2);
ihexx 15:524de2b2ef8e 77
ihexx 15:524de2b2ef8e 78 void runTask(){
ihexx 15:524de2b2ef8e 79 Timer executionTimer,sleepTimer;
ihexx 15:524de2b2ef8e 80 executionTimer.reset();
ihexx 15:524de2b2ef8e 81 sleepTimer.reset();
ihexx 15:524de2b2ef8e 82
ihexx 15:524de2b2ef8e 83 const int const_delay = int((1000.0f/freq)+0.5f);
ihexx 15:524de2b2ef8e 84 int dynamic_delay = const_delay;
ihexx 15:524de2b2ef8e 85 int tick = 0;
ihexx 17:a29ce6fc667c 86 int max_exec_time = 0;
ihexx 15:524de2b2ef8e 87 while(true){
ihexx 16:0ada6cbd41e2 88
ihexx 16:0ada6cbd41e2 89
ihexx 16:0ada6cbd41e2 90 sleepTimer.stop();
ihexx 16:0ada6cbd41e2 91 executionTimer.start();
ihexx 16:0ada6cbd41e2 92
ihexx 16:0ada6cbd41e2 93
ihexx 16:0ada6cbd41e2 94 int sleepTime = sleepTimer.read_ms();
ihexx 16:0ada6cbd41e2 95 const int drift = ((sleepTime - dynamic_delay) > 0)?
ihexx 16:0ada6cbd41e2 96 (sleepTime - dynamic_delay) : 0;
ihexx 15:524de2b2ef8e 97
ihexx 16:0ada6cbd41e2 98
ihexx 16:0ada6cbd41e2 99 // Run all tasks--------------
ihexx 16:0ada6cbd41e2 100 carSimulator::hotLoop();
ihexx 16:0ada6cbd41e2 101
ihexx 17:a29ce6fc667c 102 static const int tick_interval_filter = int((freq/filterSpeed::freq)+0.5f);
ihexx 17:a29ce6fc667c 103 if (!(tick%tick_interval_filter)) filterSpeed::hotLoop();
ihexx 17:a29ce6fc667c 104
ihexx 16:0ada6cbd41e2 105 static const int tick_interval_controls = int((freq/getControls::freq)+0.5f);
ihexx 16:0ada6cbd41e2 106 if (!(tick%tick_interval_controls)) getControls::hotLoop();
ihexx 16:0ada6cbd41e2 107
ihexx 16:0ada6cbd41e2 108 static const int tick_interval_ignitionLED = int((freq/getIgnition::freq)+0.5f);
ihexx 16:0ada6cbd41e2 109 if (!(tick%tick_interval_ignitionLED )) getIgnition::hotLoop();
ihexx 15:524de2b2ef8e 110 // Completed tasks
ihexx 15:524de2b2ef8e 111
ihexx 16:0ada6cbd41e2 112 tick++;
ihexx 16:0ada6cbd41e2 113 executionTimer.stop();
ihexx 16:0ada6cbd41e2 114 int exec_time = executionTimer.read_ms();
ihexx 17:a29ce6fc667c 115 if (exec_time > max_exec_time) max_exec_time=exec_time;
ihexx 15:524de2b2ef8e 116
ihexx 15:524de2b2ef8e 117 #if DEBUG_MODE
ihexx 17:a29ce6fc667c 118 static const int debug_log_interval = int(freq/dequeueMail::freq);
ihexx 16:0ada6cbd41e2 119 if (!(tick%debug_log_interval)){
ihexx 17:a29ce6fc667c 120 runTimeParams::debugAccess.lock();
ihexx 17:a29ce6fc667c 121 *runTimeParams::debugLog += "task_group_2," + to_string(max_exec_time) + ","
ihexx 16:0ada6cbd41e2 122 + to_string(sleepTime) + ","
ihexx 16:0ada6cbd41e2 123 + to_string(drift) + "\n\r";
ihexx 17:a29ce6fc667c 124 exec_time = 0;
ihexx 17:a29ce6fc667c 125 runTimeParams::debugAccess.unlock();
ihexx 16:0ada6cbd41e2 126 }
ihexx 17:a29ce6fc667c 127 #else
ihexx 17:a29ce6fc667c 128 static const int debug_log_interval = 1;
ihexx 17:a29ce6fc667c 129 #endif
ihexx 17:a29ce6fc667c 130
ihexx 17:a29ce6fc667c 131 static const int tick_LCM =
ihexx 17:a29ce6fc667c 132 debug_log_interval*
ihexx 17:a29ce6fc667c 133 tick_interval_ignitionLED*
ihexx 17:a29ce6fc667c 134 tick_interval_controls*
ihexx 17:a29ce6fc667c 135 tick_interval_filter;
ihexx 17:a29ce6fc667c 136
ihexx 16:0ada6cbd41e2 137 if (tick==tick_LCM) tick=0;
ihexx 15:524de2b2ef8e 138
ihexx 16:0ada6cbd41e2 139 executionTimer.reset();
ihexx 16:0ada6cbd41e2 140 sleepTimer.reset();
ihexx 16:0ada6cbd41e2 141 sleepTimer.start();
ihexx 16:0ada6cbd41e2 142
ihexx 17:a29ce6fc667c 143 dynamic_delay = const_delay -(exec_time + drift);
ihexx 15:524de2b2ef8e 144 Thread::wait(dynamic_delay);
ihexx 15:524de2b2ef8e 145 }
ihexx 15:524de2b2ef8e 146 }
ihexx 15:524de2b2ef8e 147 }