topus/stream.py at master · denilsonm/topus · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
# -*- coding: utf-8 -*-

import serial
import packet
import utils

class Stream:
	sensor_bits = 3

	CONST_NOT_ENOUGH_BYTES = 0
	CONST_INVALID_SENSOR = 1
	CONST_INVALID_HASH = 2
	CONST_SUCCESS = 3

	def __init__(self, ser=None, check_hash=False):
		if ser == None:
			raise ValueError("Every Stream instance needs a serial specified.")
		if not type(ser) == serial.Serial:
			raise ValueError("serial must be of type serial.Serial")

		self.data = []
		self.serial = ser
		self.check_hash = check_hash

	def read_serial(self):
		while self.serial.in_waiting > 0:
			d = ord(self.serial.read(size=1)[0])
			self.data.append(d)

	@staticmethod
	def hash_data(data_list):
		modval = 2**(8-Stream.sensor_bits)

		s0 = 0
		s1 = 0

		for byte in data_list:
			s0 = (s0 + byte) % modval
			s1 = (s1 + s0) % modval

		return s1 & ((1<<8-Stream.sensor_bits)-1)

	def retrieve_packet(self):
		if len(self.data) == 0:
			return Stream.CONST_NOT_ENOUGH_BYTES, []

		hash_expected = self.data[0] >> Stream.sensor_bits

		if self.check_hash:
			sensor_type = self.data[0] & ((1<<Stream.sensor_bits)-1)
		else:
			sensor_type = self.data[0]

		if not sensor_type in packet.sensor_map:
			return Stream.CONST_INVALID_SENSOR, []

		packet_type = packet.sensor_map[sensor_type]
		packet_length = packet_type.get_length()

		# Um byte extra para o header
		if len(self.data) < 1 + packet_length:
			# print(str(len(self.data)) + " / " + str(1+packet_length))
			return Stream.CONST_NOT_ENOUGH_BYTES, []

		if self.check_hash:
			hash_generated = Stream.hash_data([sensor_type] + self.data[1:(packet_length+1)])

			if hash_generated != hash_expected:
				return Stream.CONST_INVALID_HASH, []

		packet_data = self.data[0:(packet_length+1)]
		packet_data[0] = sensor_type

		return Stream.CONST_SUCCESS, packet_data

	def retrieve_packets(self):
		collected_packets = []

		while True:
			retrieve_return, packet_data = self.retrieve_packet()

			if retrieve_return == Stream.CONST_NOT_ENOUGH_BYTES:
				break
			elif retrieve_return == Stream.CONST_INVALID_HASH:
				print("A")
				self.data.pop(0)
			elif retrieve_return == Stream.CONST_INVALID_SENSOR:
				print("B")
				# Nunca deveria acontecer mas... vai que né
				self.data.pop(0)
			else:
				print("C, sensor " + str(packet_data[0]))
				# Sucesso
				collected_packets.append(packet_data)
				self.data = self.data[
					packet.sensor_map[packet_data[0]].get_length()+1:
					]

		return collected_packets

	@staticmethod
	def prepare_for_transmission(packet_data, hash_data=False):
		if not hash_data:
			return packet_data

		packet_hash = Stream.hash_data(packet_data) << Stream.sensor_bits

		packet_data[0] = packet_data[0] | packet_hash