No Description
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

fctrl.py 8.3KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308
  1. import time
  2. import socket
  3. import argparse
  4. import serial
  5. #from serial import serial
  6. import inputs
  7. import threading
  8. ################################################################################
  9. MAIN_LOOP_DELAY = 20 # time in ms
  10. SEND_THREAD_UPDATE_DELAY = 50 # time in ms
  11. ################################################################################
  12. parser = argparse.ArgumentParser()
  13. parser.add_argument("-ip", dest="ip", help="ip of the drone", action="store", default='192.168.0.1')
  14. parser.add_argument("-port", dest="port", help="port of the drone", action="store", default=8192)
  15. parser.add_argument("-ser", dest="ser", help="serial port of esp", action="store", default="")
  16. parser.add_argument("-nonet", dest="nonet", help="disable network use", action="store_true", default=False)
  17. parser.add_argument("-debug", dest="debug", help="show debug messages", action="store_true", default=False)
  18. parser.add_argument("-test", dest="test", help="generate test data", action="store_true", default=False)
  19. args = parser.parse_args()
  20. ################################################################################
  21. DEBUG = args.debug or False
  22. NO_NET = args.nonet or False
  23. TEST = args.test or False
  24. SERIAL_DEVICE = args.ser or ""
  25. if SERIAL_DEVICE != "":
  26. NO_NET = True
  27. DRONE_IP = args.ip or "192.168.0.1"
  28. DRONE_PORT = args.port or 8192
  29. KEY_ID_AUX1 = 20
  30. KEY_ID_AUX2 = 40
  31. KEY_ID_AUX3 = 60
  32. KEY_ID_AUX4 = 80
  33. AXIS_ID_PITCH = 100
  34. AXIS_ID_ROLL = 120
  35. AXIS_ID_YAW = 140
  36. AXIS_ID_THROTTLE = 160
  37. AXIS_MIN_VAL = -32768
  38. AXIS_MAX_VAL = 32767
  39. ################################################################################
  40. MSP_SET_RAW_RC = 200
  41. ################################################################################
  42. roll = 1500
  43. pitch = 1500
  44. yaw = 1500
  45. throttle = 1000
  46. aux1 = 1000
  47. aux2 = 1000
  48. aux3 = 1000
  49. aux4 = 1000
  50. troll = 0
  51. tpitch = 0
  52. tyaw = 0
  53. tthrottle = 0
  54. taux1 = 0
  55. taux2 = 0
  56. taux3 = 0
  57. taux4 = 0
  58. tdir = -50
  59. ################################################################################
  60. exit_thread = False
  61. ser_conn = None
  62. ################################################################################
  63. def toCharStr(number):
  64. hb = (number >> 8) & 0xff;
  65. lb = number & 0xff
  66. #if DEBUG == True:
  67. #print(number, " >", str(hb), " ", str(lb))
  68. return chr(lb) + chr(hb)
  69. ################################################################################
  70. def genCRC(msg):
  71. crc = 0
  72. i = 0
  73. for c in list(msg):
  74. if i == 3:
  75. crc = ord(c)
  76. elif i > 3:
  77. #if DEBUG == True:
  78. #print(chr(crc ^ ord(c)), " = ", chr(crc), " XOR ", c)
  79. crc = crc ^ ord(c)
  80. i = i + 1
  81. #if DEBUG == True:
  82. #print("CRC: ", list(chr(crc)))
  83. return crc
  84. ################################################################################
  85. def genMessage(roll, pitch, yaw, throttle, aux1, aux2, aux3, aux4):
  86. return genMWiiSerRCMsg(roll, pitch, yaw, throttle, aux1, aux2, aux3, aux4)
  87. def genMWiiSerRCMsg(roll, pitch, yaw, throttle, aux1, aux2, aux3, aux4):
  88. msg = "$M<" + chr(16) + \
  89. chr(MSP_SET_RAW_RC) + \
  90. toCharStr(int(roll)) + \
  91. toCharStr(int(pitch)) + \
  92. toCharStr(int(yaw)) + \
  93. toCharStr(int(throttle)) + \
  94. toCharStr(int(aux1)) + \
  95. toCharStr(int(aux2)) + \
  96. toCharStr(int(aux2)) + \
  97. toCharStr(int(aux3))
  98. return msg + chr(genCRC(msg))
  99. ################################################################################
  100. def readGamepad():
  101. troll = -60000
  102. tpitch = -60000
  103. tyaw = -60000
  104. tthrottle = -60000
  105. taux1 = -60000
  106. taux2 = -60000
  107. taux3 = -60000
  108. taux4 = -60000
  109. events = inputs.get_gamepad()
  110. for event in events:
  111. #print(event.ev_type, event.code, event.state)
  112. if event.code == 'ABS_X':
  113. tyaw = event.state
  114. elif event.code == 'ABS_Y':
  115. tthrottle = event.state
  116. elif event.code == 'ABS_RY':
  117. tpitch = event.state
  118. elif event.code == 'ABS_RX':
  119. troll = event.state
  120. elif event.code == 'BTN_SOUTH' and event.state == 1:
  121. taux1 = event.state
  122. # TODO read the axis and buttons state
  123. # TODO keep the axis values in range of 1000 : 2000
  124. #if DEBUG == True:
  125. #print("<< ", [ troll, tpitch, tyaw, tthrottle, taux1, taux2, taux3, taux4 ])
  126. return [ troll, tpitch, tyaw, tthrottle, taux1, taux2, taux3, taux4 ]
  127. ################################################################################
  128. def genGamepadTestData():
  129. global troll, tpitch, tyaw, tthrottle, tdir
  130. troll = troll + tdir
  131. tpitch = tpitch + tdir
  132. tyaw = tyaw + tdir
  133. tthrottle = tthrottle + tdir
  134. if tthrottle > AXIS_MAX_VAL:
  135. tdir = -50
  136. elif tthrottle < AXIS_MIN_VAL:
  137. tdir = 50
  138. taux1 = 0
  139. taux2 = 0
  140. taux3 = 0
  141. taux4 = 0
  142. #if DEBUG == True:
  143. #print("tmp<< ", [ troll, tpitch, tyaw, tthrottle, taux1, taux2, taux3, taux4 ])
  144. return [ troll, tpitch, tyaw, tthrottle, taux1, taux2, taux3, taux4 ]
  145. ################################################################################
  146. def sendMessageThread():
  147. send_msg_time_cnt = 0
  148. while True and exit_thread == False:
  149. # send the message to the drone
  150. send_msg_time_cnt += 1
  151. if send_msg_time_cnt == 150:
  152. # generate the control message
  153. msg = genMessage(roll, pitch, yaw, throttle, aux1, aux2, aux3, aux4)
  154. if DEBUG == True:
  155. print(">> ", [ int(roll), int(pitch), int(yaw), int(throttle), int(aux1), int(aux2), int(aux3), int(aux4) ])
  156. #if DEBUG == True:
  157. #print(list(msg))
  158. if NO_NET == False and ser_conn == None:
  159. s.send(msg)
  160. elif ser_conn != None:
  161. print(':'.join(hex(ord(x))[2:] for x in msg))
  162. for c in msg:
  163. ser_conn.write(bytes([ord(c)]))
  164. send_msg_time_cnt = 0
  165. #print("-")
  166. time.sleep(SEND_THREAD_UPDATE_DELAY / 1000) # main loop frequency = 1/SEND_THREAD_UPDATE_DELAY/1000
  167. ################################################################################
  168. if __name__ == '__main__':
  169. # conenct to the drone
  170. if NO_NET == False and SERIAL_DEVICE == "":
  171. try:
  172. print("Connect to ", DRONE_IP, ":", DRONE_PORT)
  173. s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
  174. s.connect((DRONE_IP, DRONE_PORT))
  175. except:
  176. print("Can't open network socket on " + DRONE_IP + ":" + DTONE_PORT)
  177. exit(-1)
  178. else:
  179. print("Network connection is not used")
  180. if SERIAL_DEVICE != "":
  181. try:
  182. ser_conn = serial.Serial(SERIAL_DEVICE, 115200, timeout=0)
  183. if ser_conn != None:
  184. print("Connected to "+ SERIAL_DEVICE)
  185. except:
  186. print("Can't open serial port " + SERIAL_DEVICE)
  187. exit(-2)
  188. exit_thread = False
  189. th = threading.Thread(target = sendMessageThread)
  190. th.start()
  191. # display codes
  192. while True:
  193. tmp = []
  194. if TEST == False:
  195. try:
  196. tmp = readGamepad()
  197. except:
  198. print("Cant read data from gamepad.")
  199. exit(-3)
  200. else:
  201. tmp = genGamepadTestData()
  202. if tmp[0] > -60000:
  203. roll = 1000 + 1000 * (tmp[0] + 32768) / 65536
  204. if tmp[1] > -60000:
  205. pitch = 1000 + (1000 * (tmp[1] + 32768) / 65536)
  206. if tmp[2] > -60000:
  207. yaw = 1000 + 1000 * (tmp[2] + 32768) / 65536
  208. if tmp[3] > -60000:
  209. t = (2000 * (tmp[3] + 32768) / 65536)
  210. if t >= 1000:
  211. throttle = t
  212. if tmp[4] > -60000:
  213. aux1 = 1500 if aux1 == 1000 else 1000
  214. if tmp[5] > -60000:
  215. aux2 = 1500 if tmp[5] else 1000
  216. if tmp[6] > -60000:
  217. aux3 = 1500 if tmp[6] else 1000
  218. if tmp[7] > -60000:
  219. aux4 = 1500 if tmp[7] else 1000
  220. #if DEBUG == True:
  221. #print("> ", [ roll, pitch, yaw, throttle, aux1, aux2, aux3, aux4 ])
  222. time.sleep(MAIN_LOOP_DELAY/1000) // f = 1/MAIN_LOOP_DELAY/1000
  223. exit_thread = True
  224. ################################################################################