Newer
Older
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
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
#!/usr/bin/env python
import struct
from envbox_database import *
class envbox_command_builder:
F_HEADER = 0xFF
CRC_INIT = 0x00
CRC_POLY = 0x18
@staticmethod
def checksum( bytea, size ):
crc = envbox_command_builder . CRC_INIT
for i in range( 0, size ):
data = bytea[ i ]
for j in range( 0, 8 ):
fb = (crc ^ data) & 0x01
if fb == 0x01:
crc = crc ^ envbox_command_builder . CRC_POLY
crc = (crc >> 1) & 0x7F
if fb == 0x01:
crc = crc | 0x80
data = data >> 1
return crc
@staticmethod
def createCommand( cmd, value=None ):
# default size when there is no argument
size = 0
if not value is None:
if type( value ) is float:
size = 4
else:
size = 1
bytea = []
bytea . append( envbox_command_builder.F_HEADER )
bytea . append( 0x00 )
bytea . append( 0x00 )
bytea . append( 0x00 )
bytea . append( cmd[0] )
bytea . append( cmd[1] )
bytea . append( 0x00 )
bytea . append( size )
if not value is None:
if type( value ) is float:
byte_val = bytearray( struct . pack('f', value) )
for i in range(0, 4):
bytea . append( byte_val[ i ] )
else:
bytea . append( value )
try:
bytea . append( envbox_command_builder.checksum( bytea, size+8 ) )
except Exception as e:
print e
#print bytea
return bytea
@staticmethod
def parseCommand( bytea ):
#print bytea
# Verify Header
if not bytea[ 0 ] == envbox_command_builder . F_HEADER:
print( 'Bad header' )
return None
header = bytea[ 0 ]
#cmd = ( bytea[ 1 ], bytea[ 2 ])
ttype = bytea[ 6 ]
size = bytea[ 7 ]
nbr = 1
try:
# Get payload as a bytearray for unpack operation
b = buffer (bytearray( bytea[ 8 : 8+size ] ))
if ttype == envbox_types . uint8:
fmt = 'B'
nbr = size / 1
elif ttype == envbox_types . uint16:
fmt = 'H'
nbr = size / 2
elif ttype == envbox_types . uint32:
fmt = 'I'
nbr = size / 4
elif ttype == envbox_types . uint64:
fmt = 'Q'
nbr = size / 8
elif ttype == envbox_types . int8:
fmt = 'b'
nbr = size / 1
elif ttype == envbox_types . int16:
fmt = 'h'
nbr = size / 2
elif ttype == envbox_types . int32:
fmt = 'i'
nbr = size / 4
elif ttype == envbox_types . int64:
fmt = 'q'
nbr = size / 8
elif ttype == envbox_types . float:
fmt = 'f'
nbr = size / 4
elif ttype == envbox_types . string:
fmt = 'p'
else:
fmt = 'I'
if fmt == 'p':
payload = str(b)
else:
if nbr == 1:
payload = struct . unpack_from( fmt, b )[ 0 ]
else:
payload = []
step = size / nbr
for i in range( 0, nbr ):
b = bytearray( bytea[ 8 + i*step : 8 + (i+1)*step ] )
payload.append( struct . unpack_from( fmt, b )[ 0 ] )
except Exception as e:
print( 'Fail to unpack data' )
print e
crc = bytea[ 8 + size ]
#if( crc != envbox_command_builder.checksum( bytea, 5 ) ):
# tsprint( 'Invalid Checksum' )
# return None
return payload