Data_Drone.cpp 17 KB
Newer Older
1
2
3
4
5
6
7
8
/**
 * @brief Class that model a drone
 * 
 * @author Sylvain Colomer
 * @date 23/04/19
 * @version 1.0
 */

9
10
11
12
13
14
15

#include <iostream>
#include <string>

using namespace std;


16
17
18
19
20
21
22
23
24
25
26
27
28
#include "../include/Data_Drone.h"


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CONTRUCTOR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

/**
 * Default constructor. Initialize var to first value
 */
Drone::Drone()
{

}

29
30
Drone::Drone(std::string serialPort_path, int serialPort_baudrate)
{
31
    communication=DRONE_SERIAL;
32
    cout<<"Data_Drone  "<< serialPort_path<<"\n";
33
34
35
    serial1 = std::shared_ptr<Serial_Port>(new Serial_Port(serialPort_path, serialPort_baudrate));
}

36
37
38
39
40
41
42
43
void Drone::open(char  *serialPort_path, int serialPort_baudrate)
{
    communication=DRONE_SERIAL;
    cout<<"Data_Drone  "<< serialPort_path<<"\n";
    serial1 = std::shared_ptr<Serial_Port>(new Serial_Port(serialPort_path, serialPort_baudrate));
}


44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127

Drone::~Drone()
{
    serial1.get()->close_serial();
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% COMMUNICATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

int Drone::read_message(mavlink_message_t &message)
{
    if(communication == DRONE_SERIAL)
    {
    return serial1.get()->read_message(message);
    }
    return 1;
}

int Drone::write_message(mavlink_message_t message)
{
    if(communication == DRONE_SERIAL)
    {
        return serial1.get()->write_message(message);
    }
    else if(communication == DRONE_WIFI)
    {
        return 1;
    }
    return 1;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INITIALISATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

int Drone::init_communication()
{
    if(communication==DRONE_SERIAL)
    {
        return Drone::serial1.get()->open_serial();
    }

    return 0;
}


/**
 * Initialisation of a drone 
 */
int Drone::init_parameters(uint limit)
{
    mavlink_message_t mavlink_message;
    bool flag_test = true;
    bool flag_success = true;
    uint counter=0;
    std::string buffer1="";

    while(flag_test)
    {
        
        flag_success = serial1.get()->read_message(mavlink_message);
        if(flag_success)
        {
            system_id = mavlink_message.sysid;
            component_id = mavlink_message.compid;
            
            flag_test=false;
        }
        
        if(counter >= limit) //Computation limit test
        {
            //Display_IHM::getInstanceOf().printLog("-> Error, no drone detected");
            return 1;
        }
        counter++;
        usleep(1000);
    }   

    return 0;
}


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CONTROL %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

/**
 * Arm with 1, un arm with 0
 */
128
129
int Drone::command_arm(int param1)
{
130
131
132
133
134
135
136
137
138
    //Message buffer
	mavlink_message_t message;
    mavlink_command_long_t command;
    command=mavlink_newCommand();
    command.target_system = 1;
    command.target_component = 1;
    command.command = MAV_CMD_COMPONENT_ARM_DISARM;
    command.confirmation = false;
    command.param1           = param1;
139
	command.param2           = 21196;
140
141
142

    mavlink_msg_command_long_encode(255, 1, &message, &command);

143
144
    if(param1 >0)
    {
145
        //Display_IHM::getInstanceOf().printLog("ARMING");
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
    }else
    {
        //Display_IHM::getInstanceOf().printLog("STOP ARMING");
    }

    return write_message(message);
}



int Drone::return_to_launch(int param1)
{
    //Message buffer
	mavlink_message_t message;
    mavlink_command_long_t command;
    command=mavlink_newCommand();
    command.target_system = 1;
    command.target_component = 1;
    command.command = MAV_CMD_NAV_RETURN_TO_LAUNCH;
    command.confirmation = false;
    command.param1           = param1;

    mavlink_msg_command_long_encode(255, 1, &message, &command);

    if(param1 >0)
    {
        //Display_IHM::getInstanceOf().printLog("ARMING");
    }else
    {
        //Display_IHM::getInstanceOf().printLog("STOP ARMING");
    }

    return write_message(message);
}

int Drone::take_off() 
{
    //Message buffer
    mavlink_message_t message;
    mavlink_command_long_t command;
	printf("===========================  take_off Demande au drone de monter en altitude \n");
    command = mavlink_newCommand();
    command.target_system = 1;
    command.target_component = 1;
     command.command = MAV_CMD_NAV_TAKEOFF;  
    command.confirmation = true ;
    command.param1 = 0;  //Pitch
	command.param2 = 0;
	command.param3 = 0;
	command.param4 = NAN; //YAW
	command.param5 = gps_raw_int.lat;  //global_position_int.lat; // local_position_ned.x;	//Latitude
	command.param6 = gps_raw_int.lon; //global_position_int.lon; //local_position_ned.y;    //longitude
	printf("departure alt = %d \n",departure_alt);
	command.param7 = departure_alt+1000; // 10m altitude

    mavlink_msg_command_long_encode(255, 1, &message, &command);

    return  write_message(message);
}


int Drone::landing() 
{
    //Message buffer
    mavlink_message_t message;
    mavlink_command_long_t command;
	printf("=============================  landing Demande au drone de faire un landing \n");
    command = mavlink_newCommand();
    command.target_system = 1;
    command.target_component = 1;
     command.command = MAV_CMD_NAV_LAND;  
    command.confirmation = true ;
    command.param1 = 1;
	command.param2 = 0;
	command.param3 = 0;
	command.param4 = NAN;
	command.param5 = gps_raw_int.lat;
	command.param6 = gps_raw_int.lat;
	command.param7 = departure_alt; 

    mavlink_msg_command_long_encode(255, 1, &message, &command);

    return  write_message(message);
}
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263

//============================================TEST=============================================================

int Drone::move_to(float param1, float param2, float param3) 
{
	int16_t xx,yy,zz;
	xx=(int16_t)param1; yy=(int16_t)param2; zz=(int16_t)param3;
    //Message buffer
    mavlink_message_t message;
    mavlink_command_long_t command;
	  // printf("Demande au drone de tourner vers la droite \n");
    command = mavlink_newCommand();
    command.target_system = 1;
    command.target_component = 1;
    command.command = MAV_FRAME_LOCAL_NED;
    command.confirmation = false;
    command.param1 = param1;

    mavlink_msg_command_long_encode(255, 1, &message, &command);

   if(param1 >0)
    {
        //Display_IHM::getInstanceOf().printLog("ARMING");
    }else
    {
        //Display_IHM::getInstanceOf().printLog("STOP ARMING");
    }

    return write_message(message);
}

//===============================================================================================================


264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
int Drone::command_right(float param1) 
{
    //Message buffer
    mavlink_message_t message;
    mavlink_command_long_t command;
	  // printf("Demande au drone de tourner vers la droite \n");
    command = mavlink_newCommand();
    command.target_system = 1;
    command.target_component = 1;
    command.command = MAV_FRAME_BODY_FRD;
    command.confirmation = false;
    command.param1 = param1;

    mavlink_msg_command_long_encode(255, 1, &message, &command);

   if(param1 >0)
    {
        //Display_IHM::getInstanceOf().printLog("ARMING");
    }else
    {
284
285
286
287
288
289
        //Display_IHM::getInstanceOf().printLog("STOP ARMING");
    }

    return write_message(message);
}

290
291
292
293
294



int Drone::command_kill(int param1)
{
295
296
297
298
299
300
301
302
303
304
305
306
    //Message buffer
	mavlink_message_t message;
    mavlink_command_long_t command;

    command=mavlink_newCommand();
    command.target_system = 1;
    command.target_component = 1;
    command.command = MAV_CMD_DO_FLIGHTTERMINATION;
    command.confirmation = false;
    command.param1           = param1;

    mavlink_msg_command_long_encode(255, 1, &message, &command);
307
308
    if(param1>0)
    {
309
310
311
312
313
314
315
316
        //Display_IHM::getInstanceOf().printLog("KILL MODE");
    }else{
        //Display_IHM::getInstanceOf().printLog("OUT KILL MODE");
    }

    return write_message(message);
}

317
318
int Drone::command_setModeGuided(float param1)
{
319
320
321
322
323
324
325
326
    //Message buffer
	mavlink_message_t message;
    mavlink_command_long_t command;

    command=mavlink_newCommand();
    command.target_system = 1;
    command.target_component = 1;
    command.command          = MAV_CMD_NAV_GUIDED_ENABLE;
327
    command.confirmation     = true; //false;
328
329
330
    command.param1           = param1; // flag >0.5 => start, <0.5 => stop

    mavlink_msg_command_long_encode(255, 1, &message, &command);
331
332
    if(param1>0)
    {
333
334
335
336
337
338
339
        //Display_IHM::getInstanceOf().printLog("GUIDED MODE");
    }else{
        //Display_IHM::getInstanceOf().printLog("OUT GUIDED MODE");
    }

    return write_message(message);
}
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
/*
 * set_landing()
{
	// Prepare command for landing
	mavlink_command_long_t com = { 0 };
	com.target_system    = system_id;
	com.target_component = autopilot_id;
	com.command          = MAV_CMD_NAV_LAND;
	com.confirmation     = true;
	com.param1           = (float) flag; // flag >0.5 => start, <0.5 => stop

	// Encode
	mavlink_message_t message;
	mavlink_msg_command_long_encode(system_id, companion_id, &message, &com);

	// Send the message
	int len = serial_port->write_message(message);

	// Done!
	return;
}*/
361

362
363
int Drone::command_directControl(float x, float y, float z, float r)
{
364
	mavlink_message_t message;
365
366
367
	int16_t xx,yy,zz,rr;
	xx=(int16_t)x; yy=(int16_t)y; zz=(int16_t)z; rr=(int16_t)r;
  //  mavlink_msg_manual_control_pack(255, 1, &message, 1, 0, xx, yy, zz, rr);
368
        mavlink_msg_manual_control_pack(255, 1, &message, 1, xx, yy, zz, rr,1);
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425

  cout<<"command_directControl  : "<<xx<<" "<<yy<<" "<<zz<<" "<<rr<<" "<<endl;
  return write_message(message);
// return 1;
}




//static uint8_t mavlink_msg_manual_control_get_target individual autopilot specifications for details.| Empty| Empty| Empty| Empty|  */
//MANUAL_CONTROL
//RC_CHANNELS_OVERRIDE


//   MAV_CMD_SET_GUIDED_SUBMODE_STANDARD=4000, /* This command sets the submode to standard guided when vehicle is in guided mode. The vehicle holds position and altitude and the user can input the desired velocites along all three axes.
//   MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
 //  MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Custom mode - this is system specific, please refer to the individual autopilot specifications for details.| Custom sub mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty|  */
//ds.command = 213; //MAV_CMD_DO_SET_POSITION_YAW_THRUST;
//   MAV_CMD_NAV_LAND=21, /* Land at location |Abort Alt| Empty| Empty| Desired yaw angle| Latitude| Longitude| Altitude|  */
//   MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty|  */

int Drone::command_left(float param1) 
{
    //Message buffer
    mavlink_message_t message;
    mavlink_command_long_t command;
	// printf("Demande au drone de tourner vers la gauche \n");
    command = mavlink_newCommand();
    command.target_system = (uint8_t) 1;
    command.target_component = (uint8_t) 1;
  //  command.command = MAV_CMD_DO_ORBIT;
    command.confirmation = (uint8_t) 0;
    command.param1 = (int16_t) param1;

    mavlink_msg_command_long_encode(255, 1, &message, &command);

    return  write_message(message);
}



int Drone::command_down(float param1) 
{
    //Message buffer
    mavlink_message_t message;
    mavlink_command_long_t command;
	// printf("Demande au drone de descendre \n");
    command = mavlink_newCommand();
    command.target_system = (uint8_t) 1;
    command.target_component = (uint8_t) 1;
    //command.command = MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT;
    command.confirmation = (uint8_t) 0;
    command.param1 = (int16_t) param1;

    mavlink_msg_command_long_encode(255, 1, &message, &command);

    return  write_message(message);
426
427
}

428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
int Drone::command_pow(float param1) 
{
    //Message buffer
    mavlink_message_t message;
    mavlink_command_long_t command;
	// printf("Demande augmentation de la puissance \n");
    command = mavlink_newCommand();
    command.target_system = (uint8_t) 1;
    command.target_component = (uint8_t) 1;
    //command.command = MAV_CMD_COMPONENT_ARM_DISARM;
    command.confirmation = (uint8_t) 0;
    command.param1 = (int16_t) param1;

    mavlink_msg_command_long_encode(255, 1, &message, &command);
	
    return  write_message(message);
}

//void MockLink::_sendRCChannels(void) 
//{
   // mavlink_message_t   msg;

    // mavlink_msg_rc_channels_pack_chan(_vehicleSystemId,
       //                               _vehicleComponentId,
       //                               _mavlinkChannel,
       //                               &msg,
       //                               0,                     // time_boot_ms
       //                               16,                    // chancount
       //                               1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500,   // channel 1-8
       //                               1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500,   // channel 9-16
       //                               UINT16_MAX, UINT16_MAX,
       //                               0);                    // rssi

    //respondWithMavlinkMessage(msg);
// }




467
468
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ? %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

469
470
mavlink_command_long_t Drone::mavlink_newCommand()
{
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
    mavlink_command_long_t com;
    com.target_system = 0;
    com.target_component = 0.;
    com.command = 0;
    com.confirmation = false;
    com.param1=0; 
    com.param2=0; 
    com.param3=0; 
    com.param4=0; 
    com.param5=0; 
    com.param6=0; 
    com.param7=0;
    return com;
}

486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
/*
PARAM_REQUEST_LIST 
Request all parameters of this component. After this request, all parameters are emitted. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html

mavlink_param_union_t param;
int32_t integer = 20000;
param.param_int32 = integer;
param.type = MAV_PARAM_TYPE_INT32;

// Then send the param by providing the float bytes to the send function
mavlink_msg_param_set_send(xxx, xxx, param.param_float, param.type, xxx);

??????   mavlink_msg_param_set_send ( 255, 1,  20000, MAV_PARAM_TYPE_INT32, ????)

static inline void mavlink_msg_param_set_send( mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type)
*/
/**
 * @brief Send a param_set message
 * @param chan MAVLink channel to send the message   : 255
 *
 * @param target_system  System ID  : 1 
 * @param target_component  Component ID
 * @param param_id  Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
 * @param param_value  Onboard parameter value
 * @param param_type  Onboard parameter type.
 */


/*
 You need to assign base_mode, main_mode, and sub_mode.
For instance, in order to trigger ‘position’ flight mode, you can use the following pymavlink code. I think that it will be same in dronekit code.

master.mav.command_long_send ( master.target_system, master.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, base_mode, main_mode, sub_mode, 0, 0, 0, 0)

sub_mode is only defined for the AUTO_MOD

master.mav.command_long_send( master.target_system, master.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 217, 3, 0, 0, 0, 0, 0)
 
    Manual: base_mode:217, main_mode:1, sub_mode:0
    Stabilized: base_mode:209, main_mode:7, sub_mode:0
    Acro: base_mode:209, main_mode:5, sub_mode:0
    Rattitude: base_mode:193, main_mode:8, sub_mode:0
    Altitude: base_mode:193, main_mode:2, sub_mode:0
    Offboard: base_mode:209, main_mode:6, sub_mode:0
    Position: base_mode:209, main_mode:3, sub_mode:0
    Hold: base_mode:217, main_mode:4, sub_mode:3
    Missition: base_mode:157, main_mode:4, sub_mode:4
    Return: base_mode:157, main_mode:4, sub_mode:5
    Follow me: base_mode:157, main_mode:4, sub_mode:8
*/

537
538
539
540
int Drone::command_setMode(Drone_mode mode)
{
    switch(mode)
    {
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
        case DRONE_OFF: 
        //We must reset all value to 0
            Drone::mode=mode;
        break;

        case DRONE_MANUAL_DIRECT: 
            if(remote_x!=0 && remote_y!=0 && remote_z!=0 && remote_r!=0){
                ////Display_IHM::getInstanceOf().printLog("Server: command refused");
                //beep();
            }else{
                //Display_IHM::getInstanceOf().printLog("Server: mode manual_direct");
                Drone::mode=mode;
            }
        break;

        default:
        break;
    }

    return 0;
561
}
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583

/*
toggle_offboard_control( bool flag )
{
	// Prepare command for off-board mode
	mavlink_command_long_t com;
	com.target_system    = system_id;
	com.target_component = autopilot_id;
	com.command          = MAV_CMD_NAV_GUIDED_ENABLE;
	com.confirmation     = true;
	com.param1           = (float) flag; // flag >0.5 => start, <0.5 => stop

	// Encode
	mavlink_message_t message;
	mavlink_msg_command_long_encode(system_id, companion_id, &message, &com);

	// Send the message
	int len = serial_port->write_message(message);

	// Done!
	return len;
}*/