TestDroneRemoteControl.cpp 7.39 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
/**
 *  @author Sylvain Colomer
 *  @date 18/04/19.
 * 
 * https://www.tldp.org/HOWTO/html_single/Text-Terminal-HOWTO/#colors
 * https://www.tldp.org/HOWTO/html_single/Text-Terminal-HOWTO/#colors
 * https://en.wikipedia.org/wiki/ANSI_escape_code
 * https://fr.wikipedia.org/wiki/Curses
 * http://www.termsys.demon.co.uk/vtansi.htm
 */

12
#include "include/PixhawkServer.h"
13
14
#include <iostream>
#include <string>
15
16
#include <iostream>
#include <fstream>	
17
18
19

using namespace std;

20
char device_path[255] = "/dev/ttyUSB0";
21
int device_baudrate = 57600;
22
23
24
25
26
27
28
//Drone drone1(device_path, device_baudrate);
Drone drone1;


    // General object
    shared_ptr<Serial_Port_ReadingThread> readingThread;
    shared_ptr<Serial_Port_WritingThread> writingThread;
29

30
31
ofstream MyFile;
ofstream MyFilew;
32

33
34
35
36
blc_channel blc_control_arm("/pixhawk.control.arm", BLC_CHANNEL_READ, 'IN16', 'NDEF', 1, 1); // 1=on, 0=off;
blc_channel blc_control_remote_vector("/pixhawk.control.remoteVectors", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 4);
    //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
37

38
39
    //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
40
blc_channel blc_highres_imu("/pixhawk.sensors.imu", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 9);
41
42
43
44
45
	//Channel use to know altitude of drone
blc_channel blc_attitude("/pixhawk.sensors.attitude", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 7);
    //Channel use to know GPS values of drone
blc_channel blc_local_position_ned("/pixhawk.sensors.local_position_ned", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 6);
blc_channel blc_global_position("/pixhawk.sensors.global_position", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 3);
46

47
48
    //Different blc channel use to publish data
blc_channel blc_heartbeat; //Hearbeat message  PG: PB ????? pas d'init  pour l'instant ????
49
blc_channel blc_test_detection("/pixhawk.test.detection", BLC_CHANNEL_READ, 'IN16', 'NDEF', 1, 7);
50
51
52

