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

7
#include <stdio.h>
8
9
#include <iostream>
#include <string>
10
11
12
13
14
15
16
17
#include <chrono>
#include <iostream>
#include <fstream>	


#include <sys/time.h>


18
19
20

using namespace std;

21
#include "../include/Com_SerialReadingThread.h"
22
#include "../include/global_variables.h"
23

24
extern ofstream MyFile;
25
26


27
28
//#define DISPLAY

29
double t_old = -1.;
30

31
Serial_Port_ReadingThread::Serial_Port_ReadingThread(int task_period, int task_deadline):
32
33
34
    Abstract_ThreadClass(task_period, task_deadline)
{
    //Init object
35
   // Serial_Port_ReadingThread::drone1 = drone1;
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61

    //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));
62
  //  blc_highres_imu = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.highres_imu", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 9));
63
64
65
66
67
68
69
70
71
    // 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()
{

}

72
73
void Serial_Port_ReadingThread::run()
{
74
    //long long currentThreadDelay;
75
76
77
78
79
80
81
    bool success=false;

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

    currentState = LifeCoreState::RUN;

82
83
    while(isRunFlag())
    {
84
85
86
        usleep(task_period);

        gettimeofday(&end_checkpoint, 0);
87
      //  currentThreadDelay=(end_checkpoint.tv_sec-front_checkpoint.tv_sec) * 1000000L + (end_checkpoint.tv_usec-front_checkpoint.tv_usec);
88
89
        
 /*       if (currentThreadDelay > task_period )
90
        {
91
92
            cout<<"ReadingThread delay = "<<currentThreadDelay<<endl;

93
94
            gettimeofday(&front_checkpoint, 0);

95
96
            if (currentThreadDelay > task_period + task_deadline)
            {
97
98
                currentState = LifeCoreState::DEADLINE_EXCEEDED;
            }
99
100
            else 
            {
101
102
                currentState = LifeCoreState::RUN;
                mavlink_message_t message;
103
104
105
                success = drone1.read_message(message);
                if(success)
                {
106
107
108
                    read_messages(message);
                }
            }
109
110
111
        }*/
        
    //    cout<<"ReadingThread delay = "<<currentThreadDelay<<endl;
112

113
114
115
116
117
               currentState = LifeCoreState::RUN;
                mavlink_message_t message;
                success = drone1.read_message(message);
                if(success)
                {
118
//cout<<__FUNCTION__<<" read_message call \n";
119
120
                    read_messages(message);
                }
121
122
123
    }
}

124
125
126

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

127
128
void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
{
129
130
	struct timeval t;
	double t_current, dt;
131
    std::string buffer1="";
132
    
133
134

    // Handle current timestamp
135
136
    drone1.timestamps = get_time_usec();
    buffer1 = "Time: "+std::to_string(drone1.timestamps);
137
138
139
    //Display_IHM::getInstanceOf().printData(buffer1, 7, 1);

    //Display_IHM::getInstanceOf().printLog(std::to_string(message.msgid));
140
    
141
142
   // cout<<"read_message "<<buffer1<<endl;
    // cout<<"  msgid = "<<message.msgid<<endl;
143
144
145
146
147
    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)
       {
148
       //   cout<<" message name = "<<mavlink_message_names[i].name<<endl;
149
150
151
152
          break;
       }
    }
    
153
154
155
156
157
    switch (message.msgid)
    {
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
            //Display_IHM::getInstanceOf().printLog("heartbeat");
158
            mavlink_msg_heartbeat_decode(&message, &drone1.heartbeat);
159
            buffer1 = 
160
161
162
163
164
165
                "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);
166
167
                
            cout<<buffer1<<endl;     
168
169
170
171
172
173
174
175
176

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

            break;
        }

        //ACK COMMAND
        case MAVLINK_MSG_ID_COMMAND_ACK:
        {
177
178
            mavlink_msg_command_ack_decode(&message, &drone1.ack);
            handle_command_ack(&drone1.ack);
179
180
181
182
183
184
            
            break;
        }

        case MAVLINK_MSG_ID_SYS_STATUS:
        {
185
186
            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);
187
            //Display_IHM::getInstanceOf().printData(buffer1, 9, 1);
