#!/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