Data_Drone.h 6.41 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
/**
 * @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"
27
//#include "common/mavlink_msg_compassmot_status.h"
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

/**
 * 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,
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
	DRONE_MANUAL_DIRECT
	
	// Mode manual with direct control of the motors
};


 enum class FlightMode {
        Unknown,
        Ready,
        Takeoff,
        Hold,
        Mission,
        ReturnToLaunch,
        Land,
        Offboard,
        FollowMe,
        Manual,
        Altctl,
        Posctl,
        Acro,
        Rattitude,
        Stabilized,
    };


namespace px4 {

enum PX4_CUSTOM_MAIN_MODE {
    PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
    PX4_CUSTOM_MAIN_MODE_ALTCTL,
    PX4_CUSTOM_MAIN_MODE_POSCTL,
    PX4_CUSTOM_MAIN_MODE_AUTO,
    PX4_CUSTOM_MAIN_MODE_ACRO,
    PX4_CUSTOM_MAIN_MODE_OFFBOARD,
    PX4_CUSTOM_MAIN_MODE_STABILIZED,
    PX4_CUSTOM_MAIN_MODE_RATTITUDE
};

enum PX4_CUSTOM_SUB_MODE_AUTO {
    PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
    PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
    PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
    PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
    PX4_CUSTOM_SUB_MODE_AUTO_RTL,
    PX4_CUSTOM_SUB_MODE_AUTO_LAND,
    PX4_CUSTOM_SUB_MODE_AUTO_RTGS,
    PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET
98
99
};

100
101
102
103
104
105
106
107
108
109
110
111
112
union px4_custom_mode {
    struct {
        uint16_t reserved;
        uint8_t main_mode;
        uint8_t sub_mode;
    };
    uint32_t data;
    float data_float;
};

} // namespace px4


113
114
115
116
117
118
119
120
121
122
123
/**
 * Drone classes
 */
class Drone
{

public:

	Drone_Communication drone_communication;

	Drone_Motors motors=UNARM;	
124
//	Drone_mode mode=DRONE_OFF;
125
126
127
128
129
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

	
	//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
164
	mavlink_home_position_t home_position;
165
166
167
168
169
170
171
172

	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;
173
    mavlink_gps_raw_int_t gps_raw_int;
174
     mavlink_command_long_t command_long;
175
176
    
    int departure_alt;
177
178
179
180
181
182
183
184
185
186
187
188
189
	
public:

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

	Drone();

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

	~Drone();
190
191
	
	void open(char *serialPort_path, int serialPort_baudrate);
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

	//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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
	 */
229
230
	int command_setMode(Drone_mode my_mode);
	
231
232

	int command_arm(int param1);
233
234
235
	
	//int command_float(int param1);

236
237
238

	int command_kill(int param1);

239
240
//	int command_setModeGuided(float param1);
	int make_command_msg_rate(int param1);
241
	int command_directControl(float x, float y, float z, float r);
242
	int move_drone_to(float x, float y, float z);
243
244
245
246
247
248
	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) ;
249
	int go_to();
250
251
252
253
	int waypoint(float param5, float param6, float param7);
	int battery_check(float x);


254
255
256
257
258
259
	int waypointforback(float param6);
	int waypointupdown(float param7);
	int take_off_vtol();

	int followhold();
	int make_mode(int param1);
260
	int get_home_x(float x);
261

262
263
	int request_home_position(float x, float y, float z);
	void send_setpoint_velocity(float vx, float vy, float vz);
264
265
266

	int move_to(float param1, float param2, float param3);

267
268
269
	int command_up(float param1) ;
	int command_down(float param1) ;
	int command_pow(float param1) ;
270
271
272
273
274
275
276
277
278
279
280
281

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

	mavlink_command_long_t mavlink_newCommand();

};



#endif // SERIAL_PORT_H_