188
            buffer1 = "Voltage: "+std::to_string(drone1.sys_status.voltage_battery);
189
190
            //Display_IHM::getInstanceOf().printData(buffer1, 10, 1);
            
191
192
            cout<<buffer1<<endl;
            
193
194
195
196
197
            break;
        }

        case MAVLINK_MSG_ID_BATTERY_STATUS:
        {
198
            mavlink_msg_battery_status_decode(&message, &drone1.battery_status);
199
200
201
202
203
            break;
        }

        case MAVLINK_MSG_ID_RADIO_STATUS:
        {
204
            mavlink_msg_radio_status_decode(&message, &drone1.radio_status);
205
206
207
208
209
210
            break;
        }

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

213
            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";
214
            //Display_IHM::getInstanceOf().printData(buffer1, 11, 1);
215
            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";
216
            //Display_IHM::getInstanceOf().printData(buffer1, 12, 1);
217
            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";
218
219
            //Display_IHM::getInstanceOf().printData(buffer1, 13, 1);

220
221
222
223
224
225
226
227
228
            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;
229
            
230
       //     cout<<"Com_SerialReadingThread "<<buffer1<<endl;
231
232
233
            break;
        }

234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
		case MAVLINK_MSG_ID_GPS_RAW_INT:
		{
		//	cout << "MAVLINK_MSG_ID_GPS_RAW_INT:" << endl;
			mavlink_msg_gps_raw_int_decode(&message, &drone1.gps_raw_int);
	//		current_messages.time_stamps.gps_raw_int = get_time_usec();
	//		this_timestamps.gps_raw_int = current_messages.time_stamps.gps_raw_int;
	/*		cout <<"\tlat: " << drone1.gps_raw_int.lat <<"\tlon: " << drone1.gps_raw_int.lon <<"\talt: " << drone1.gps_raw_int.alt <<endl;
			cout <<"\teph: " << drone1.gps_raw_int.eph <<"\tepv: " << drone1.gps_raw_int.epv <<endl;
			cout <<"\tvel: " << drone1.gps_raw_int.vel <<"\tcog: " << drone1.gps_raw_int.cog <<endl;
			cout <<"\tfix: " << (int)drone1.gps_raw_int.fix_type <<"\tsat: " << (int)drone1.gps_raw_int.satellites_visible <<endl;*/
			
			if( drone1.motors == UNARM) { drone1.departure_alt = drone1.gps_raw_int.alt; cout<<"un_armed alt set to "<<drone1.departure_alt<<endl; }
			break;
		}

249
250
251
        //GPS
        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
        {
252
            mavlink_msg_local_position_ned_decode(&message, &drone1.local_position_ned);
253

254
            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";
255
            //Display_IHM::getInstanceOf().printData(buffer1, 14, 1);
256
            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";
257
            //Display_IHM::getInstanceOf().printData(buffer1, 15, 1);
258
259
       //      cout<<buffer1<<endl;

260
261
262
263
264
265
266
267
268
            
            blc_local_position_ned.floats[0] = drone1.local_position_ned.x;
            blc_local_position_ned.floats[1] = drone1.local_position_ned.y;
            blc_local_position_ned.floats[2] = drone1.local_position_ned.z;
            blc_local_position_ned.floats[3] = drone1.local_position_ned.vx;
            blc_local_position_ned.floats[4] = drone1.local_position_ned.vy;
            blc_local_position_ned.floats[5] = drone1.local_position_ned.vz;
            
            
269
270
271
272
            break;
        }
        case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
        {
273
            mavlink_msg_position_target_local_ned_decode(&message, &drone1.position_target_local_ned);
274

275
276
277
278
            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);
         //   cout<<buffer1<<endl;

         /*   Display_IHM::getInstanceOf().printData(buffer1, 10, 1);
279
            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);
280
281
282
283
284
285
            Display_IHM::getInstanceOf().printData(buffer1, 11, 1);*/
            break;
        }

        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
        {
286
            mavlink_msg_global_position_int_decode(&message, &drone1.global_position_int);
287
            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);
288
289
      //     cout<<buffer1<<endl;

