Browse Source

'tools/fctrl.py' ändern

Added some error handling
klaute 1 year ago
parent
commit
0d721ddf28
1 changed files with 35 additions and 31 deletions
  1. 35
    31
      tools/fctrl.py

+ 35
- 31
tools/fctrl.py View File

@@ -12,6 +12,12 @@ import threading
12 12
 
13 13
 ################################################################################
14 14
 
15
+MAIN_LOOP_DELAY = 20 # time in ms
16
+
17
+SEND_THREAD_UPDATE_DELAY = 50 # time in ms
18
+
19
+################################################################################
20
+
15 21
 parser = argparse.ArgumentParser()
16 22
 parser.add_argument("-ip",    dest="ip",    help="ip of the drone",     action="store",      default='192.168.0.1')
17 23
 parser.add_argument("-port",  dest="port",  help="port of the drone",   action="store",      default=8192)
@@ -75,6 +81,7 @@ tdir = -50
75 81
 
76 82
 ################################################################################
77 83
 
84
+exit_thread = False
78 85
 ser_conn = None
79 86
 
80 87
 ################################################################################
@@ -202,7 +209,7 @@ def sendMessageThread():
202 209
 
203 210
     send_msg_time_cnt = 0
204 211
 
205
-    while True:
212
+    while True and exit_thread == False:
206 213
         # send the message to the drone
207 214
         send_msg_time_cnt += 1
208 215
         if send_msg_time_cnt == 150:
@@ -217,37 +224,17 @@ def sendMessageThread():
217 224
 
218 225
             if NO_NET == False and ser_conn == None:
219 226
                 s.send(msg)
227
+
220 228
             elif ser_conn != None:
221 229
                 print(':'.join(hex(ord(x))[2:] for x in msg))
222
-                #ser_conn.write(b'$')
223
-                #ser_conn.write(b'M')
224
-                #ser_conn.write(b'<')
225
-                #ser_conn.write(bytes([0x10]))
226
-                #ser_conn.write(bytes([0xc8]))
227
-                #ser_conn.write(bytes([int(roll) & 0xff]))
228
-                #ser_conn.write(bytes([(int(roll) >> 8) & 0xff]))
229
-                #ser_conn.write(bytes([int(pitch) & 0xff]))
230
-                #ser_conn.write(bytes([(int(pitch) >> 8) & 0xff]))
231
-                #ser_conn.write(bytes([int(yaw) & 0xff]))
232
-                #ser_conn.write(bytes([(int(yaw) >> 8) & 0xff]))
233
-                #ser_conn.write(bytes([int(throttle) & 0xff]))
234
-                #ser_conn.write(bytes([(int(throttle) >> 8) & 0xff]))
235
-                #ser_conn.write(bytes([int(aux1) & 0xff]))
236
-                #ser_conn.write(bytes([(int(aux1) >> 8) & 0xff]))
237
-                #ser_conn.write(bytes([int(aux2) & 0xff]))
238
-                #ser_conn.write(bytes([(int(aux2) >> 8) & 0xff]))
239
-                #ser_conn.write(bytes([int(aux3) & 0xff]))
240
-                #ser_conn.write(bytes([(int(aux3) >> 8) & 0xff]))
241
-                #ser_conn.write(bytes([int(aux4) & 0xff]))
242
-                #ser_conn.write(bytes([(int(aux4) >> 8) & 0xff]))
243
-                #ser_conn.write(bytes([genCRC(msg)]))
230
+ 
244 231
                 for c in msg:
245 232
                     ser_conn.write(bytes([ord(c)]))
246 233
 
247 234
             send_msg_time_cnt = 0
248 235
 
249 236
         #print("-")
250
-        time.sleep(0.001) # main loop frequency 1/1ms
237
+        time.sleep(SEND_THREAD_UPDATE_DELAY / 1000) # main loop frequency = 1/SEND_THREAD_UPDATE_DELAY/1000
251 238
 
252 239
 ################################################################################
253 240
 
@@ -255,17 +242,26 @@ if __name__ == '__main__':
255 242
 
256 243
     # conenct to the drone
257 244
     if NO_NET == False and SERIAL_DEVICE == "":
258
-        print("Connect to ", DRONE_IP, ":", DRONE_PORT)
259
-        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
260
-        s.connect((DRONE_IP, DRONE_PORT))
245
+      	try:
246
+        	print("Connect to ", DRONE_IP, ":", DRONE_PORT)
247
+        	s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
248
+        	s.connect((DRONE_IP, DRONE_PORT))
249
+        except:
250
+            print("Can't open network socket on " + DRONE_IP + ":" + DTONE_PORT)
251
+            exit(-1)
261 252
     else:
262 253
         print("Network connection is not used")
263 254
 
264 255
     if SERIAL_DEVICE != "":
265
-        ser_conn = serial.Serial(SERIAL_DEVICE, 115200, timeout=0)
266
-        if ser_conn != None:
267
-            print("Connected to "+ SERIAL_DEVICE)
268
-
256
+      	try:
257
+        	ser_conn = serial.Serial(SERIAL_DEVICE, 115200, timeout=0)
258
+        	if ser_conn != None:
259
+	            print("Connected to "+ SERIAL_DEVICE)
260
+      	except:
261
+            print("Can't open serial port " + SERIAL_DEVICE)
262
+            exit(-2)
263
+
264
+    exit_thread = False
269 265
     th = threading.Thread(target = sendMessageThread)
270 266
     th.start()
271 267
 
@@ -274,7 +270,11 @@ if __name__ == '__main__':
274 270
 
275 271
         tmp = []
276 272
         if TEST == False:
273
+          try:
277 274
             tmp = readGamepad()
275
+          except:
276
+            print("Cant read data from gamepad.")
277
+            exit(-3)
278 278
         else:
279 279
             tmp = genGamepadTestData()
280 280
 
@@ -300,5 +300,9 @@ if __name__ == '__main__':
300 300
         #if DEBUG == True:
301 301
             #print("> ", [ roll, pitch, yaw, throttle, aux1, aux2, aux3, aux4 ])
302 302
 
303
+        time.sleep(MAIN_LOOP_DELAY/1000) // f = 1/MAIN_LOOP_DELAY/1000
304
+
305
+	exit_thread = True
306
+
303 307
 ################################################################################
304 308
 

Loading…
Cancel
Save