void command_loop()
{
53
	int ret;
54
55
56
    mavlink_message_t message;
    string buffer_command = "";

57
	writingThread.get()->set_task_period(100000);
58
    cout<<"Enter command : \n";
59
60
    cin >> buffer_command;

61
    if(buffer_command=="a")
62
63
    {
       cout<<"arm \n";
64
        drone1.command_arm(1);
65
    }
66
    else if(buffer_command=="!a")
67
68
    {
        
69
        drone1.command_arm(0);
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
    else if(buffer_command=="f")
    {
       cout<<"fly \n";
      ret = drone1.take_off();
      cout<<"fly ret = "<<ret<<endl;
    }
    else if(buffer_command=="!f")
    {
        //drone1.take(0);
    }
    else  if(buffer_command=="i")
    {
       cout<<"return \n";
        drone1.return_to_launch(1);
    }
    else if(buffer_command=="!i")
    {
        
        drone1.return_to_launch(0);
    }
    else  if(buffer_command=="l")
    {
       cout<<"landing \n";
        drone1.landing();
    }
 
     /*if(buffer_command=="u")
    {
       cout<<"takeoff \n";
        drone1.command_up(1);
    }
    else if(buffer_command=="!u")
    {
        
        drone1.command_up(0);
    }
    */
    
   else if(buffer_command=="r")
    {
       cout<<"right \n";
        drone1.command_right(1);
    }
    else if(buffer_command=="!r")
    {
        
        drone1.command_right(0);
    }
    
    else if(buffer_command=="k")
121
    {
122
123
       cout<<"kill \n";
        drone1.command_kill(1);
124
    }
125
    else if(buffer_command=="!k")
126
    {
127
        drone1.command_kill(0);
128
    }
129
    else if(buffer_command=="z")
130
    {
131
        cout<<"motor 1 \n";
132
        drone1.command_directControl(1000,0,1000,0);
133
    }
134
    else if(buffer_command=="d")
135
    {
136
        drone1.command_directControl(0,1000,1000,0);
137
    }
138
    else if(buffer_command=="q")
139
    {
140
        drone1.command_directControl(-1000,0,1000,0);
141
    }
142
    else if(buffer_command=="s")
143
    {
144
        drone1.command_directControl(0,0,0,1000);
145
146
147
    }
    else if(buffer_command=="!m")
    {
148
        drone1.command_directControl(0,0,0,0);
149
150
151
    }
    else if(buffer_command=="tone")
    {
152
       cout<<"Tone \n";
153
        //Display_IHM::getInstanceOf().printLog("PLAY TONE");
154
155
        mavlink_msg_play_tune_pack(255, drone1.component_id, &message, 1, 1 , "AAAA", "");
        drone1.write_message(message);
156
157
158
    }
    else
    {
159
       cout<<"command not recognized\n";
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
        //Display_IHM::getInstanceOf().printLog("Error: what ?");
    }

    //PARAM_REQUEST_READ
}




// else if(buffer_command=="1") // SET_ATTITUDE_TARGET
// {
//     Display_IHM::getInstanceOf().printLog("U");
//     mavlink_setAttitudeTarget(&message, *drone1.get(), 800, 0, 0, 0);
//     drone1.get()->write_message(message);
// }
// else if(buffer_command=="2") // SET_ATTITUDE_TARGET
// {
//     Display_IHM::getInstanceOf().printLog("");
//     mavlink_setAttitudeTarget(&message, *drone1.get(), 0, 800, 0, 0);
//     drone1.get()->write_message(message);
// }
// else if(buffer_command=="3") // SET_ATTITUDE_TARGET
// {
//     Display_IHM::getInstanceOf().printLog("");
//     mavlink_setAttitudeTarget(&message, *drone1.get(), 0, 0, 800, 0);
//     drone1.get()->write_message(message);
// }
// else if(buffer_command=="4") // SET_ATTITUDE_TARGET
// {
//     Display_IHM::getInstanceOf().printLog("");
//     mavlink_setAttitudeTarget(&message, *drone1.get(), 0, 0, 0, 800);
//     drone1.get()->write_message(message);
// }
193
194
195
196
197
198
199
200
201
202



/**
 * Only the main exe of the programm
 * @return
 */
int main(int argc, char *argv[])
{
    // General var
203
204
205
206
207
  //  bool flag_run = true; //Flag use for the main loop of the system

   if(argc>1)  {strcpy(device_path ,argv[1]); printf("port = %s \n",device_path);}
   
   drone1.open(device_path, device_baudrate);
208
209

    string buffer1="";
210

211
212
213
214
//    shared_ptr<Display_IHM> ihm;

   // drone1 = shared_ptr<Drone>(new Drone(device_path, device_baudrate));
  //  drone1 = new Drone(device_path, device_baudrate);
215
216
217
218
219
	
				     
//		ofstream MyFile("mesures.txt");
		MyFile.open("mesures.txt");    
		MyFilew.open("tcdrain.txt");    
220

221
222
223
224
   writingThread = shared_ptr<Serial_Port_WritingThread>(new Serial_Port_WritingThread(1000000, 200));  //beug ne marche pas 
																						
   readingThread = shared_ptr<Serial_Port_ReadingThread>(new Serial_Port_ReadingThread(10000, 200)); //beug ne marche pas 
																						
225

226
    cout<<"Welcome to pixhawk server"<<endl;
227
    cout<<"Init communicationn"<<endl;
228
    cout<<"-> Open  " <<device_path<<endl;
229
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
    if(drone1.init_communication()==0)
    { 
        cout<<"-> Succes"<<endl;
    }
    else
    {
        cout<<"-> Fail, exiting now"<<endl;
        sleep(2);
        exit(1);
    }
    
    cout<<"Pull parameters"<<endl;
    if(drone1.init_parameters(10000)==0)
    { 
        cout<<"-> Succes"<<endl;
    }
    else
    {
        cout<<"-> Fail, exiting"<<endl;
        sleep(2); 
        exit(1);
    }

    // Display
    //Display_IHM::getInstanceOf().displayDroneState(drone1);
    //Display_IHM::getInstanceOf().displayMotorsState(drone1);

256
257
258
    //Display_IHM::getInstanceOf().printLog("Begin");   
    
   readingThread.get()->start();
259
260
261
    writingThread.get()->start();
    sleep(1);
    
262
263
264
    //cout<<"Arm"<<endl;
    //drone1.command_arm(1);
    //sleep(1);
265

266
267
268
    //cout<<"test motors"<<endl;
    //drone1.command_directControl(0,1000,0,0);
    //sleep(1);
269
270
271
272
273
274
275

//    cout<<"Unarm"<<endl;    
//    drone1.get()->command_arm(0);
//    sleep(1);

//    readingThread.get()->stop();
//    writingThread.get()->stop();
276

277
278
279
280
   while(true)
   {
      command_loop();
   }
281
282
		MyFile.close();
		MyFilew.close();
283
284
    exit(0);
}