File content as of revision 20:202e0046527e:

#include "core.h"
namespace getControls{
    //Read Accelerator and Brake
    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 = accel.read();
        runTimeParams::liveAccess.unlock();
    }
}
namespace getIgnition{
    //Read engine on/off switch and show current state on a LED
    const float freq = 2; //hz
    DigitalIn ignition(PORT_IGNITION);
    DigitalOut led1(IGNITION_LED);
    
    static inline void hotLoop(){
            led1 = ignition.read();
    }
}
namespace carSimulator{
    //Compute speed given car controls and timing data
    const float freq = 20.0f;
    static inline void hotLoop(){
        const float friction = 0.1f; //constant retardation
        runTimeParams::liveAccess.lock();
        
        //Check if ignition is on, and don't accelerate if it isn't
        float accel;
        if (getIgnition::ignition.read()){
            accel = runTimeParams::accelForce - 
            (runTimeParams::brakeForce+friction);
        }
        else{
            accel= -(runTimeParams::brakeForce+friction);
        }
        
        static int i = 0; //iterator for speed
        const int i_prev = i;
        i = (i>=3)? 0: i+1;
        //Store to speed array in round robin format
        float tmpSpeed = accel + runTimeParams::speed[i_prev];
        tmpSpeed = (tmpSpeed>0)?tmpSpeed:0;
        tmpSpeed = (tmpSpeed>200)?200:tmpSpeed;
        runTimeParams::speed[i] = tmpSpeed;
        runTimeParams::odometer += tmpSpeed*0.01f;
        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

    void runTask(){
        //Init
        Timer executionTimer,sleepTimer;
        executionTimer.reset();
        sleepTimer.reset();
        
        const int const_delay = int((1000.0f/freq)+0.5f); //ideal scheduling rate
        int dynamic_delay = const_delay; //compensating for release/execution time
        int tick = 0; //downsample task frequencies
        
        #if DEBUG_MODE
        int max_exec_time = 0; //for logging
        #endif
        
        while(true){
            //Determine scheduling compensators:
                //1: release time drift 
                //2: execution time 
            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 DEBUG_MODE
            //Debug Logs (once per dequeue call to avoid memory issues)
            if (exec_time > max_exec_time) max_exec_time=exec_time;
            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
            
            //Reset tick count
            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();
            //compensate for delays
            dynamic_delay = const_delay -(exec_time + drift);
            Thread::wait(dynamic_delay);
        }   
    }
}