Data_Drone.h 4.64 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
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
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
/**
 * @brief Tool class which convey to open a serial port
 * @file serial_port.cpp
 * 
 * @author Sylvain ColomerJoystickModeAttitude
 * @date 19/04/19
 * @version 1.1 
 */

#ifndef DRONE_H
#define DRONE_H


#include <common/mavlink.h>

#include <iostream>
#include <string>
#include <iostream>
#include <unistd.h>
#include <termios.h>
#include <thread>
#include <mutex>
#include <sys/time.h>

#include "Com_Serial.h"
#include "Com_Wifi.h"

/**
 * Define about drone communication protocol. it's use to automatic open specific communication
 */
enum Drone_Communication {
	DRONE_WIFI,
	DRONE_SERIAL,
	DRONE_DUAL
};

/**
 * define about drone mode
 */
enum Drone_Motors{
	ARM,
	UNARM
};

/**
 * define about drone mode
 */
enum Drone_mode{
	DRONE_OFF,
	DRONE_MANUAL_DIRECT // Mode manual with direct control of the motors
};

/**
 * Drone classes
 */
class Drone
{

public:

	Drone_Communication drone_communication;

	Drone_Motors motors=UNARM;	
	Drone_mode mode=DRONE_OFF;

	
	//Communication part
	Drone_Communication communication=DRONE_SERIAL;
	std::shared_ptr<Serial_Port> serial1;
	std::shared_ptr<Wifi_Port> wifi1;

	
	//Drone system var
	uint8_t system_id=-1;
	uint8_t component_id=-1;
	uint8_t autopilot_id=-1;

	// Var use in remote mode to control the drone. 
	// Warning: if the value are highter than 0 before, it will be dangerous because the drone will make an hight jump
	int remote_x=0;
	int remote_y=0;
	int remote_z=0;
	int remote_r=0;

	// Timestamp use for relative navigation
	uint64_t timestamps;

	//Drone buffers corresponding to input var. Very usefull for GPS navigation 
	mavlink_heartbeat_t heartbeat; //Heartbeat. gather Id and others
	mavlink_command_ack_t ack; //Ack command: one of the most usefull parameters. it convey to read return of command
	mavlink_sys_status_t sys_status;
    mavlink_ping_t ping;
    mavlink_param_value_t param_value;
    mavlink_servo_output_raw_t servo_output_raw;
	mavlink_battery_status_t battery_status;
    mavlink_autopilot_version_t autopilot_version;
    mavlink_estimator_status_t estimator_status;
    mavlink_vibration_t vibration;
    mavlink_extended_sys_state_t extended_sys_state;
	mavlink_radio_status_t radio_status;

    mavlink_highres_imu_t highres_imu; //Accelerometers of the system
	mavlink_altitude_t altitude; //Altitude data

	mavlink_attitude_t attitude; //Attitude of the drone use to manual control and target attitude
    mavlink_attitude_target_t attitude_target; //Attitude of the drone use to manual control and target attitude

	mavlink_global_position_int_t global_position_int;

    mavlink_local_position_ned_t local_position_ned;
    mavlink_position_target_local_ned_t position_target_local_ned;
112
113
114
    mavlink_gps_raw_int_t gps_raw_int;
    
    int departure_alt;
115
116
117
118
119
120
121
122
123
124
125
126
127
	
public:

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

	Drone();

	/**
	 * Default constructor use for serial port system
	 */
	Drone(std::string serialPort_path, int serialPort_baudrate);

	~Drone();
128
129
	
	void open(char *serialPort_path, int serialPort_baudrate);
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169

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

		/**
	 * Function which convey to get message from selected connection
	 * By defalt, we use serial port communication (radio or usb)
	 */
	int read_message(mavlink_message_t &message); //int communicationWay

	/**
	 * Method that allow to write message with a specific communication protocol
	 */
	int write_message(mavlink_message_t message);

	/**
	 * 
	 */
	int write_message(mavlink_message_t message, Drone_Communication protocol);

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

	/**
	 * Init communication system
	 */
	int init_communication();

	/**
	 * Function use to automatic initialize the drone with time limit
	 * Need to have before intialize the communication
	 */
	int init_parameters(uint limit);

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

	/**
	 * Convey to change the mode of the drone
	 */
	int command_setMode(Drone_mode mode);

	int command_arm(int param1);
170
171
172
	
	//int command_float(int param1);

173
174
175
176
177
178

	int command_kill(int param1);

	int command_setModeGuided(float param1);
	
	int command_directControl(float x, float y, float z, float r);
179
180
181
182
183
184
185
186
187
188
	int return_to_launch(int param1);
	int take(int param1) ;
	int take_off();
	int landing();
	int command_right(float param1) ;
	int command_left(float param1) ;

	int command_up(float param1) ;
	int command_down(float param1) ;
	int command_pow(float param1) ;
189
190
191
192
193
194
195
196
197
198
199
200

	// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ? %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

	mavlink_command_long_t mavlink_newCommand();

};



#endif // SERIAL_PORT_H_