bexus-26-imu-fusion-flight-software/groundstation/functions.py

#!/usr/bin/python3
# -*- coding: utf-8 -*-

import struct
import copy
from collections import OrderedDict

def safe_cast(val, to_type, default=None):
	try:
		return to_type(val)
	except (ValueError, TypeError):
		return default

def listToInt(a, signed=False):
	res = 0
	for x in reversed(a):
		res = res * 256 + x
	if signed and res >= 256 ** len(a) / 2:
		res -= 256 ** len(a)
	return res
	
def listToDouble(a):
	return struct.unpack('d', a)[0]

def listToFloat(a):
	return struct.unpack('f', a)[0]
	
def listToVector(a, double=True):
	if double:
		return struct.unpack('ddd', a)
	else:
		return struct.unpack('iii', a)

def listToVector_Uint(a):
	return struct.unpack('iii', a)
		
def getSettings(set, name, default=None):
	val = set.value(name)
	if val == None:
		return default
	
	return val
	
	
class PktParser():
	def __init__(self, struct, offset=0):
		self.props = OrderedDict()
		
		for line in struct.splitlines():
			prop = self._getInfo(line)
			if prop:
				length, cb, name = prop
				
				#print("{:4d} {:2d} {}".format(offset, length, name))
				
				self.props[name] = lambda data, c=cb, o=offset, l=length: c(data[o:o+l])
				offset += length
				
		self.end = offset
		self.offset = offset
		
		
	def _getInfo(self, line):
		i = 0
		type = None
		name = None
		
		for word in line.split():
			if word == "":
				#print("(empty)")
				continue
				
			if word[:2] in ["//", "/*"]:
				#print(word)
				break
				
			if word == "struct":
				continue
				
			if i == 0: #type
				type = self._getType(word)
				if type:
					i += 1
					continue
			
			if i == 1: #name
				p = word.find(";")
				name = word[:p]
			
		if type and name:
			return type + (name,)
			
	def _getType(self, name):
		
		if name == "uint32_t":
			return (4, listToInt)
			
		if name == "int32_t":
			return (4, lambda v: listToInt(v, True))
		
		if name == "double":
			return (8, listToDouble)
		
		if name == "float":
			return (4, listToFloat)
		
		if name == "vector_Uint":
			return (12, listToVector_Uint)
			
		if name == "vector":
			return (24, listToVector)
			
		print("Type {} invalid".format(name))	
		
		return None
	
	def parse(self, data):
		
		if len(data) < self.end:
			print("data too short {} < {}".format(len(data), self.end))
			return
		
		ret = OrderedDict()
		
		for prop in self.props.keys():
			ret[prop] = self.props[prop](data)
		
		d = {}
		for key in ret:
			if type(ret[key]) is tuple:
				d.update(dict(zip((key+"_x",key+"_y",key+"_z"), ret[key])))
		ret.update(d)
		
		return ret
		
	def keys(self):
		tmp = b'\x00' * self.end
		return self.parse(tmp).keys()
		
def confirm(prompt=None, resp=False):
	if prompt is None:
		prompt = 'Confirm'

	if resp:
		prompt = '%s [%s]|%s: ' % (prompt, 'y', 'n')
	else:
		prompt = '%s [%s]|%s: ' % (prompt, 'n', 'y')
		
	while True:
		ans = raw_input(prompt)
		if not ans:
			return resp
		if ans not in ['y', 'Y', 'n', 'N']:
			print ('please enter y or n.')
			continue
		if ans == 'y' or ans == 'Y':
			return True
		if ans == 'n' or ans == 'N':
			return False