Com_SerialReadingThread.cpp 13.5 KB
Newer Older
1
2
3
4
5
6
/**
 *  @author Sylvain Colomer
 *  @date 18/04/19.
 *  trash-put /dev/shm/file
 */

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

using namespace std;

12
#include "../include/Com_SerialReadingThread.h"
13
#include "../include/global_variables.h"
14
15
16
17

//#define DISPLAY


18
Serial_Port_ReadingThread::Serial_Port_ReadingThread(int task_period, int task_deadline):
19
20
21
    Abstract_ThreadClass(task_period, task_deadline)
{
    //Init object
22
   // Serial_Port_ReadingThread::drone1 = drone1;
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

    //Init dictionnary
    //mavlink_dic
    //ifstream file1("test.txt", ios::in); 
    //std::string buffer1;
    /*if(file1)
    {
        
        while(getline(file1, buffer1))
        {
            std::cout << line << std::endl;
        }
    }
    else{
        std::cout<< "Error : unable to open file" << std::endl;
        exit(1);
    }
    */

    //Creation of blc channels
    //Example: blc_heartbeat = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.heartbeat", BLC_CHANNEL_WRITE, 'IN16', 'NDEF', 1, 6));

    //blc_local_position_ned = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.local_position_ned", BLC_CHANNEL_WRITE, 'IN16', 'NDEF', 1, 6));
    //blc_global_position_int = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.global_position_int", BLC_CHANNEL_WRITE, 'IN16', 'NDEF', 1, 6));
    // blc_position_target_local_ned = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.position_target_local_ned", BLC_CHANNEL_WRITE, 'IN16', 'NDEF', 1, 6));
    // blc_position_target_global_int = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.position_target_global_int", BLC_CHANNEL_WRITE, 'IN16', 'NDEF', 1, 6));
49
  //  blc_highres_imu = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.highres_imu", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 9));
50
51
52
53
54
55
56
57
58
    // blc_attitude = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.attitude", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 9));

}

Serial_Port_ReadingThread::~Serial_Port_ReadingThread()
{

}

59
60
void Serial_Port_ReadingThread::run()
{
61
62
63
64
65
66
67
68
    long long currentThreadDelay;
    bool success=false;

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

    currentState = LifeCoreState::RUN;

69
70
    while(isRunFlag())
    {
71
72
73
74
        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);
75
76
        
 /*       if (currentThreadDelay > task_period )
77
        {
78
79
            cout<<"ReadingThread delay = "<<currentThreadDelay<<endl;

80
81
            gettimeofday(&front_checkpoint, 0);

82
83
            if (currentThreadDelay > task_period + task_deadline)
            {
84
85
                currentState = LifeCoreState::DEADLINE_EXCEEDED;
            }
86
87
            else 
            {
88
89
                currentState = LifeCoreState::RUN;
                mavlink_message_t message;
90
91
92
                success = drone1.read_message(message);
                if(success)
                {
93
94
95
                    read_messages(message);
                }
            }
96
97
98
99
100
101
102
103
104
105
106
107
        }*/
        
    //    cout<<"ReadingThread delay = "<<currentThreadDelay<<endl;
        
               currentState = LifeCoreState::RUN;
                mavlink_message_t message;
                success = drone1.read_message(message);
                if(success)
                {
                   cout<<__FUNCTION__<<" read_message call \n";
                    read_messages(message);
                }
108
109
110
    }
}

111
112
113

	static const struct { const char *name; uint32_t msgid; } mavlink_message_names[] = MAVLINK_MESSAGE_NAMES;

