File content as of revision 1:d980a57e422a:

#include "my_tasks.h"
#include "my_structures.h"
#include "my_tools.h"

#ifndef __CAR_STATS__
#define __CAT_STATS__

#define AIR_DENSITY         1.225   // kg/m3

// TESLA MODEL X
#define DRAG_COEF           0.24
#define REF_AREA            0.62   // m²

#endif

using namespace std;

// TASKS at 10Hz and 5Hz
void timer1(void *arg)
{
    static int counterTimer1 = 0;
    
    counterTimer1++;
    task1_readAccelBrake(arg);
    
    if (counterTimer1 % 2 == 0)
    {
        task3_filterSpeed(arg);
    }
}

// TASKS at 2Hz, 1 Hz and 0.5 Hz
void timer2(void *arg)
{
    static int counterTimer2 = 0;
    
    counterTimer2++;
    task2_readEngineState(arg);
    task4_showUseOfBrake(arg);
    task6_writeLCD(arg);
    
    if (counterTimer2 % 2 == 0)
    {
        task5_monitorSpeed(arg);
        task9_readSideLight(arg);
    }
    if (counterTimer2 % 4 == 0)
    {
        task10_turnIndic(arg);
    }
}

// TASKS at 0.2 Hz and 0.05 Hz
void timer3(void *arg)
{
    static int counterTimer3 = 0;
    
    counterTimer3++;
    task7_sendToMailQueue(arg);
    
    if (counterTimer3 % 4 == 0)
    {
        task8_dumpMailQueue(arg);
    }
}









// CAR SIMULATOR
void task0_carSim(void *arg)
{
    const float dt = 0.05; // 20Hz
    carStructure *myCar = (carStructure *) arg;
    
    // Block access on RAWValues structure
    myCar->p_rawMutex->lock();
    engineRAWValues &p_rawValues = *(myCar->p_rawValues);
    
    // Copy the values
    int  engine = p_rawValues.engineState;
    float accel = p_rawValues.acceleratorValue;
    float brake = p_rawValues.brakeValue;
    
    // Release the mutex once we copied the values
    myCar->p_rawMutex->unlock();
    

    
    // Add it the vector saving the speed values
    myCar->p_statMutex->lock();
    // Get last speed value stored
    float lastSpeed =(*(myCar->p_carStats->p_speedVector))[SIZE_SPEED_VECTOR-1];
    myCar->p_statMutex->unlock();

    // ---------------------------------------------------------------------- //
    // Acceleration Force
    float accelForce = MAX_SPEED * (accel - brake) * engine;
    
    // ---------------------------------------------------------------------- //
    /* 
     * DRAG FORCE -> F = 0.5 C ρ A V²
     * 
     * A = Reference area, m2.
     * C = Drag coefficient, unitless.
     * F = Drag force, N.
     * V = Velocity, m/s.
     * ρ = Density of fluid (liquid or gas), kg/m3.
     */
    float dragForce = 0.5*DRAG_COEF
                        *lastSpeed*lastSpeed
                        *REF_AREA
                        *AIR_DENSITY;
    if (lastSpeed == 0) dragForce = 0;

    // ---------------------------------------------------------------------- //
    // Compute speed of car on Euler Scheme
    float newSpeed = lastSpeed*0.98
                    + dt*(accelForce - dragForce);
    if (newSpeed < 0) newSpeed = 0;

    // ---------------------------------------------------------------------- //
    // Lock Mutex
    myCar->p_statMutex->lock();
    // Delete first element of vector
    deleteFirstValueVec(*(myCar->p_carStats->p_speedVector));
    // Add new speed value
    myCar->p_carStats->p_speedVector->push_back(newSpeed); // m/s
    
    // ---------------------------------------------------------------------- //
    // Euler Scheme for traveled distance
    float distance = myCar->p_carStats->distance + dt*newSpeed; //m
    myCar->p_carStats->distance = distance;
    // Unlock Mutex
    myCar->p_statMutex->unlock();
    
    // ---------------------------------------------------------------------- //
    myCar->p_PC->printf("Accel: %.2f | Brake: %.2f\r\n", accel, brake);
    myCar->p_PC->printf("New Speed: %.1f km/h\r\n", newSpeed*3.6);
    myCar->p_PC->printf("Traveled Distance: %.3f km\r\n", distance/1000);
}

