File content as of revision 12:90b5d8eae5ec:

#include "core.h"

DigitalOut led1(LED3);
DigitalOut led2(LED2);
Thread thread,thread_group_2HZ,thread_group_1HZ;

//Merge tasks with same frequency
namespace runTimeParams{
    Mutex liveAccess;
    float brakeForce = 0.0f;
    float accelForce = 0.0f;
    float avgSpeed = 0.0f;
    float odometer = 0.0f;
    #if DEBUG_MODE
    string debugLog = "task,execution_time_ms,lastSleep,drift\n";
    #endif
    }


namespace task_group_1{
    Thread thread;
    Timer executionTimer,sleepTimer;
    bool tick = false;
    
    void init(){
        executionTimer.reset();
        sleepTimer.reset();
        display::init();
    }
    void runTask(){
        const int const_delay = int(1000.0f/display::freq);
        int dynamic_delay = const_delay;
        while(true){
            sleepTimer.stop();
            executionTimer.start();
            
            
            int sleepTime = sleepTimer.read_ms();
            const int drift = ((sleepTime - dynamic_delay) > 0)?
                                        (sleepTime - dynamic_delay) : 0;
            
            
            // Run all tasks
            display::hotLoop();
            
            
            executionTimer.stop();
            int exec_time = executionTimer.read_ms();
            
            #if DEBUG_MODE
            runTimeParams::liveAccess.lock();
            //runTimeParams::debugLog += "GROUP_1," + to_string(executionTimer.read_ms()) + ","
//                        + to_string(sleepTimer.read_ms()) + ", \n";
            runTimeParams::odometer = float(sleepTime);
            runTimeParams::avgSpeed = float(exec_time);
            runTimeParams::liveAccess.unlock();
            #endif
            
            executionTimer.reset();
            sleepTimer.reset();
            sleepTimer.start();
            dynamic_delay = const_delay - (exec_time + drift);
            Thread::wait(dynamic_delay);
        }   
    }
}

int init(){
    //Run task initializers
    task_group_1::init();
    
    //Start task hotloops
    
    task_group_1::thread.start(task_group_1::runTask);
//    getControls::thread.start(getControls::runTask);
    return 0;
}
 
void led2_thread() {
    while (true) {
        led2 = !led2;
        Thread::wait(1000);
    }
}
 
int main() {
    init();
    thread.start(led2_thread);
    
    while (true) {
        led1 = !led1;
        Thread::wait(500);
    }
}