Cmd_ControlThread.cpp 1.91 KB
Newer Older
1
2
3
4
5
6
/**
 *  @author Sylvain Colomer
 *  @date 18/04/19.
 */

#include "../include/Cmd_ControlThread.h"
7
#include "../include/global_variables.h"
8

9
Serial_Port_WritingThread::Serial_Port_WritingThread(int task_period, int task_deadline):
10
11
    Abstract_ThreadClass(task_period, task_deadline)
{
12
 //   Serial_Port_WritingThread::drone1 = drone1;
13
14

    //Channels use to arm the drone
15
//    blc_control_arm= std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.control.arm", BLC_CHANNEL_READ, 'IN16', 'NDEF', 1, 1)); // 1=on, 0=off
16
17

    //Channels use to control manually the drone
18
 //   blc_control_remote_vector = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.control.remoteVectors", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 4));
19
20
21
22
23
24
25
26
}

Serial_Port_WritingThread::~Serial_Port_WritingThread()
{

}


27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
int Serial_Port_WritingThread::main_loop()
{

    //Main loop of the system
    switch (drone1.mode==DRONE_OFF)
    {
        
    }
    // We take care only for difference in mode control
    /*if(blc_message_arm.ints16[0] ==1){

    }*/

    return 0;
}


void Serial_Port_WritingThread::run()
{
46
47
48
49
50
51
52
    long long currentThreadDelay;

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

    currentState = LifeCoreState::RUN;

53
54
    while(isRunFlag())
    {
55
56
57
58
59
        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);

60
61
        if (currentThreadDelay > task_period )
        {
62
63
            gettimeofday(&front_checkpoint, 0);

64
65
            if (currentThreadDelay > task_period + task_deadline)
            {
66
67
                currentState = LifeCoreState::DEADLINE_EXCEEDED;
            }
68
69
            else 
            {  //PG: strange code (check older version)
70
                currentState = LifeCoreState::RUN;
71
                main_loop(); // PG: strange name?
72
73
74
75
76
            }
        }
    }
}