114
115
void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
{
116
117
118
    std::string buffer1="";

    // Handle current timestamp
119
120
    drone1.timestamps = get_time_usec();
    buffer1 = "Time: "+std::to_string(drone1.timestamps);
121
122
123
    //Display_IHM::getInstanceOf().printData(buffer1, 7, 1);

    //Display_IHM::getInstanceOf().printLog(std::to_string(message.msgid));
124
    
125
126
127
128
129
130
131
132
133
134
135
136
    cout<<__FUNCTION__<<buffer1<<endl;
    cout<<"  msgid = "<<message.msgid<<endl;
    for(unsigned int i=0;i<sizeof(mavlink_message_names);i++)
    {
      // cout<<"i = "<<i<<" "<<mavlink_message_names[i].msgid<<" "<<mavlink_message_names[i].name<<endl;
       if(mavlink_message_names[i].msgid==message.msgid)
       {
          cout<<" message name = "<<mavlink_message_names[i].name<<endl;
          break;
       }
    }
    
137
138
139
140
141
    switch (message.msgid)
    {
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
            //Display_IHM::getInstanceOf().printLog("heartbeat");
142
            mavlink_msg_heartbeat_decode(&message, &drone1.heartbeat);
143
            buffer1 = 
144
145
146
147
148
149
                "Hearthbeat:"+std::to_string(drone1.heartbeat.type)+
                " "+std::to_string(drone1.heartbeat.autopilot)+
                " "+std::to_string(drone1.heartbeat.base_mode)+
                " "+std::to_string(drone1.heartbeat.custom_mode)+
                " "+std::to_string(drone1.heartbeat.system_status)+
                " "+std::to_string(drone1.heartbeat.mavlink_version);
150
151
                
            cout<<buffer1<<endl;     
152
153
154
155
156
157
158
159
160

            //Display_IHM::getInstanceOf().printData(buffer1, 8, 1);

            break;
        }

        //ACK COMMAND
        case MAVLINK_MSG_ID_COMMAND_ACK:
        {
161
162
            mavlink_msg_command_ack_decode(&message, &drone1.ack);
            handle_command_ack(&drone1.ack);
163
164
165
166
167
168
            
            break;
        }

        case MAVLINK_MSG_ID_SYS_STATUS:
        {
169
170
            mavlink_msg_sys_status_decode(&message, &drone1.sys_status);
            buffer1 = "Error: "+std::to_string(drone1.sys_status.errors_count1)+" "+std::to_string(drone1.sys_status.errors_count2)+" "+std::to_string(drone1.sys_status.errors_count3)+" "+std::to_string(drone1.sys_status.errors_count4);
171
            //Display_IHM::getInstanceOf().printData(buffer1, 9, 1);
172
            buffer1 = "Voltage: "+std::to_string(drone1.sys_status.voltage_battery);
173
174
            //Display_IHM::getInstanceOf().printData(buffer1, 10, 1);
            
175
176
            cout<<buffer1<<endl;
            
177
178
179
180
181
            break;
        }

        case MAVLINK_MSG_ID_BATTERY_STATUS:
        {
182
            mavlink_msg_battery_status_decode(&message, &drone1.battery_status);
183
184
185
186
187
            break;
        }

        case MAVLINK_MSG_ID_RADIO_STATUS:
        {
188
            mavlink_msg_radio_status_decode(&message, &drone1.radio_status);
189
190
191
192
193
194
            break;
        }

        //ACCELEROMETERS
        case MAVLINK_MSG_ID_HIGHRES_IMU:
        {
195
            mavlink_msg_highres_imu_decode(&message, &drone1.highres_imu);
196

197
            buffer1 = buffer1+" Acc: "+std::to_string(drone1.highres_imu.xacc)+" "+std::to_string(drone1.highres_imu.yacc)+" "+std::to_string(drone1.highres_imu.zacc)+"\n";
198
            //Display_IHM::getInstanceOf().printData(buffer1, 11, 1);
199
            buffer1 = buffer1+" Gyro: "+std::to_string(drone1.highres_imu.xgyro)+" "+std::to_string(drone1.highres_imu.ygyro)+" "+std::to_string(drone1.highres_imu.zgyro)+"\n";
200
            //Display_IHM::getInstanceOf().printData(buffer1, 12, 1);
201
            buffer1 = buffer1+" Mag: "+std::to_string(drone1.highres_imu.xmag)+" "+std::to_string(drone1.highres_imu.ymag)+" "+std::to_string(drone1.highres_imu.zmag)+"\n";
202
203
            //Display_IHM::getInstanceOf().printData(buffer1, 13, 1);

204
205
206
207
208
209
210
211
212
            blc_highres_imu.floats[0] = drone1.highres_imu.xacc;
            blc_highres_imu.floats[1] = drone1.highres_imu.yacc;
            blc_highres_imu.floats[2] = drone1.highres_imu.zacc;
            blc_highres_imu.floats[3] = drone1.highres_imu.xgyro;
            blc_highres_imu.floats[4] = drone1.highres_imu.ygyro;
            blc_highres_imu.floats[5] = drone1.highres_imu.zgyro;
            blc_highres_imu.floats[6] = drone1.highres_imu.xmag;
            blc_highres_imu.floats[7] = drone1.highres_imu.ymag;
            blc_highres_imu.floats[8] = drone1.highres_imu.zmag;
213
214
            
            cout<<"Com_SerialReadingThread "<<buffer1<<endl;
215
216
217
218
219
220
            break;
        }

        //GPS
        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
        {
221
            mavlink_msg_local_position_ned_decode(&message, &drone1.local_position_ned);
222

223
            buffer1 =  buffer1 + "\n Pos_coord: "+std::to_string(drone1.local_position_ned.x)+" "+std::to_string(drone1.local_position_ned.y)+" "+std::to_string(drone1.local_position_ned.z)+"\n";
224
            //Display_IHM::getInstanceOf().printData(buffer1, 14, 1);
225
            buffer1 = "Pos_speed: "+std::to_string(drone1.local_position_ned.vx)+" "+std::to_string(drone1.local_position_ned.vy)+" "+std::to_string(drone1.local_position_ned.vz)+"\n";
226
227
228
229
230
            //Display_IHM::getInstanceOf().printData(buffer1, 15, 1);
            break;
        }
        case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
        {
231
            mavlink_msg_position_target_local_ned_decode(&message, &drone1.position_target_local_ned);
232

233
            /*buffer1 = "Pos_coord_t: "+std::to_string(drone1.position_target_local_ned.x)+" "+std::to_string(drone1.position_target_local_ned.y)+" "+std::to_string(drone1.position_target_local_ned.z);
234
            Display_IHM::getInstanceOf().printData(buffer1, 10, 1);
235
            buffer1 = "Pos_speed_t: "+std::to_string(drone1.position_target_local_ned.vx)+" "+std::to_string(drone1.position_target_local_ned.vy)+" "+std::to_string(drone1.position_target_local_ned.vz);
236
237
238
239
240
241
            Display_IHM::getInstanceOf().printData(buffer1, 11, 1);*/
            break;
        }

        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
        {
242
243
            /*mavlink_msg_global_position_int_decode(&message, &drone1.global_position_int);
            buffer1 = "GPS: "+std::to_string(drone1.global_position_int.lat)+" "+std::to_string(drone1.global_position_int.lon)+" "+std::to_string(drone1.global_position_int.alt);
244
245
246
247
248
249
250
            Display_IHM::getInstanceOf().printData(buffer1, 12, 1);*/
            break;
        }

        //Attitude function
        case MAVLINK_MSG_ID_ATTITUDE:
        {
251
            mavlink_msg_attitude_decode(&message, &drone1.attitude);
252
            
253
254
255
256
257
258
259
            buffer1 = buffer1 + "\n Attitude_time: "+to_string(drone1.attitude.time_boot_ms)+"\n";
           // Display_IHM::getInstanceOf().printData(buffer1, 13, 1);
            buffer1 = buffer1 + " Attitude_p (roll, pitch, yaw): "+to_string(drone1.attitude.roll)+" "+to_string(drone1.attitude.pitch)+" "+std::to_string(drone1.attitude.yaw)+"\n";
           // Display_IHM::getInstanceOf().printData(buffer1, 14, 1);
            buffer1 = buffer1 + " Attitude_s: "+to_string(drone1.attitude.rollspeed)+" "+to_string(drone1.attitude.pitchspeed)+" "+to_string(drone1.attitude.yawspeed)+"\n";
          //  Display_IHM::getInstanceOf().printData(buffer1, 15, 1);*/
           cout<<"MAVLINK_MSG_ID_ATTITUDE : \n"<<buffer1<<endl;
260
261
262
263
264
            
            break;
        }
        case MAVLINK_MSG_ID_ATTITUDE_TARGET:
        {
265
            mavlink_msg_attitude_target_decode(&message, &drone1.attitude_target);
266

267
            /*buffer1 = "Attitude_target: "+std::to_string(drone1.attitude_target.time_boot_ms);
268
            Display_IHM::getInstanceOf().printData(buffer1, 16, 1);
269
            buffer1 = "Attitude_target_q: "+std::to_string(drone1.attitude_target.q[0])+" "+std::to_string(drone1.attitude_target.q[1])+" "+std::to_string(drone1.attitude_target.q[2])+" "+std::to_string(drone1.attitude_target.q[3]);
270
            Display_IHM::getInstanceOf().printData(buffer1, 17, 1);
271
            buffer1 = "Attitude_tar_r: "+std::to_string(drone1.attitude_target.body_roll_rate)+" "+std::to_string(drone1.attitude_target.body_pitch_rate)+" "+std::to_string(drone1.attitude_target.body_yaw_rate);
272
            Display_IHM::getInstanceOf().printData(buffer1, 18, 1);
273
            buffer1 = "Attitude_tar_m: "+std::to_string(drone1.attitude_target.type_mask);
274
275
276
277
278
279
280
281
282
283
284
285
            Display_IHM::getInstanceOf().printData(buffer1, 19, 1);*/
            break;
        }

        default:
        {
            //printf("Warning, did not handle message id %i\n",message.msgid);
            break;
        }


    }
