Thread_SerialPort.cpp 14.7 KB
Newer Older
1
/**
2
 *  @author Sylvain Colomer, Alexis Constant, P. Gaussier
3
4
5
 *  @date 19/11/19.
 *  trash-put /dev/shm/file
 */
6
7
8
9
10
 
#include <iostream>
#include <string>

using namespace std;
11
12

#include "../include/Thread_SerialPort.h"
13
#include "../include/global_variables.h"
14
15
16

//#define DISPLAY

17
18
19
20
//Channels use to control manually the drone
blc_channel  blc_control_motors("/pixhawk.control.motors", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 4); //x, y, z, r
//Channels use to command the drone
blc_channel  blc_control_commands("/pixhawk.control.commands", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 4); // 1=on, 0=off
21

22
blc_channel  blc_drone_data("/pixhawk.control.data", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 12); // 1=on, 0=off
23
24
25
26
27
28
29
30
31
32
33
34
35
    //Roll,Pitch,Yaw (3), xAcc, yAcc, zAcc (3), Bat, Heading, Heartbeat (3), Lat, Lon, Alt (3),  => 4x3=12 
    // battery_status.battery_remaining
    // altitude.altitude_relative
    // attitude.pitch*180/PI
    // attitude.roll*180/PI
    // attitude.yaw*180/PI
    // vfr_hud.heading
    // highres_imu.xgyro
    // highres_imu.ygyro
    // highres_imu.zgyro

    //Create or reopen an asynchrone channel "/channel_example" sending data of type int8/char in format undefined format of one dimension (vector) of SIZE.
/*    channel.create_or_open("/pixhawks.control.data", BLC_CHANNEL_WRITE, 'UIN8', 'TEXT',  1, SIZE);*/
36
37
38
39
40
41
42

Thread_SerialPort::Thread_SerialPort(int task_period, int task_deadline, std::shared_ptr<Serial_Port> serial_Port_1):
    Abstract_ThreadClass(task_period, task_deadline)
{
    Thread_SerialPort::serial_Port_1 = serial_Port_1;
    
    cout<<"init shared memory\n";
43
    blc_drone_data.reset(); //Clear the memory 
44
45
46
47
48
49
50
51

}

Thread_SerialPort::~Thread_SerialPort()
{

}

52
53
54
void Thread_SerialPort::run()
{
   cout<<"start Thread_SErialPort run()\n";
55
56
57
58
59
60
    long long currentThreadDelay;
    bool success=false;

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

61
62
    while(isRunFlag())
    {
63
64
65
66
67
68
        state = THREAD_STATE_WORK_SLEEP;
        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);

69
70
        if (currentThreadDelay > task_period )
        {
71
72
            gettimeofday(&front_checkpoint, 0);

73
74
            if (currentThreadDelay > task_period + task_deadline)
            {
75
76
                state = THREAD_STATE_WORK_DEADLINE;
            }
77
78
            else 
            {
79
                state = THREAD_STATE_WORK_TASK;
80
              //  cout<<"R/W drone \n ";
81
82

                write_messages();
83
              //  cout<<"end W \n";
84
85

                //Task1: read message
86
87
                if(mavlink1.get()->read_message_serial(message, serial_Port_1))
                {
88
89
                    read_messages();
                }
90
              //  cout<<"end R/W \n";
91
92
93
94
95
            }
        }
    }
}

96
97
void Thread_SerialPort::write_messages()
{
98
    if(blc_control_commands.floats[0]>0.5) //arm command
99
100
    {
				//printf("mémoire partagée : demande armement \n");
101
				//blc_control_commands.floats[0] = 0.0;
102
  //      blc_control_commands.floats[0] = 0.0;
103
104
105
        serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_arm(1));
								
    }
106
    	
107
    if(blc_control_commands.floats[1]>0.5) // arm unarm command
108
109
    {
						//printf("mémoire partagée : demande desarmement \n");
110
						//blc_control_commands.floats[0] = 0.0;
111
    //    blc_control_commands.floats[1] = 0.0;
112
113
114
        serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_arm(1));
    }
			
115
	if(blc_control_motors.floats[0]>0.5) // right command
116
117
   { 
         //printf("mémoire partagée : demande motor 0 \n"); 
118
         //blc_control_commands.floats[1] = 0.0;S
119
         serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_right(1));
120
					
121
   }
122
			
123
124
125
   else // left command
   { 
         //printf("mémoire partagée : demande motor < 0,5 \n"); 
126
         //blc_control_commands.floats[1] = 0.0;S
127
128
         serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_left(1));
   }
129
			
130
//	while (blc_control_motors.floats[1]<0.5) //PB si on a un while ici PG !
131
//   {
132
			if(blc_control_motors.floats[1]>0.5) // up/down axis command
133
         { 
134
						//printf("mémoire partagée : demande motors 1 > 0,5 \n"); 
135
						//blc_control_commands.floats[1] = 0.0;S
136
               serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_up(1));
137
138
					//	action.set_takeoff_altitude(3.0);
			
