frsky.py
· 3.1 KiB · Python
Sin formato
#!/usr/bin/env python
'''
_____ ____ _
| ___| __/ ___|| | ___ _
| |_ | '__\___ \| |/ / | | |
| _|| | ___) | <| |_| |
|_| |_| |____/|_|\_\\__, |
|___/
Host Protocol Decoder (HUB Data)
Note: TX/RX are inverted
'''
import serial
import struct
import math
ser = serial.Serial("/dev/ttyUSB0", 9600, timeout=2)
data = []
userdata = []
count = 0
stuffnext = False
stuffunext = False
ignorenext = False
frsky = {
"latitude" : 0,
"longitude" : 0,
"galtitude" : 0,
"altitude" : 0,
"temperature1" : 0,
"temperature2" : 0,
"gpsspeed" : 0
}
def ProcessUserData():
if userdata[1] == 0x1:
fract, inte = math.modf(frsky["galtitude"])
frsky["galtitude"] = ((userdata[3] << 4) + userdata[2]) + fract
#print frsky["galtitude"]
elif userdata[1] == 0x9:
fract, inte = math.modf(frsky["galtitude"])
frsky["galtitude"] = inte + ((userdata[3] << 4) + userdata[2]) / 1000.0;
elif userdata[1] == 0x5:
frsky["temperature2"] = userdata[2]
elif userdata[1] == 0x10:
fract, inte = math.modf(frsky["altitude"])
frsky["altitude"] = ((userdata[3] << 4) + userdata[2]) + fract
elif userdata[1] == 0x21:
fract, inte = math.modf(frsky["altitude"])
frsky["altitude"] = inte + ((userdata[3] << 4) + userdata[2]) / 10.0;
elif userdata[1] == 0x13:
fract, inte = math.modf(frsky["latitude"])
frsky["latitude"] = ((userdata[3] << 8) + userdata[2]) + fract
elif userdata[1] == 0x1B:
fract, inte = math.modf(frsky["latitude"])
frsky["latitude"] = inte + ((userdata[3] << 8)%10000 + userdata[2]) / 1e4;
print frsky["latitude"],frsky["longitude"]
elif userdata[1] == 0x12:
fract, inte = math.modf(frsky["longitude"])
frsky["longitude"] = fract + ((userdata[3] << 8) + userdata[2]);
elif userdata[1] == (0x12+8):
fract, inte = math.modf(frsky["longitude"])
frsky["longitude"] = inte + ((userdata[3] << 8)%10000 + userdata[2]) / 1e4;
#print frsky["longitude"]
def ProcessUserPacket(byte):
global stuffunext, userdata
if byte == 0x5E:
if len(userdata) == 4:
ProcessUserData()
userdata = [0x5E]
elif byte == 0x5D:
stuffunext = True
else:
if stuffunext:
userdata.append(byte^0x60)
else:
userdata.append(byte)
stuffunext = False
while True:
buff = ser.read(1)
if ord(buff) == 0x7E:
if len(data) == 10:
if data[1] == 0xFD:
for i in range(data[2]&0x7):
ProcessUserPacket(data[4+i])
count = 0
data = [0x7E]
else:
if ord(buff) == 0x7D:
stuffnext = True
#elif ord(buff) == 0x18:
# ignorenext = True
else:
if stuffnext:
data.append(ord(buff) ^ 0x20)
else:
#elif not ignorenext:
data.append(ord(buff))
#else:
# print "Duplicated"
ignorenext = False
stuffnext = False
| 1 | #!/usr/bin/env python |
| 2 | |
| 3 | ''' |
| 4 | _____ ____ _ |
| 5 | | ___| __/ ___|| | ___ _ |
| 6 | | |_ | '__\___ \| |/ / | | | |
| 7 | | _|| | ___) | <| |_| | |
| 8 | |_| |_| |____/|_|\_\\__, | |
| 9 | |___/ |
| 10 | |
| 11 | Host Protocol Decoder (HUB Data) |
| 12 | |
| 13 | Note: TX/RX are inverted |
| 14 | |
| 15 | ''' |
| 16 | |
| 17 | import serial |
| 18 | import struct |
| 19 | import math |
| 20 | |
| 21 | ser = serial.Serial("/dev/ttyUSB0", 9600, timeout=2) |
| 22 | |
| 23 | data = [] |
| 24 | userdata = [] |
| 25 | count = 0 |
| 26 | stuffnext = False |
| 27 | stuffunext = False |
| 28 | ignorenext = False |
| 29 | |
| 30 | frsky = { |
| 31 | "latitude" : 0, |
| 32 | "longitude" : 0, |
| 33 | "galtitude" : 0, |
| 34 | "altitude" : 0, |
| 35 | "temperature1" : 0, |
| 36 | "temperature2" : 0, |
| 37 | "gpsspeed" : 0 |
| 38 | } |
| 39 | |
| 40 | def ProcessUserData(): |
| 41 | if userdata[1] == 0x1: |
| 42 | fract, inte = math.modf(frsky["galtitude"]) |
| 43 | frsky["galtitude"] = ((userdata[3] << 4) + userdata[2]) + fract |
| 44 | #print frsky["galtitude"] |
| 45 | elif userdata[1] == 0x9: |
| 46 | fract, inte = math.modf(frsky["galtitude"]) |
| 47 | frsky["galtitude"] = inte + ((userdata[3] << 4) + userdata[2]) / 1000.0; |
| 48 | |
| 49 | elif userdata[1] == 0x5: |
| 50 | frsky["temperature2"] = userdata[2] |
| 51 | elif userdata[1] == 0x10: |
| 52 | fract, inte = math.modf(frsky["altitude"]) |
| 53 | frsky["altitude"] = ((userdata[3] << 4) + userdata[2]) + fract |
| 54 | elif userdata[1] == 0x21: |
| 55 | fract, inte = math.modf(frsky["altitude"]) |
| 56 | frsky["altitude"] = inte + ((userdata[3] << 4) + userdata[2]) / 10.0; |
| 57 | |
| 58 | elif userdata[1] == 0x13: |
| 59 | fract, inte = math.modf(frsky["latitude"]) |
| 60 | frsky["latitude"] = ((userdata[3] << 8) + userdata[2]) + fract |
| 61 | elif userdata[1] == 0x1B: |
| 62 | fract, inte = math.modf(frsky["latitude"]) |
| 63 | frsky["latitude"] = inte + ((userdata[3] << 8)%10000 + userdata[2]) / 1e4; |
| 64 | print frsky["latitude"],frsky["longitude"] |
| 65 | |
| 66 | elif userdata[1] == 0x12: |
| 67 | fract, inte = math.modf(frsky["longitude"]) |
| 68 | frsky["longitude"] = fract + ((userdata[3] << 8) + userdata[2]); |
| 69 | elif userdata[1] == (0x12+8): |
| 70 | fract, inte = math.modf(frsky["longitude"]) |
| 71 | frsky["longitude"] = inte + ((userdata[3] << 8)%10000 + userdata[2]) / 1e4; |
| 72 | #print frsky["longitude"] |
| 73 | |
| 74 | def ProcessUserPacket(byte): |
| 75 | global stuffunext, userdata |
| 76 | if byte == 0x5E: |
| 77 | if len(userdata) == 4: |
| 78 | ProcessUserData() |
| 79 | userdata = [0x5E] |
| 80 | elif byte == 0x5D: |
| 81 | stuffunext = True |
| 82 | else: |
| 83 | if stuffunext: |
| 84 | userdata.append(byte^0x60) |
| 85 | else: |
| 86 | userdata.append(byte) |
| 87 | stuffunext = False |
| 88 | |
| 89 | while True: |
| 90 | buff = ser.read(1) |
| 91 | if ord(buff) == 0x7E: |
| 92 | if len(data) == 10: |
| 93 | if data[1] == 0xFD: |
| 94 | for i in range(data[2]&0x7): |
| 95 | ProcessUserPacket(data[4+i]) |
| 96 | count = 0 |
| 97 | data = [0x7E] |
| 98 | else: |
| 99 | if ord(buff) == 0x7D: |
| 100 | stuffnext = True |
| 101 | #elif ord(buff) == 0x18: |
| 102 | # ignorenext = True |
| 103 | else: |
| 104 | if stuffnext: |
| 105 | data.append(ord(buff) ^ 0x20) |
| 106 | else: |
| 107 | #elif not ignorenext: |
| 108 | data.append(ord(buff)) |
| 109 | #else: |
| 110 | # print "Duplicated" |
| 111 | ignorenext = False |
| 112 | stuffnext = False |
| 113 |