286
   cout<<"End function : "<<buffer1<<endl;
287
288
289
} 


290
291
uint64_t Serial_Port_ReadingThread::get_time_usec()
{
292
293
294
295
296
	static struct timeval _time_stamp;
	gettimeofday(&_time_stamp, NULL);
	return _time_stamp.tv_sec*1000000 + _time_stamp.tv_usec;
}

297
298
299
int Serial_Port_ReadingThread::testAck(mavlink_command_ack_t *ack, std::string message)
{
    string buffer1="-> Ack- ";
300
301
302
303
304
305
306
307
308
309
310
    buffer1+=message;

    if (ack->result == MAV_RESULT_ACCEPTED) {
        buffer1+="-succes";
        //Display_IHM::getInstanceOf().printLog(buffer1);
        return 0;
    } else {
        buffer1+="-fail";
        //Display_IHM::getInstanceOf().printLog(buffer1);
        return 1;
    }
311
    cout<<__FUNCTION__<<buffer1<<endl;
312
313
314
315
}

void Serial_Port_ReadingThread::handle_command_ack(mavlink_command_ack_t *ack)
{
316
317
    switch (ack->command) 
    {
318
        case MAV_CMD_COMPONENT_ARM_DISARM:
319
320
            if(testAck(ack, "Arm_desarm") == 1)
            { //if command is succeed
321
322
323
324
                /*if(drone1.motors == ARM){
                    drone1.motors = UNARM;
                }else if(drone1.motors == UNARM){
                    drone1.motors = ARM;
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
                }*/
            }
            //motors
            break;
        case MAV_CMD_NAV_GUIDED_ENABLE:
            testAck(ack, "navGuided");
            break;
        case MAVLINK_MSG_ID_PLAY_TUNE:
            testAck(ack, "Tone");
            break;
        case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
            testAck(ack, "Attitude");
            break;
        case MAVLINK_MSG_ID_MANUAL_CONTROL:
            testAck(ack, "Manual control");
            break;
        default:
            testAck(ack, "Unknow");
            break;
    }
    ////Display_IHM::getInstanceOf().displayDroneState(drone1);
    
}