290
291
292
293
294
            //Display_IHM::getInstanceOf().printData(buffer1, 12, 1);*/
            
            blc_global_position.floats[0] = drone1.global_position_int.lat;
            blc_global_position.floats[1] = drone1.global_position_int.lon;
            blc_global_position.floats[2] = drone1.global_position_int.alt;
295
            cout<<buffer1<<endl;
296
            
297
298
299
300
            break;
        }

        //Attitude function
301
        
302
303
        case MAVLINK_MSG_ID_ATTITUDE:
        {
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
			gettimeofday(&t, NULL);
			 t_current = t.tv_sec+t.tv_usec*1e-6; 
			 
	//		cout<<"t_old = "<<t_old<<endl;
		
			if(t_old>0.)
			{
				dt=t_current-t_old;
		//		cout<<"\n \n >>>>  period  msg_id_attitude "<<dt<<"\n\n\n";
			}
			t_old=t_current;
		//	cout<<"t_old = "<<t_old<<endl;
			
		   MyFile << dt << endl;

319
            mavlink_msg_attitude_decode(&message, &drone1.attitude);
320
            
321
322
323
324
325
326
            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);*/
327
       //    cout<<"MAVLINK_MSG_ID_ATTITUDE     : \n"<<buffer1<<endl;
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
           
           blc_attitude.floats[0] = drone1.attitude.time_boot_ms;
           blc_attitude.floats[1] = drone1.attitude.roll;
           blc_attitude.floats[2] = drone1.attitude.pitch;
           blc_attitude.floats[3] = drone1.attitude.yaw;
           blc_attitude.floats[4] = drone1.attitude.rollspeed;
           blc_attitude.floats[5] = drone1.attitude.pitchspeed;
           blc_attitude.floats[6] = drone1.attitude.yawspeed;
           
           /*blc_attitude.floats[0] = 5.24;
           blc_attitude.floats[1] = 4.65;
           blc_attitude.floats[2] = 0.45;
           blc_attitude.floats[3] = 8.15;
           blc_attitude.floats[4] = 2.59;
           blc_attitude.floats[5] = 1.57;
           blc_attitude.floats[6] = 1.20;*/
344
345
                    
          break;
346
        }
347
	
348
349
        case MAVLINK_MSG_ID_ATTITUDE_TARGET:
        {
350
            mavlink_msg_attitude_target_decode(&message, &drone1.attitude_target);
351

352
            /*buffer1 = "Attitude_target: "+std::to_string(drone1.attitude_target.time_boot_ms);
353
            Display_IHM::getInstanceOf().printData(buffer1, 16, 1);
354
            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]);
355
            Display_IHM::getInstanceOf().printData(buffer1, 17, 1);
356
            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);
357
            Display_IHM::getInstanceOf().printData(buffer1, 18, 1);
358
            buffer1 = "Attitude_tar_m: "+std::to_string(drone1.attitude_target.type_mask);
359
            Display_IHM::getInstanceOf().printData(buffer1, 19, 1);*/
360
            
361
362
363
364
365
366
367
368
369
370
371
            break;
        }

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


    }
372
 //  cout<<"End function : "<<buffer1<<endl;
373
374
375
} 


376
377
uint64_t Serial_Port_ReadingThread::get_time_usec()
{
378
379
380
381
382
	static struct timeval _time_stamp;
	gettimeofday(&_time_stamp, NULL);
	return _time_stamp.tv_sec*1000000 + _time_stamp.tv_usec;
}

383
384
385
int Serial_Port_ReadingThread::testAck(mavlink_command_ack_t *ack, std::string message)
{
    string buffer1="-> Ack- ";
386
387
388
389
390
391
392
393
394
395
396
    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;
    }
397
    cout<<"testAck "<<buffer1<<endl;
398
399
400
401
}

void Serial_Port_ReadingThread::handle_command_ack(mavlink_command_ack_t *ack)
{
402
403
    switch (ack->command) 
    {
404
        case MAV_CMD_COMPONENT_ARM_DISARM:
405
406
            if(testAck(ack, "Arm_desarm") == 1)
            { //if command is succeed
407
                if(drone1.motors == ARM){
408
                    drone1.motors = UNARM;
409
                } else if(drone1.motors == UNARM){
410
                    drone1.motors = ARM;
411
                }
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
            }
            //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);
    
}