139
140
141
         }
			else  // PG : ????
         { 
142
						//printf("mémoire partagée : demande motor 1 < 0,5 \n"); 
143
						//blc_control_commands.floats[1] = 0.0;S
144
145
               serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_down(1));
         }
146
					
147
//		}
148
149
	
			
150
	if(blc_control_motors.floats[3]>0.5) //pow command
151
152
   {
         //printf("mémoire partagée : demande d'augmentation de la puissance \n");
153
         //blc_control_commands.floats[0] = 0.0;
154
         serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_pow(1));
155
				
156
   }
157
158
159

    // COMMAND FOR CONTROL THE DRONE. THIS PART OF THE THREAD NEED TO BE CHANGE BEFORE ALL APPLICATION
    // if(1){
160
    //     drone1.command_directControl(blc_control_motors.floats[1],blc_control_motors.floats[0],blc_control_motors.floats[3],blc_control_motors.floats[2]);
161
162
163
164
    // }
}


165
166
void Thread_SerialPort::read_messages()
{
167
    string buffer1="";
168
    Bus::getInstanceOf().event_newMessage(message);
169
 //   cout<< "LOG "<<message.msgid<<"\n";
170
171
172
173
174
175
176
177
178
179
180
181
182
183

    switch (message.msgid)
    {
        //SENSORS HANG
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
            mavlink_msg_heartbeat_decode(&message, &heartbeat);
            Bus::getInstanceOf().update(heartbeat);
            break;
        }
        case MAVLINK_MSG_ID_SYS_STATUS:
        {
            mavlink_msg_sys_status_decode(&message, &sys_status);
            Bus::getInstanceOf().update(sys_status);
184
185
            
            buffer1 = "Error: "+std::to_string(sys_status.errors_count1)+" "+std::to_string(sys_status.errors_count2)+" "+std::to_string(sys_status.errors_count3)+" "+std::to_string(sys_status.errors_count4);
186
            //Display_IHM::getInstanceOf().printData(buffer1, 9, 1);
187
            buffer1 = "Voltage: "+std::to_string(sys_status.voltage_battery);
188
189
190
191
192
            //Display_IHM::getInstanceOf().printData(buffer1, 10, 1);*/
            break;
        }

        //ACCELEROMETERS
193
        case MAVLINK_MSG_ID_HIGHRES_IMU:
194
195
        {
            mavlink_msg_highres_imu_decode(&message, &highres_imu);
196
197
            //mavlink_msg_highres_imu_decode(&message, &drone1.highres_imu);
         //   Bus::getInstanceOf().update(highres_imu);
198

199
            buffer1 = "Acc: "+std::to_string(highres_imu.xacc)+" "+std::to_string(highres_imu.yacc)+" "+std::to_string(highres_imu.zacc);
200
201
202
203
204
205
            //Display_IHM::getInstanceOf().printData(buffer1, 11, 1);
            buffer1 = "Gyro: "+std::to_string(highres_imu.xgyro)+" "+std::to_string(highres_imu.ygyro)+" "+std::to_string(highres_imu.zgyro);
            //Display_IHM::getInstanceOf().printData(buffer1, 12, 1);
            buffer1 = "Mag: "+std::to_string(highres_imu.xmag)+" "+std::to_string(highres_imu.ymag)+" "+std::to_string(highres_imu.zmag);
            //Display_IHM::getInstanceOf().printData(buffer1, 13, 1);
            break;
206
        }
207
208
209
210

        //ACK COMMAND
        /*case MAVLINK_MSG_ID_COMMAND_ACK:
        {
211
212
            mavlink_msg_command_ack_decode(&message, &drone1.ack);
            handle_command_ack(&drone1.ack);
213
214
215
216
217
218
219
            break;
        }



        case MAVLINK_MSG_ID_BATTERY_STATUS:
        {
220
            mavlink_msg_battery_status_decode(&message, &drone1.battery_status);
221
222
223
224
225
            break;
        }

        case MAVLINK_MSG_ID_RADIO_STATUS:
        {
226
            mavlink_msg_radio_status_decode(&message, &drone1.radio_status);
227
            break;
228
        }*/
229
230
231
232



        //GPS
233
     /*   case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
234
        {
235
            mavlink_msg_local_position_ned_decode(&message, &drone1.local_position_ned);
236

237
            buffer1 = "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);
238
            //Display_IHM::getInstanceOf().printData(buffer1, 14, 1);
239
            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);
240
241
242
243
244
            //Display_IHM::getInstanceOf().printData(buffer1, 15, 1);
            break;
        }
        case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
        {
245
            mavlink_msg_position_target_local_ned_decode(&message, &drone1.position_target_local_ned);
246

247
            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);
248
            // Display_IHM::getInstanceOf().printData(buffer1, 10, 1);
249
            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);
250
251
252
253
254
255
256
257
258
259
260
261
262
263
            // Display_IHM::getInstanceOf().printData(buffer1, 11, 1);
            break;
        }*/

        /*case MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET:{
            mavlink_msg_local_position_ned_system_global_offset_decode(&message, &local_position_ned_system_global_offset);
            Bus::getInstanceOf().update(local_position_ned_system_global_offset);
            break;
        }*/
        
        /*case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
        {
            mavlink_msg_global_position_int_decode(&message, &global_position_int);
            Bus::getInstanceOf().update(global_position_int);
264
265
            // 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);
266
267
268
269
270
271
272
273
274
275
            // Display_IHM::getInstanceOf().printData(buffer1, 12, 1);
            break;
        }*/

        /*case MAVLINK_MSG_ID_GPS_STATUS:{
            mavlink_msg_gps_status_decode(&message, &gps_status);
            Bus::getInstanceOf().update(gps_status);
            break;
        }*/

276
        /*
277
278
        case MAVLINK_MSG_ID_GPS_RAW_INT:
        {
279
280
281
            mavlink_msg_gps_raw_int_decode(&message, &gps_raw_int);
            Bus::getInstanceOf().update(gps_raw_int);
            break;
282
        }*/
283
284
285
286

        //Attitude function
        case MAVLINK_MSG_ID_ATTITUDE:
        {
287
            //mavlink_msg_attitude_decode(&message, &drone1.attitude);
288
289
290
            mavlink_msg_attitude_decode(&message, &attitude);
            Bus::getInstanceOf().update(attitude);
            
291
            buffer1 = "Attitude_t: "+std::to_string(attitude.time_boot_ms);
292
            // Display_IHM::getInstanceOf().printData(buffer1, 13, 1);
293
             buffer1 = "Attitude_p: "+std::to_string(attitude.roll)+" "+std::to_string(attitude.pitch)+" "+std::to_string(attitude.yaw);
294
            // Display_IHM::getInstanceOf().printData(buffer1, 14, 1);
295
             buffer1 = "Attitude_s: "+std::to_string(attitude.rollspeed)+" "+std::to_string(attitude.pitchspeed)+" "+std::to_string(attitude.yawspeed);
296
297
298
299
            // Display_IHM::getInstanceOf().printData(buffer1, 15, 1);
            
            break;
        }
300
        /*
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
        case MAVLINK_MSG_ID_VFR_HUD:{
            mavlink_msg_vfr_hud_decode(&message, &vfr_hud);
            Bus::getInstanceOf().update(vfr_hud);
            break;
        }
        
        case MAVLINK_MSG_ID_FLIGHT_INFORMATION:{
            mavlink_msg_flight_information_decode(&message, &flight_information);
            Bus::getInstanceOf().update(flight_information);
            break;
        }

        case MAVLINK_MSG_ID_MANUAL_CONTROL:{
            mavlink_msg_manual_control_decode(&message, &manual_control);
            Bus::getInstanceOf().update(manual_control);
            break;
        }
318
        */
319
320
        /*case MAVLINK_MSG_ID_ATTITUDE_TARGET:
        {
321
            mavlink_msg_attitude_target_decode(&message, &drone1.attitude_target);
322

323
324
325
             buffer1 = "Attitude_target: "+std::to_string(drone1.attitude_target.time_boot_ms);
            //Display_IHM::getInstanceOf().printData(buffer1, 16, 1);
             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]);
326
            // Display_IHM::getInstanceOf().printData(buffer1, 17, 1);
327
             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);
328
            // Display_IHM::getInstanceOf().printData(buffer1, 18, 1);
329
             buffer1 = "Attitude_tar_m: "+std::to_string(drone1.attitude_target.type_mask);
330
331
332
333
334
335
            // Display_IHM::getInstanceOf().printData(buffer1, 19, 1);
            break;
        }*/

        default:
        {
336
            printf("Warning, did not handle message id %i\n",message.msgid);
337
338
339
            break;
        }
    }
340
    cout<<"Thread_SerialPort "<<buffer1<<endl;
341
342
343

} 

344
345
int Thread_SerialPort::testAck(mavlink_command_ack_t *ack, std::string message)
{
346
347
348
    std::string buffer1="-> Ack-";
    buffer1+=message;

349
350
    if (ack->result == MAV_RESULT_ACCEPTED) 
    {
351
352
353
        buffer1+="-succes";
        //Display_Interface::Instance_of().print_Log(buffer1);
        return 0;
354
355
    } else 
    {
356
357
358
359
360
361
362
363
        buffer1+="-fail";
        //Display_Interface::Instance_of().print_Log(buffer1);
        return 1;
    }
}

void Thread_SerialPort::handle_command_ack(mavlink_command_ack_t *ack)
{
364
365
    switch (ack->command) 
    {
366
        case MAV_CMD_COMPONENT_ARM_DISARM:
367
368
            if(testAck(ack, "Arm_desarm") == 1) //if command is succeed
            {
369
370
371
372
                //if(drone1.motors == ARM){
                //    drone1.motors = UNARM;
                //}else if(drone1.motors == UNARM){
                //    drone1.motors = ARM;
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
                //}
            }
            //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;
392
    }    
393
}