I apologize for the lateness in this response but have been overloaded with college and work lately. I managed to get a python code working I found online and managed to modify it to accept socks commands. I also added an auto reconnect to the socks connection and made it possible to fly with the wii remote over wifi. The server and client codes are below but I am having trouble on how to modify this code to receive weather or not the battery should notify me it is of need to charge. Better yet, what the current battery readings are. I have begun to set up the code to receive the input on another GPIO pin on the raspberry pi and remembered this post. If anyone is still interested in helping or is looking for a code themselves, the code is below.
Code: Select all
// Server
import sys
import serial
import socket
import struct
import binascii
import time
import threading
import types
import subprocess
multi_info = {
'rcdata': [],
'imu': [],
'mean_imu':[],
'debug': None,
'rbuffer': '',
}
CMD2CODE = {
'MSP_IDENT' : 100,
'MSP_STATUS' : 101,
'MSP_RAW_IMU' : 102,
'MSP_SERVO' : 103,
'MSP_MOTOR' : 104,
'MSP_RC' : 105,
'MSP_RAW_GPS' : 106,
'MSP_COMP_GPS' : 107,
'MSP_ATTITUDE' : 108,
'MSP_ALTITUDE' : 109,
'MSP_ANALOG' : 110,
'MSP_RC_TUNING' : 111,
'MSP_PID' : 112,
'MSP_BOX' : 113,
'MSP_MISC' : 114,
'MSP_MOTOR_PINS' : 115,
'MSP_BOXNAMES' : 116,
'MSP_PIDNAMES' : 117,
'MSP_WP' : 118,
'MSP_BOXIDS' : 119,
'MSP_SET_RAW_RC' : 200,
'MSP_SET_RAW_GPS' : 201,
'MSP_SET_PID' : 202,
'MSP_SET_BOX' : 203,
'MSP_SET_RC_TUNING' : 204,
'MSP_ACC_CALIBRATION' : 205,
'MSP_MAG_CALIBRATION' : 206,
'MSP_SET_MISC' : 207,
'MSP_RESET_CONF' : 208,
'MSP_SET_WP' : 209,
'MSP_SWITCH_RC_SERIAL' : 210,
'MSP_IS_SERIAL' : 211,
'MSP_DEBUG' : 254,
}
CODE2INFO = {
100: {'type': '', 'data': None},
102: {'type': '9h', 'data': multi_info['imu']},
105: {'type': '8h', 'data': multi_info['rcdata']},
254: {'type': '4h', 'data': multi_info['debug']},
210: {'type': '', 'data': None},
}
def send(data_length, code, data):
global multi_info
global ser
checksum = 0
total_data = ['$', 'M', '<', data_length, code] + data
for n in struct.pack('<2B%dh' % len(data), *total_data[3:len(total_data)]):
checksum = checksum ^ ord(n)
total_data.append(checksum)
try:
ser.write(struct.pack('<3c2B%dhB' % len(data), *total_data))
ser.flush()
except Exception, ex:
print 'send data failed'
connect()
def recieve():
global multi_info
global ser
ihead_flag = '$M>'
need_read = True
while need_read:
try:
ser.flush()
multi_info['rbuffer'] += ser.read(ser.inWaiting())
except Exception, ex:
print 'recieve data failed'
connect()
try:
pos = multi_info['rbuffer'].find(ihead_flag)
except Exception, ex:
break
if pos >=0:
try:
dl = ord(multi_info['rbuffer'][pos+len(ihead_flag):pos+len(ihead_flag)+1])
except Exception, ex:
break
data = multi_info['rbuffer'][pos+len(ihead_flag):pos+len(ihead_flag) + dl + 3]
if (dl + 3) == len(data) and len(data) > 3:
checksum = 0
orig_checksum = data[-1:]
for i in data[:-1]:
checksum = checksum ^ ord(i)
if ord(orig_checksum) == checksum:
code = ord(data[1:2])
dl = ord(data[0:1])
if CODE2INFO[code]['data'] != None:
if type(CODE2INFO[code]['data']) == types.ListType:
del CODE2INFO[code]['data'][:]
CODE2INFO[code]['data'].extend(struct.unpack('<' + CODE2INFO[code]['type'], data[2:2+dl]))
elif (dl + 3) > len(data):
break
multi_info['rbuffer'] = multi_info['rbuffer'][pos + len(ihead_flag) + len(data):]
else:
multi_info['rbuffer'] = ''
break
class serialData(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
def run(self):
while 1:
send(0, CMD2CODE['MSP_RAW_IMU'], [])
recieve()
class wirelessData(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
def run(self):
roll = 1500
pitch = 1500
yaw = 1500
throttle = 1000
aux1 = 1500
aux2 = 1500
aux3 = 1500
aux4 = 1500
subprocess.call('echo "18" > /sys/class/gpio/export',shell=True)
subprocess.call('echo "in" > /sys/class/gpio/gpio18/direction',shell=True)
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.bind(('', 1080))
sock.listen(1)
conn, crap = sock.accept()
while 1:
battery_low = subprocess.check_output('cat /sys/class/gpio/gpio18/value', shell=True).rstrip()
incoming = conn.recv(1024)
if incoming:
try:
rc_roll,rc_pitch,rc_yaw,rc_throttle,rc_option = incoming.split(',')
except ValueError:
continue
roll = int( rc_roll )*1000/255+1000
pitch = int( rc_pitch )*1000/255+1000
yaw = int( rc_yaw )*1000/255+1000
throttle = int(rc_throttle)*1000/255+1000
data = []
data.append(True and roll)
data.append(True and pitch)
data.append(True and yaw)
data.append(True and throttle)
data.append(True and aux1)
data.append(True and aux2)
data.append(True and aux3)
data.append(True and aux4)
send(16, CMD2CODE['MSP_SET_RAW_RC'], data)
try:
conn.sendall(
str(multi_info['imu']) +
"," +
str(battery_low) +
"," +
str(data)
)
except Exception as exception:
sock.listen(1)
conn, crap = sock.accept()
continue
class gStreamer(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
def run(self):
while 1:
subprocess.call('raspivid -t 999999 -h 240 -w 320 -fps 60 -b 100000 -ex auto -vs -o - | gst-launch-1.0 -v fdsrc ! h264parse ! rtph264pay config-interval=1 pt=96 ! gdppay ! tcpserversink host=192.168.1.3 port=40',shell=True)
time.sleep(1)
if __name__ == "__main__":
try:
try:
ser = serial.Serial('/dev/ttyAMA0', 115200)
except Exception, ex:
print 'open serial port fail\n'
sys.exit()
thread0 = serialData()
thread0.daemon = True
thread0.start()
thread1 = wirelessData()
thread1.daemon = True
thread1.start()
thread2 = gStreamer()
thread2.daemon=True
thread2.start()
while 1:
time.sleep(100)
except (KeyboardInterrupt, SystemExit):
print 'KeyboardInterrupt -- Terminating All Threads'
subprocess.call('echo "18" > /sys/class/gpio/unexport',shell=True)
//Client
import socket
import serial
import time
import threading
import subprocess
connected = False
class gStreamer(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
def run(self):
while 1:
if connected:
subprocess.call('gst-launch-1.0 -v tcpclientsrc host=192.168.1.3 port=40 ! gdpdepay ! rtph264depay ! avdec_h264 ! videoconvert ! autovideosink sync=false',shell=True,stdout=open('/dev/null', 'w'), stderr=subprocess.STDOUT)
time.sleep(1)
else:
time.sleep(1)
class rc(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
def run(self):
global ser
global rc
while 1:
rc = ser.readline()
if __name__ == "__main__":
try:
ser = serial.Serial('/dev/tty.SLAB_USBtoUART', 9600, timeout=1)
thread0 = rc()
thread0.daemon=True
rc = ser.readline()
thread0.start()
thread1 = gStreamer()
thread1.daemon=True
thread1.start()
host = '192.168.1.3'
port = 1080
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect((host, port))
connected = True
print "Connected"
while True:
try:
roll,pitch,yaw,throttle,option = rc.rstrip().split(',')
except ValueError:
print "ERROR: incorrect or no data recieved"
continue
throttle = str((int(throttle)-134)*255/121)
try:
sock.sendall(roll+","+pitch+","+yaw+","+throttle+","+option)
incoming = sock.recv(1024)
except Exception as exception:
connected = False
print "Connection Lost"
while True:
try:
print "Reconnecting"
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect((host, port))
connected = True
print "Reconnected"
break
except Exception as exception:
print "Failed"
time.sleep(1)
continue
print incoming
except (KeyboardInterrupt, SystemExit):
print 'KeyboardInterrupt -- Terminating All Threads'
I thank you all for your suggestions. And if anyone has any advice I would love to hear it. I am planning on upgrading this setup to a glider eventually to achieve longer flight time and farther distance. I have a ways to go however. Also, not sure on which glider to purchase quite yet.