// TASK 1
void task1_readAccelBrake(void *arg)
{
    carStructure &myCar = *((carStructure *) arg);
    
    // Block access on RAWValues structure
    (myCar.p_rawMutex)->lock();
    engineRAWValues &p_rawValues = *(myCar.p_rawValues);
    
    // Read Accelerator and Brake Values
    p_rawValues.acceleratorValue = readAnalogInput(*(myCar.p_accelerator));
    p_rawValues.brakeValue       = readAnalogInput(*(myCar.p_brake));
    
    // Release the mutex
    (myCar.p_rawMutex)->unlock();
}

// TASK 2                  
void task2_readEngineState(void *arg)
{
    carStructure *myCar = (carStructure *) arg;
    
    // Block access on RAWValues structure
    myCar->p_rawMutex->lock();
    engineRAWValues &p_rawValues = *(myCar->p_rawValues);
    
    // Read Engine State
    int state = myCar->p_engineSwitch->read();
    p_rawValues.engineState = state;
    if (state)
    {
        *(myCar->p_engineLight) = ON;
    }
    else
    {
        *(myCar->p_engineLight) = OFF;
    }
    
    // Release the mutex
    myCar->p_rawMutex->unlock();
}

// TASK 3
void task3_filterSpeed(void *arg)
{
    carStructure &myCar = *((carStructure *) arg);
    
    // Block access on carStat
    myCar.p_statMutex->lock();
    
    // Copy pointers to values we want to manipulate
    carStatistics &carStat = *(myCar.p_carStats);
    vector<float> &speedVec = *(carStat.p_speedVector);
    
    // Compute average speed on sizeOfAvg last readings
    float sum = 0;
    for (int i = 1; i < carStat.sizeOfAvg + 1; i++)
    {
        sum += speedVec[SIZE_SPEED_VECTOR - i];
    }
    // Compute the average speed
    carStat.averageSpeed = sum / carStat.sizeOfAvg;
    
    // Release the mutex
    myCar.p_statMutex->unlock();
}

// TASK 4
void task4_showUseOfBrake(void *arg)
{
    carStructure &myCar = *((carStructure *) arg);
    
    // Block access on RAWValues structure
    myCar.p_rawMutex->lock();
    engineRAWValues &p_rawValues = *(myCar.p_rawValues);

    // Read Brake State
    float brake = p_rawValues.brakeValue;
    
    // Release the mutex
    myCar.p_rawMutex->unlock();
    
    // Deal with the copied value
    monitorValue(brake, *(myCar.p_brakeLight), 0.05);
}

// TASK 5
void task5_monitorSpeed(void *arg)
{
    carStructure &myCar = *((carStructure *) arg);
    
    // Block access on carStat
    myCar.p_statMutex->lock();
    // Get an easier access to the vector storing speed values
    vector<float> &speedVec = *(myCar.p_carStats->p_speedVector);
    float lastSpeed = speedVec[SIZE_SPEED_VECTOR - 1];
    
    // Release the mutex
    myCar.p_statMutex->unlock();
    
    // Deal with the copied value
    monitorValue(lastSpeed, *(myCar.p_overspeedLight), 141.0/3.6);
}

