File content as of revision 17:a29ce6fc667c:

#include "core.h"
namespace getControls{
    //I/O
    AnalogIn brake(PORT_BRAKE);
    AnalogIn accel(PORT_ACCEL);
    
    const float freq = 10.0f;
    
    static inline void hotLoop(){
        runTimeParams::liveAccess.lock();
        runTimeParams::brakeForce = brake.read();
        runTimeParams::accelForce = brake.read();
        runTimeParams::liveAccess.unlock();
    }
}
namespace getIgnition{
    //Read engine on/off switch and show current state on a LED
    static const float freq = 2; //hz
    
    //I/O
    DigitalIn ignition(PORT_IGNITION);
    DigitalOut led1(IGNITION_LED);
    
    static inline void hotLoop(){
            led1 = ignition.read();
    }
}

namespace carSimulator{
    Thread thread;
    const float freq = 20.0f;
    
    static inline void hotLoop(){
        
        static int i = 0;
        const int i_prev = i;
        i = (i>=3)? 0: i+1;
        
        const float friction = 0.1f;
        
        runTimeParams::liveAccess.lock();
        
        //v2 = at+v1
        float accel;
        if (getIgnition::ignition.read()){
            accel = runTimeParams::accelForce - 
            (runTimeParams::brakeForce+friction);
        }
        else{
            accel= -(runTimeParams::brakeForce+friction);
        }
        
        float tmpSpeed = accel * + runTimeParams::speed[i_prev];
        runTimeParams::speed[i] = (tmpSpeed>0)?tmpSpeed:0;
        runTimeParams::liveAccess.unlock();
        
        }
}
namespace filterSpeed{
    //Filter speed with averaging filter
    static const float freq = 5; //hz 
    static inline void hotLoop(){
            runTimeParams::liveAccess.lock();
            runTimeParams::avgSpeed = (runTimeParams::speed[0] +
                            runTimeParams::speed[1] +
                            runTimeParams::speed[2])/3;
            runTimeParams::liveAccess.unlock();
            
    } 
}


namespace task_group_2{
    Thread thread;
    const float freq = 20.0f; //hz
    DigitalOut led2(LED2);

    void runTask(){
        Timer executionTimer,sleepTimer;
        executionTimer.reset();
        sleepTimer.reset();
        
        const int const_delay = int((1000.0f/freq)+0.5f);
        int dynamic_delay = const_delay;
        int tick = 0;
        int max_exec_time = 0;
        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--------------
            carSimulator::hotLoop();
            
            static const int tick_interval_filter = int((freq/filterSpeed::freq)+0.5f);
            if (!(tick%tick_interval_filter)) filterSpeed::hotLoop();
            
            static const int tick_interval_controls = int((freq/getControls::freq)+0.5f);
            if (!(tick%tick_interval_controls)) getControls::hotLoop();
            
            static const int tick_interval_ignitionLED = int((freq/getIgnition::freq)+0.5f);
            if (!(tick%tick_interval_ignitionLED )) getIgnition::hotLoop();
            // Completed tasks
            
            tick++;
            executionTimer.stop();
            int exec_time = executionTimer.read_ms();
            if (exec_time > max_exec_time) max_exec_time=exec_time;
            
            #if DEBUG_MODE
            static const int debug_log_interval = int(freq/dequeueMail::freq);
            if (!(tick%debug_log_interval)){
                runTimeParams::debugAccess.lock();
                *runTimeParams::debugLog += "task_group_2," + to_string(max_exec_time) + ","
                            + to_string(sleepTime) + ","
                            + to_string(drift) + "\n\r";
                exec_time = 0;
                runTimeParams::debugAccess.unlock();
            }
            #else
            static const int debug_log_interval = 1;
            #endif
            
            static const int tick_LCM = 
                debug_log_interval*
                tick_interval_ignitionLED*
                tick_interval_controls*
                tick_interval_filter;
                
            if (tick==tick_LCM) tick=0;
            
            executionTimer.reset();
            sleepTimer.reset();
            sleepTimer.start();
            
            dynamic_delay = const_delay -(exec_time + drift);
            Thread::wait(dynamic_delay);
        }   
    }
}