Com_SerialWritingThread.cpp 2.59 KB
Newer Older
1
/**
2
 *  @author Sylvain Colomer, P. Gaussier
3
4
5
 *  @date 18/04/19.
 */

6
7
8
9
10
#include <iostream>
#include <string>

using namespace std;

11
#include "../include/Com_SerialWritingThread.h"
12
#include "../include/global_variables.h"
13
14


15
Serial_Port_WritingThread::Serial_Port_WritingThread(int task_period, int task_deadline):
16
17
    Abstract_ThreadClass(task_period, task_deadline)
{
18
//    Serial_Port_WritingThread::drone1 = drone1;
19
20
21
22
23
24
25
26
27


}

Serial_Port_WritingThread::~Serial_Port_WritingThread()
{

}

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

30
31
void Serial_Port_WritingThread::run()
{
32
33
34
35
36
37
    long long currentThreadDelay;

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

    currentState = LifeCoreState::RUN;
38
39
40
    cout<<__FUNCTION__<<endl;
    while(isRunFlag())
    {
41
42
43
44
        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);
45
        cout<<__FUNCTION__<<" wake up "<<endl;
46

47
48
        if (currentThreadDelay > task_period )
        {
49
50
51

            gettimeofday(&front_checkpoint, 0);

52
53
            if (currentThreadDelay > task_period + task_deadline)
            {
54
55
                currentState = LifeCoreState::DEADLINE_EXCEEDED;
            }
56
57
            else 
            {
58
59
60
61
                currentState = LifeCoreState::RUN;
                main_loop();
            }
        }
62
63
        cout<<__FUNCTION__<<" end currentState = "<<currentState<<" "<<string_currentState[currentState]<<endl;
        
64
65
66
    }
}

67
68
int Serial_Port_WritingThread::main_loop()
{
69
    std::string buffer1="";
70
71
    
    cout<<__FUNCTION__<<endl;
72

73
74
75
    if(blc_control_commands.floats[0]>0.5 /*&& drone1.motors == UNARM*/)
    {
       cout<<" command_arm 1\n";
76
        drone1.command_arm(1);
77
    }
78
79
80
    if(blc_control_commands.floats[1]>0.5 /*&& drone1.motors == ARM*/)
    {
        cout<<" command_arm 0 \n";
81
        drone1.command_arm(0);
82
    }
83
84
85
    if(1/*drone1.motors == ARM*/)
    {
       cout<<" drone1.command_directControl \n";
86
        drone1.command_directControl(blc_control_motors.floats[1],blc_control_motors.floats[0],blc_control_motors.floats[3],blc_control_motors.floats[2]);
87
88
89
90
91
    }
    /*
    buffer1 = "Arm: "+std::to_string(arm);
    Display_IHM::getInstanceOf().printData(buffer1, 18, 1);*/

92
    /*switch (drone1.mode){
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
        case DRONE_OFF:        
            
        break;

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

    return 0;
}