// TASK 6
void task6_writeLCD(void *arg)
{
    carStructure &myCar = *((carStructure *) arg);
    
    // Block access on carStat
    myCar.p_statMutex->lock();
    
    carStatistics &carStat = *(myCar.p_carStats);
        
    // Copy what we want
    float avgSpeed = carStat.averageSpeed;
    float distance = carStat.distance;
    
    // Release the mutex
    myCar.p_statMutex->unlock();
    
    writeOnLCD(*(myCar.p_lcd), distance, avgSpeed);
}

// TASK 7
void task7_sendToMailQueue(void *arg)
{
    carStructure *myCar = (carStructure *) arg;
    
    // Block access on carStat
    myCar->p_statMutex->lock();
    carStatistics &carStat = *(myCar->p_carStats);
    // Copy what we want
    float avgSpeed = carStat.averageSpeed;
    // Release the mutex
    myCar->p_statMutex->unlock();
    
    // Block access on RAWValues structure
    myCar->p_rawMutex->lock();
    engineRAWValues &p_rawValues = *(myCar->p_rawValues);
    // Read Brake State
    float brake = p_rawValues.brakeValue;
    float accel = p_rawValues.acceleratorValue;
    // Release the mutex
    myCar->p_rawMutex->unlock(); 
    
    // New Queue Object
    mailStruct *mail = myCar->p_mailQueue->alloc();
    // Copy values
    mail->accelerator = accel;
    mail->brake = brake;
    mail->avgSpeed = avgSpeed;
    // Add to queue
    myCar->p_mailQueue->put(mail);
    myCar->p_PC->printf("\r\nPUT in Queue. In: %i\r\n", myCar->nbElementInQueue);
    
    // Lock Counter
    myCar->p_mailMutex->lock();
    myCar->nbElementInQueue += 1;
    // Unlock queue
    myCar->p_mailMutex->unlock();
}

// TASK 8
void task8_dumpMailQueue(void *arg)
{
    carStructure *myCar = (carStructure *) arg;
    
    // Lock Mail
    myCar->p_mailMutex->lock();
    int nbOfElement = myCar->nbElementInQueue;
    // Unlock Counter
    myCar->p_mailMutex->unlock();
    
    myCar->p_PC->printf("\r\nDumping Queue. In: %i\r\n", nbOfElement);
    // Go through every element of the queue
    for (int i = 0; i < nbOfElement ; i++)
    {
        // Get Them
        osEvent evt = myCar->p_mailQueue->get();
        if (evt.status == osEventMail) 
        {
            mailStruct *mail = (mailStruct *)evt.value.p;
            // Send them to the PC
            myCar->p_PC->printf("\tElement %i \r\n\t Accel: %.2f\r\n\t",
                                i+1, mail->accelerator);                  
            myCar->p_PC->printf(" Brake: %.2f\r\n\t Speed: %.2f\r\n",
                                mail->brake, (mail->avgSpeed)*3.6);
            
            myCar->p_mailQueue->free(mail);
        }
    }
    
    // Lock Counter
    myCar->p_mailMutex->lock();
    // Reset the counter of element in the queue
    myCar->nbElementInQueue = 0;
    // Unlock Mail
    myCar->p_mailMutex->unlock();
}

// TASK 9
void task9_readSideLight(void *arg)
{
    carStructure *myCar = (carStructure *) arg;
    
    if (myCar->p_sideLightSwitch->read())
    {
        *(myCar->p_sideLightIndicator) = ON;
    }
    else
    {
        *(myCar->p_sideLightIndicator) = OFF;
    }
}

// TASK 10
void task10_turnIndic(void *arg)
{
    carStructure *myCar = (carStructure *) arg;

    static bool right = false;
    static bool left  = false;
    
    // We check if the pins have changed
    if ( right != myCar->p_rightSwitch->read() 
      || left  != myCar->p_leftSwitch->read() )
    {
        // If the state of the switch has changed, we update the PWM frequency 
        // of the LEDs
        right = myCar->p_rightSwitch->read();
        left  = myCar->p_leftSwitch->read();
        updateLEDs(myCar, right, left);
    }
}