Com_SerialWritingThread.cpp

/**
 *  @author Sylvain Colomer, P. Gaussier
 *  @date 18/04/19.
 */

#include <iostream>
#include <string>

using namespace std;

#include "../include/Com_SerialWritingThread.h"
#include "../include/global_variables.h"


Serial_Port_WritingThread::Serial_Port_WritingThread(int task_period, int task_deadline):
    Abstract_ThreadClass(task_period, task_deadline)
{
//    Serial_Port_WritingThread::drone1 = drone1;


}

Serial_Port_WritingThread::~Serial_Port_WritingThread()
{

}

string string_currentState[]={"TO_INIT", "INIT", "READY", "RUN", "STOP", "QUIT", "PROBLEM", "DEADLINE_EXCEEDED"};

void Serial_Port_WritingThread::run()
{
    long long currentThreadDelay;

    gettimeofday(&begin, 0);
    gettimeofday(&front_checkpoint, 0);

    currentState = LifeCoreState::RUN;
    cout<<__FUNCTION__<<endl;
    while(isRunFlag())
    {
        usleep(task_period);

        gettimeofday(&end_checkpoint, 0);
        currentThreadDelay=(end_checkpoint.tv_sec-front_checkpoint.tv_sec) * 1000000L + (end_checkpoint.tv_usec-front_checkpoint.tv_usec);
        cout<<__FUNCTION__<<" wake up "<<endl;

        if (currentThreadDelay > task_period )
        {

            gettimeofday(&front_checkpoint, 0);

            if (currentThreadDelay > task_period + task_deadline)
            {
                currentState = LifeCoreState::DEADLINE_EXCEEDED;
            }
            else 
            {
                currentState = LifeCoreState::RUN;
                main_loop();
            }
        }
        cout<<__FUNCTION__<<" end currentState = "<<currentState<<" "<<string_currentState[currentState]<<endl;
        
    }
}

int Serial_Port_WritingThread::main_loop()
{
    std::string buffer1="";
    
    cout<<__FUNCTION__<<endl;

    if(blc_control_commands.floats[0]>0.5 /*&& drone1.motors == UNARM*/)
    {
       cout<<" command_arm 1\n";
        drone1.command_arm(1);
    }
    if(blc_control_commands.floats[1]>0.5 /*&& drone1.motors == ARM*/)
    {
        cout<<" command_arm 0 \n";
        drone1.command_arm(0);
    }
    if(1/*drone1.motors == ARM*/)
    {
       cout<<" drone1.command_directControl \n";
        drone1.command_directControl(blc_control_motors.floats[1],blc_control_motors.floats[0],blc_control_motors.floats[3],blc_control_motors.floats[2]);
    }
    /*
    buffer1 = "Arm: "+std::to_string(arm);
    Display_IHM::getInstanceOf().printData(buffer1, 18, 1);*/

    /*switch (drone1.mode){
        case DRONE_OFF:        
            
        break;

        case DRONE_MANUAL_DIRECT:        
            
        break;
        
    }*/
    //*blc_control_arm
    //blc_control_remote_vector

    return 0;
}