-
Notifications
You must be signed in to change notification settings - Fork 4
Expand file tree
/
Copy pathcrsf.py
More file actions
executable file
·1556 lines (1362 loc) · 60.6 KB
/
crsf.py
File metadata and controls
executable file
·1556 lines (1362 loc) · 60.6 KB
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
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#!/usr/bin/env python3
'''
CRSF client in Python. Works via TCP or UART connections.
Can be used to connect to WiFi module on Crossfire.
It can sniff broadcast frames, receive paramters, etc.
For navigating inside the CRSF menu use keys: UP, DOWN, Q, ENTER, BACKSPACE.
'''
# TODO: figure out why TCP doesn't work with the "menu" option
# TODO: introduce more command-line arguments: port, baud, host, origin
# TODO: implement integer types (these are rarely used)
# TODO: implement command polling and resends
# TODO: implement CRSFv3
import sys, time, functools, os, curses
import serial, socket, queue, threading
TCP_HOST = '192.168.4.1'
TCP_PORT = 60950 # this TCP port is used by Fusion
ORIGIN_ADDR = "CRSF.FC_ADDR"
SERIAL_PORT = '/dev/tty.usbserial-AQ00MH7Z'
SERIAL_BAUD = 416666
# TODO: can also use WebSocket if Fusion is needed simultaneously
TICK_SPEED = 20 # Ticks per microsecond (for LOG frames)
SHORT_HIST_SIZE = 451 # 90 s
HIST_SIZE = 10*SHORT_HIST_SIZE
COLORIZE = not sys.platform.startswith('win')
class Terminal:
GREEN, RED = 2, 160 # Unix terminal colors
@staticmethod
def fg(text, color):
'''
Adds foreground color for Unix terminals.
Leaves text without change on Windows.
'''
if sys.platform.startswith('win'):
return text
else:
return "\33[38;5;" + str(color) + "m" + text + "\33[0m"
@staticmethod
def green(text):
return Terminal.fg(text, Terminal.GREEN)
@staticmethod
def red(text):
return Terminal.fg(text, Terminal.RED)
class CRSF:
SYNC = 0xc8
# CRSF Device Addresses
BROADCAST_ADDR = 0x00
CLOUD_ADDR = 0x0E # MQTT server
WIFI_ADDR = 0x12
REMOTE_ADDR = 0xEA
RX_ADDR = 0xEC
TX_ADDR = 0xEE
FC_ADDR = 0xC8 # flight controller
VTX_ADDR = 0xCE
# CRSF Frame Types
MSG_TYPE_GPS = 0x02
MSG_TYPE_GPST = 0x03
MSG_TYPE_BATT = 0x08
MSG_TYPE_VTX_TEL = 0x10
MSG_TYPE_LINK_STATS = 0x14
MSG_TYPE_PPM = 0x16 # channel values
MSG_TYPE_PPM3 = 0x17 # CRSF V3 (packed channel values)
MSG_TYPE_LINK_STATS_RX = 0x1C # CRSF V3
MSG_TYPE_LINK_STATS_TX = 0x1D # CRSF V3
MSG_TYPE_ATTD = 0x1E
MSG_TYPE_MADD = 0x1F
MSG_TYPE_PING = 0x28
MSG_TYPE_DEVICE_INFO = 0x29
MSG_TYPE_PARAM_ENTRY = 0x2B
MSG_TYPE_PARAM_READ = 0x2C
MSG_TYPE_PARAM_WRITE = 0x2D
MSG_TYPE_CMD = 0x32
MSG_TYPE_LOG = 0x34
MSG_TYPE_REMOTE = 0x3A # Remote-related frames
MSG_TYPE_MAVLINK_ENV = 0xAA
# CRSF menu parameter types
PARAM_TYPE_UINT8 = 0
PARAM_TYPE_INT8 = 1
PARAM_TYPE_UINT16 = 2
PARAM_TYPE_INT16 = 3
PARAM_TYPE_UINT32 = 4
PARAM_TYPE_INT32 = 5
PARAM_TYPE_FLOAT = 8
PARAM_TYPE_TEXT_SELECTION = 9
PARAM_TYPE_STRING = 10
PARAM_TYPE_FOLDER = 11
PARAM_TYPE_INFO = 12
PARAM_TYPE_COMMAND = 13
PARAM_TYPE_OUT_OF_RANGE = 127
# CRSF frame structure (field offsets)
OFFSET_LENGTH = 1
OFFSET_MSG_TYPE = 2
# Menu
MENU_COMMAND_READY = 0 # --> feedback
MENU_COMMAND_START = 1 # <-- input
MENU_COMMAND_PROGRESS = 2 # --> feedback
MENU_COMMAND_CONFIRMATION_NEEDED = 3 # --> feedback
MENU_COMMAND_CONFIRM = 4 # <-- input
MENU_COMMAND_CANCEL = 5 # <-- input
MENU_COMMAND_POLL = 6 # <-- input
@staticmethod
def decode_int32(data):
uint32 = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3]
if uint32 & 0x80000000:
return -(~uint32) - 1
else:
return uint32
@staticmethod
def encode_int32(val):
return [
(val >> 24) & 0xFF,
(val >> 16) & 0xFF,
(val >> 8) & 0xFF,
val & 0xFF,
]
# Message type -> human readable name
msg_name = {}
for name in CRSF.__dict__:
if name.startswith('MSG_TYPE_'):
msg_name[CRSF.__dict__[name]] = name[9:]
#print(msg_name) # show known message types
ORIGIN_ADDR = eval(ORIGIN_ADDR)
# Device address -> human readable name
dev_name = {}
for name in CRSF.__dict__:
if name.endswith('_ADDR'):
dev_name[CRSF.__dict__[name]] = name[:-5]
class crsf_crc8:
RESET_VALUE = 0x00
POLYNOM_1 = 0xD5 # CRC8 DVB-S2 (polynomial used for validating CRSF frames)
POLYNOM_2 = 0xBA # custom polynomial used for validating CRSF commands
MSB_SET = 0x80
FINISH_VALUE = 0x00
def __init__(self, poly = None):
self.val = self.RESET_VALUE
self.poly = poly if poly is not None else self.POLYNOM_1
def _calc(self, byte):
for i in range(8):
msb_flag = self.val & self.MSB_SET
self.val <<= 1
if byte & self.MSB_SET:
self.val += 1
byte <<= 1
if msb_flag:
self.val ^= self.poly
self.val &= 0xFF
def digest(self, data):
for x in data:
self._calc(x)
def finish(self):
self._calc(self.FINISH_VALUE)
return self.val
def calc_crc8(data):
'''Calculate CRC8 as in CRSF frames'''
crc = crsf_crc8(poly = crsf_crc8.POLYNOM_1)
crc.digest(data)
return crc.finish()
class crsf_frame:
def __init__(self, data):
self.data = bytes(data)
@property
def len(self):
return self.data[1] + 2
@property
def type(self):
return self.data[2]
@property
def bytes(self):
return self.data
@property
def is_extended(self):
'''Extended CRSF frames include their ORIGIN and DESTINATION'''
return self.type >= 0x28 and self.type <= 0x96 or self.type == CRSF.MSG_TYPE_MAVLINK_ENV
@property
def origin(self):
return self.data[4] if self.is_extended else None
@property
def destination(self):
return self.data[3] if self.is_extended else None
@property
def payload(self):
return self.data[5 if self.is_extended else 3:-1]
def parse(self):
if self.type == CRSF.MSG_TYPE_DEVICE_INFO:
delim = self.payload.index(0x00)
device_name, tail = self.payload[:delim], self.payload[delim+1:]
device_name = bytes(device_name).decode()
sn = (tail[0] << 24) | (tail[1] << 16) | (tail[2] << 8) | tail[3]
hw_id = (tail[4] << 24) | (tail[5] << 16) | (tail[6] << 8) | tail[7]
sw_id = (tail[8] << 24) | (tail[9] << 16) | (tail[10] << 8) | tail[11]
param_count = tail[12]
version = tail[13]
return device_name, sn, hw_id, sw_id, param_count, version
else:
raise ValueError("cannot parse frame of type 0x%02x" % self.type)
def __str__(self):
s = 'SYNC ' if self.bytes[0] == CRSF.SYNC else ('%02x ' % self.bytes[0])
s += 'L=%d ' % self.len
s += ('(%s) ' % msg_name[self.type]) if self.type in msg_name else ('(t=%02x) ' % self.type)
i = 3
if self.is_extended:
s += '%s->%s ' % (dev_name.get(self.bytes[i+1], '%02x' % self.bytes[i+1]),
dev_name.get(self.bytes[i ], '%02x' % self.bytes[i ]))
i += 2
s += ' '.join(map(lambda x: "%02x" % x, self.bytes[i:]))
return s
class crsf_parser:
def __init__(self, silent):
self.data = bytearray()
self.silent = silent
def digest(self, data):
'''Digests incoming bytes, yields complete CRSF frames'''
self.data += bytearray(data)
#print(len(self.data))
while len(self.data) >= 4:
while self.data and self.data[0] != CRSF.SYNC:
if not self.silent:
sys.stderr.write("byte %02x discarded\n" % self.data[0])
self.data = self.data[1:]
if len(self.data) > 1:
frame_len = self.data[1] + 2
if len(self.data) >= frame_len:
crc_byte = self.data[frame_len - 1]
calc_crc = calc_crc8(self.data[2:frame_len - 1])
if crc_byte == calc_crc:
frame, self.data = self.data[:frame_len], self.data[frame_len:]
yield crsf_frame(frame)
else:
if not self.silent:
sys.stderr.write("crc mismatch; byte discarded\n")
self.data = self.data[1:]
else:
break
else:
break
def log_data(header, data):
print(str(header) + ':', ''.join(map(lambda x: " %02x" % x, data)))
def rle_decode(data):
'''Decode Run-Length Encoded number'''
off = 0
res = 0
ch = 0x80
while (ch & 0x80) and off < 6:
ch = data[off]
tmp = ch & 0x7F
res |= (tmp << (7*off))
off += 1
return (res, off)
def msg_decode(data):
'''Decode debug message for XLOG frame type'''
if (data[0] & 0x80) == 0:
msg = (bytes([data[0] & 0x7F]).decode(), 1)
else:
i = data.find(0)
data = bytearray(data)
data[0] &= 0x7F
msg = (bytes(data[:i]).decode(), i+1)
return msg
bin_byte = lambda x: '{:08b}'.format(x)
ticks_to_us = lambda x: (x - 992)*5//8 + 1500
def ppm_channels_decode(data):
data = ''.join(map(bin_byte, reversed(data)))
assert(len(data) % 11 == 0)
ticks = [int(data[x:x+11], 2) for x in range(len(data)-11, 0, -11)]
return list(map(ticks_to_us, ticks))
up_lqi_history, down_lqi_history = [], []
def explain_link_stats(data, colorize=True):
global up_lqi_history, down_lqi_history
cur_time = time.time()
up_lqi_history.append((cur_time, data[2]))
down_lqi_history.append((cur_time, data[8]))
up_lqi_history = up_lqi_history[-HIST_SIZE:]
down_lqi_history = down_lqi_history[-HIST_SIZE:]
s = 'Uplink: RSSI={}/{}'.format(-data[0], -data[1])
s += ', LQI={:3d}%, SNR={}, Ant.={}'.format(data[2], data[3], data[4])
s += ', RFmode={}, RFpwr={}'.format(data[5], data[6])
s += '; Downlink: RSSI={}, LQI={:3d}%, SNR={}'.format(-data[7], data[8], data[9])
# Show average LQI (both uplink and downlink)
if len(up_lqi_history) > 1 and len(down_lqi_history) > 1:
def get_hist_lqi(lqi_hist):
avg = sum([x[1] for x in lqi_hist])/len(lqi_hist)
if colorize:
green, red = Terminal.green, Terminal.red
else:
green = red = lambda x: x
return '{}/{:.1f}'.format(
green('100') if avg == 100 else red('{:.2f}'.format(avg)),
(lqi_hist[-1][0] - lqi_hist[0][0])
)
short_up = get_hist_lqi(up_lqi_history[-SHORT_HIST_SIZE:])
long_up = get_hist_lqi(up_lqi_history[-HIST_SIZE:])
short_down = get_hist_lqi(down_lqi_history[-SHORT_HIST_SIZE:])
long_down = get_hist_lqi(down_lqi_history[-HIST_SIZE:])
s += '\n History: Uplink LQI={}, {}; Downlink LQI={}, {}'.format(short_up, long_up, short_down, long_down)
return s
ppm_times = []
last_time = 0
def explain_frame(frame, colorize=True):
'''
Return text with some explanation/analysis of some types of CRSF frames
or empty string.
'''
global last_time, ppm_times
s = ''
data = frame.bytes
# Parse certain kinds of frames
if frame.type == CRSF.MSG_TYPE_PPM:
channels = ppm_channels_decode(frame.payload)
s += '\n CH1..16: ' + ', '.join(map(str, channels))
curr = time.time()*1000
ppm_times = [curr] + ppm_times
ppm_times = ppm_times[:500]
s += ' {:.2f} {:.2f}'.format(curr - last_time,
(ppm_times[0] - ppm_times[-1])/len(ppm_times))
last_time = curr
elif frame.type == CRSF.MSG_TYPE_PPM3:
s += '\n CRSFv3'
elif frame.type == CRSF.MSG_TYPE_LINK_STATS:
s += '\n ' + explain_link_stats(frame.payload, colorize)
elif frame.type == CRSF.MSG_TYPE_DEVICE_INFO:
device_name, sn, hw_id, fw_id, param_count, version = frame.parse()
s += '\n Device: {name}, S/N=0x{sn:x} HW_ID=0x{hw_id:x}, SW_ID=0x{sw_id:x}, param count={cnt}, v={v}'.format(
sn=sn, name=device_name, hw_id=hw_id, sw_id=fw_id, cnt=param_count, v=version)
elif frame.type == CRSF.MSG_TYPE_PARAM_ENTRY:
i = 5 # payload start
if data[i+3:i+4] == [CRSF.PARAM_TYPE_INFO]:
# "Info" parameter (unmodifiable string)
name = data[i+4:]
delim = name.index(0x00)
name, val = name[:delim], data[i+4+delim+1:]
val = val[:val.index(0x00)]
name = bytes(name).decode()
val = bytes(val).decode()
s += '\n ' + name + ': ' + val
elif frame.type == CRSF.MSG_TYPE_LOG:
tm = functools.reduce(lambda v,e: v*256 + e, (data[5:9]))
val = ''
if data[-2] == 0:
val = data[9:-2].decode()
else:
val = ' '.join(map(lambda x: "%02x" % x, data[9:-1]))
s += '\n tick {ticks} ({ms} ms): {val}'.format(ticks=tm, ms=tm//(TICK_SPEED*1000), val=val)
return s
def crsf_log_frame(header, frame):
'''Print CRSF frame in a partially parsed way'''
s = str(header) + ': ' + str(frame)
s += explain_frame(frame, COLORIZE)
print(s)
def create_frame(data):
'''Takes CRSF frame data, sets correct length byte and adds CRC8 byte'''
frame = bytearray([CRSF.SYNC, 0]) + bytearray(data)
frame[CRSF.OFFSET_LENGTH] = len(frame) - 1
frame.append(calc_crc8(frame[2:]))
return crsf_frame(frame)
def create_ping_frame(dest=CRSF.BROADCAST_ADDR, orig=ORIGIN_ADDR):
'''Create broadcast PING frame from bytes'''
return create_frame([
CRSF.MSG_TYPE_PING, # type
dest, # destination
orig # origin
])
def create_param_read_frame(dest, param_num, chunk, orig=ORIGIN_ADDR):
return create_frame([
CRSF.MSG_TYPE_PARAM_READ, # type
dest, # destination
orig, # origin
param_num,
chunk
])
def create_param_write_frame(dest, param_num, value: bytearray, orig=ORIGIN_ADDR):
return create_frame([
CRSF.MSG_TYPE_PARAM_WRITE, # type
dest, # destination
orig, # origin
param_num] +
list(value)
)
def create_device_info_frame(dest=CRSF.BROADCAST_ADDR, orig=ORIGIN_ADDR):
'''Return "fake" device information about Agent Python'''
name = "Agent Python"
return create_frame([
CRSF.MSG_TYPE_DEVICE_INFO, # type
dest, # destination
orig, # origin
# Payload
] + list(name.encode()) + [ # name of entity
0x00, # NUL to terminate string
0x12, 0x34, 0x56, 0x78, # S/N
0x01, 0x23, 0x45, 0x02, # HW ID
0x00, 0x00, 0x11, 0x11, # FW ID
0x00, # parameter total
0x01 # parameter version number
])
class CRSFConnection:
def read_crsf(self):
pass
def write_crsf(self, frame):
pass
class TCPConnection(CRSFConnection):
'''Class for exchanging CRSF over TCP'''
TCP_TIMEOUT_MS = 1000
TCP_RECV = 2048
def __init__(self, silent):
self.parser = crsf_parser(silent)
self.frames = [] # incoming frames that were already parsed
self.silent = silent
# Connect via TCP socket
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.settimeout(TCPConnection.TCP_TIMEOUT_MS)
self.socket.connect((TCP_HOST, TCP_PORT))
def read_crsf(self):
# Receive data from serial
data = None
try:
data = self.socket.recv(TCPConnection.TCP_RECV)
except socket.timeout:
if not self.silent:
sys.stderr.write('Timeout\n')
except KeyboardInterrupt:
if not self.silent:
print('KeyboardInterrup: Quit')
sys.exit(0)
except Exception as e:
if not self.silent:
sys.stderr.write('ERROR: TCP disconnected\n')
del self.socket
raise(e)
# TODO: move to super
# Parse data
if data and len(data):
#print(len(data))
for frame in self.parser.digest(data):
self.frames.append(frame)
# Return next frame
return self.frames.pop(0) if self.frames else None
def write_crsf(self, frame):
self.socket.send(frame.bytes)
class SerialConnection(CRSFConnection):
'''Class for exchanging CRSF over UART'''
SERIAL_SLEEP = 0.001
def __init__(self, silent):
self.parser = crsf_parser(silent)
self.serial = serial.Serial(SERIAL_PORT, baudrate=SERIAL_BAUD)
self.in_queue = queue.Queue()
# Receiving thread
self.thread = threading.Thread(target=self.read_thread)
self.thread.setDaemon(1)
self.alive = threading.Event()
self.alive.set()
self.thread.start()
def read_thread(self):
'''Separate thread for receiving frames asynchronously'''
while self.alive.isSet():
# How many bytes are in waiting?
waiting = self.serial.in_waiting
data = self.serial.read(waiting if waiting else 1)
if data and len(data):
for frame in self.parser.digest(data):
self.in_queue.put(frame, block=False)
time.sleep(SerialConnection.SERIAL_SLEEP)
print('Exit')
def read_crsf(self):
try:
frame = self.in_queue.get(block=False)
except queue.Empty:
frame = None
return frame
def write_crsf(self, frame):
self.serial.write(frame.bytes)
def parse_args():
'''Parse command line arguments'''
import argparse, pathlib
arg_parse = argparse.ArgumentParser()
arg_parse.add_argument('--tcp', action = 'store_true',
help = 'use TCP connection instead of UART' )
arg_parse.add_argument('--menu', action = 'store_true',
help = 'CRSF menu mode (otherwise - logs mode)' )
opts = arg_parse.parse_args()
return opts
def get_crsf_connection(use_tcp, silent):
if not silent:
print('Connecting with {}...'.format('TCP' if use_tcp else 'UART'))
if use_tcp:
return TCPConnection(silent)
else:
return SerialConnection(silent)
def log_mode(use_tcp):
print('Press Ctrl+C to exit')
last_ping = last_read = 0
# Outer loop to reconnect on disconnect
while True:
conn = get_crsf_connection(use_tcp, False)
# Update state machine
while True:
try:
frame = conn.read_crsf()
except Exception as e:
print("err", e)
break
# Display data
if frame:
crsf_log_frame('Received', frame)
# Process frame
if frame.type == CRSF.MSG_TYPE_PING:
resp_frame = create_device_info_frame(dest=frame.origin, orig=ORIGIN_ADDR)
conn.write_crsf(resp_frame)
# Send ping frame (need to send something periodically so that connection is not reset)
if time.time() - last_ping > 10:
last_ping = time.time()
# Send ping frame
frame = create_ping_frame()
crsf_log_frame('Sending ping', frame)
conn.write_crsf(frame)
#frame = create_frame([CRSF.MSG_TYPE_PARAM_READ, CRSF.TX_ADDR, CRSF.CLOUD_ADDR, 1, 0])
#s.send(frame.bytes)
time.sleep(0.0001)
class CRSFParam:
'''
CRSF parameters are, basically, key-value pairs.
"Key" is the name of the parameter. "Value" is modifiable by user
(except for INFO type, for which the value is constant).
'''
# Parameter types determine the type of the value
TEXT_SELECTION_TYPE = 0x09 # value is uint8_t, with a list of options
STRING_TYPE = 0x0a # value is a string, entered by user directly
FOLDER_TYPE = 0x0b # no modifiable value; groups other parameters
INFO_TYPE = 0x0c # value is constant, CANNOT be modified
COMMAND_TYPE = 0x0d
# Numeric types (in practice, rarely used)
UINT8_TYPE = 0
INT8_TYPE = 1
UINT16_TYPE = 2
INT16_TYPE = 3
UINT32_TYPE = 4
INT32_TYPE = 5
FLOAT_TYPE = 8
def __init__(self, param_num, debug_cb=None):
# TODO: track if parameter is changing frequently or always stable
# TODO: poll stable parameters less often
# TODO: ensure that different chunks are from short time frame
# TODO: detect changes in chunks, invalidate parameter if some chunk changed
# Common CRSF-related attributes
self.num = param_num
self.parent_folder = None
self.type = None
self.name = '...'
self.chunks = [] # payloads of all chunks
self.value = None # except for FOLDER_TYPE
self.hidden = False
# Type-specific attributes
self.min = self.default = self.max = None # numeric types and TEXT_SELECTION_TYPE
self.unit = None # only in numeric and TEXT_SELECTION_TYPE
self.children = None # only for FOLDER_TYPE
self.options = [] # only for TEXT_SELECTION_TYPE
self.max_length = None # only for STRING_TYPE
self.decimal_point = None # only for FLOAT_TYPE
self.step_size = None # only for FLOAT_TYPE
self.status = None # only for COMMAND_TYPE
self.timeout = None # only for COMMAND_TYPE
self.info = None # only for COMMAND_TYPE
# Additional attributes
self.debug_cb = debug_cb
self.created_time = time.time()
self.obtained_time = 0 # when was this parameter last refreshed?
def is_folder(self):
return self.type == self.FOLDER_TYPE
def is_info(self):
return self.type == self.INFO_TYPE
def is_string_input(self):
return self.type == self.STRING_TYPE
def is_selection(self):
return self.type == self.TEXT_SELECTION_TYPE
def is_command(self):
return self.type == self.COMMAND_TYPE
def is_float(self):
return self.type == self.FLOAT_TYPE
def is_number_input(self):
return self.type in [self.FLOAT_TYPE,
self.UINT8_TYPE, self.INT8_TYPE,
self.UINT16_TYPE, self.INT16_TYPE,
self.UINT32_TYPE, self.INT32_TYPE]
def is_input(self):
return self.is_command() or self.is_string_input() or self.is_number_input() or self.is_selection()
def create_param_read_frame(self, origin):
if self.chunks:
# Determine if any chunks are missing or need updating
if None in self.chunks:
chunk = self.chunks.index(None)
else:
self.debug("error 5")
return None
else:
chunk = 0
frame = create_param_read_frame(origin, self.num, chunk)
return frame
def create_write_frame(self, origin, value: bytearray):
if value is not None:
return create_param_write_frame(origin, self.num, value)
else:
self.debug('error: value is None')
def get_float_decimal(self):
return eval(('0.' + '0'*self.decimal_point + '1') if self.decimal_point else '1')
def render_float(self, val=None):
'''Convert integer int32 to float based on self.decimal_point'''
if val is None:
val = self.value
if self.type == self.FLOAT_TYPE:
return val * self.get_float_decimal()
else:
raise ValueError("not float")
def encode_float(self, val):
'''Convert float to int32 based on self.decimal_point'''
val = int(round(val / self.get_float_decimal()))
if val < self.min or val > self.max:
self.debug("error: {} out of range {}..{}".format(val, self.min, self.max))
return None
return val
def debug(self, txt):
if self.debug_cb:
self.debug_cb(txt)
def process_frame(self, frame):
'''Parse parameter information frame'''
if frame.type != CRSF.MSG_TYPE_PARAM_ENTRY:
self.debug('invalid frame type')
return
payload = frame.payload
param_num, chunks_remain = payload[:2]
# Handle chunking
if chunks_remain:
# More chunks expected.
if not self.chunks:
# Assumption! Received first chunk
self.chunks = [None]*(chunks_remain + 1)
self.chunks[-chunks_remain - 1] = frame.payload
return
elif self.chunks:
# Last chunk received. Reassemble payload from chunks.
self.chunks[-1] = frame.payload
if None not in self.chunks and \
list(reversed([x[1] for x in self.chunks])) == list(range(len(self.chunks))) and \
len([1 for x in self.chunks if x[0] == self.num]) == len(self.chunks):
payload = self.chunks[0][:2]
for x in self.chunks:
payload += x[2:]
else:
self.debug("error: chunks error")
return
self.chunks = []
else:
# Trivial case: no chunking.
# CRSF frame contains the parameter entry entirely.
pass
parent_folder, data_type = payload[2:4]
hidden, data_type = data_type & 0x80, data_type & 0x7F
try:
nul = 4 + payload[4:].index(0x00)
name = bytes(payload[4:nul]).decode()
except:
# Invalid frame?
self.debug('frame error: ' + str(frame))
return
# Type-specific value
if data_type == self.FOLDER_TYPE:
try:
end = nul + 1 + payload[nul+1:].index(0xFF)
children = list(payload[nul+1:end])
except:
self.debug('frame error 2: ' + str(frame))
return
# Folder frame parsed successfully
if chunks_remain == 0:
self.children = children
self.parent_folder = parent_folder
self.type = data_type
self.name = name
self.hidden = hidden
self.debug("folder %d OK" % param_num)
self.obtained_time = time.time()
else:
self.debug("error: folder %d: multichunk folders not supported" % param_num)
return
elif data_type == self.COMMAND_TYPE:
try:
end = nul + 3 + payload[nul+3:].index(0x00)
info = bytes(payload[nul+3:end]).decode()
except:
self.debug('frame error 7: ' + str(frame))
return
if chunks_remain == 0:
self.parent_folder = parent_folder
self.type = data_type
self.name = name
self.hidden = hidden
self.status = payload[nul + 1]
self.timeout = payload[nul + 2]*0.1 if payload[nul + 2] % 10 else payload[nul + 2]//10
self.info = info
self.debug("command %d OK" % param_num)
self.obtained_time = time.time()
else:
self.debug("error: folder %d: multichunk folders not supported" % param_num)
return
elif data_type == self.FLOAT_TYPE:
try:
end = nul + 22 + payload[nul+22:].index(0x00)
unit = bytes(payload[nul+22:end]).decode()
except:
self.debug('frame error 5: ' + str(frame))
return
if chunks_remain == 0:
self.parent_folder = parent_folder
self.type = data_type
self.hidden = hidden
self.name = name
self.value = CRSF.decode_int32(payload[nul+1:nul+5])
self.min = CRSF.decode_int32(payload[nul+5:nul+9])
self.max = CRSF.decode_int32(payload[nul+9:nul+13])
self.default = CRSF.decode_int32(payload[nul+13:nul+17])
self.decimal_point = payload[nul+17]
self.step_size = CRSF.decode_int32(payload[nul+18:nul+22])
self.unit = unit
self.debug("float {} = {}, {}, {} OK".format(param_num, self.value, self.max, self.unit))
self.obtained_time = time.time()
else:
self.debug("error: float %d: multichunk floats not supported" % param_num)
return
elif data_type in [self.STRING_TYPE, self.INFO_TYPE]:
try:
end = nul + 1 + payload[nul+1:].index(0x00)
value = bytes(payload[nul+1:end]).decode()
except:
self.debug('frame error 4: ' + str(frame))
return
self.max_length = payload[end + 1] if data_type == self.STRING_TYPE else len(value)
if chunks_remain == 0:
self.parent_folder = parent_folder
self.type = data_type
self.name = name
self.hidden = hidden
self.value = value
self.debug("{} {} {} OK".format('info' if data_type == self.INFO_TYPE else 'string',
param_num, self.max_length))
self.obtained_time = time.time()
else:
self.debug("error: string/info %d: multichunk strings not supported" % param_num)
return
elif data_type == self.TEXT_SELECTION_TYPE:
try:
end = nul + 1 + payload[nul+1:].index(0x00)
options = bytes(payload[nul+1:end]).decode()
options = options.split(';')
except:
self.debug('frame error 6: ' + str(frame))
return
value, val_min, val_max, val_def = payload[end+1:end+5]
# TODO: unit
if chunks_remain == 0:
self.parent_folder = parent_folder
self.type = data_type
self.name = name
self.hidden = hidden
self.value = value
self.options = options
self.min = val_min
self.default = val_def
self.max = val_max
self.debug("selection %d OK" % (val_max + 1))
self.obtained_time = time.time()
else:
self.debug("error: folder %d: multichunk folders not supported" % param_num)
return
else:
self.debug("error: data_type %d" % data_type)
self.parent_folder = parent_folder
self.type = data_type
self.name = name
self.type = data_type
self.hidden = hidden
if chunks_remain == 0:
self.obtained_time = time.time()
class CRSFDevice:
'''Holds all the information obtained from a device'''
POLL_PERIOD_S = 2.0
POLL_RESPONSE_SPEED_UP_FACTOR = 0.95
FOLDER_PERIOD_S = 10
PARAM_TIMEOUT_S = 120
class InvalidType(ValueError):
pass
def __init__(self, frame, menu_ui, debug_cb=None):
self.origin = 0 # network address of this device
self.last_seen = 0 # when was the last time "device information" was obtained from the device?
self.last_read_time = 0 # when was the last time "param read" was sent to the device?
self.last_read_frame = 0
# Parsed from "device information" frame
self.name = 0x00
self.sn = 0x00
self.hwid = 0x00
self.fwid = 0x00
self.param_count = 0
self.param_version = 0
if frame and frame.type == CRSF.MSG_TYPE_DEVICE_INFO:
self.name, self.sn, self.hwid, self.fwid, self.param_count, self.param_ver = frame.parse()
self.origin = frame.origin
elif frame:
raise ValueError("frame of wrong type")
# List of CRSFParam objects, first object - root folder
self.menu = [None]*(self.param_count + 1)
self.menu_ui = menu_ui
self.debug_cb = debug_cb
def debug(self, txt):
if self.debug_cb:
self.debug_cb(txt)
def match(self, frame):
'''Compare device information of this device to "device information" frame'''
if frame and frame.type == CRSF.MSG_TYPE_DEVICE_INFO:
return frame.origin == self.origin and \
frame.parse() == \
(self.name, self.sn, self.hwid, self.fwid, self.param_count, self.param_ver)
else:
raise ValueError("DEVICE_INFO frame expected")
def poll_params(self, conn, folder):
'''Periodically send out "param read" frames for the parameters in current folder'''
if not (0 <= folder < len(self.menu)):
self.debug('invalid folder: not 0<=%s<%d' % (str(folder), len(self.menu)))
return
if time.time() - self.last_read_time < self.POLL_PERIOD_S:
return
self.last_read_time = time.time()
if self.menu[folder] is None:
self.menu[folder] = CRSFParam(folder, debug_cb=self.debug_cb)
elif self.menu[folder].obtained_time and not self.menu[folder].is_folder():
raise CRSFDevice.InvalidType('folder type expected')
# Decide which parameter to load
frame = None
if not self.menu[folder].obtained_time or time.time() - self.menu[folder].obtained_time > self.FOLDER_PERIOD_S:
# Load the current folder itself
frame = self.menu[folder].create_param_read_frame(self.origin)
elif self.menu[folder].obtained_time:
oldest_time, oldest_child = None, None
for child in self.menu[folder].children:
if self.menu[child] is None or not self.menu[child].obtained_time and \
time.time() - self.menu[child].created_time > self.PARAM_TIMEOUT_S:
# Create or recreate parameter to start obtaining it from beginning
self.menu[child] = CRSFParam(child, debug_cb=self.debug_cb)
frame = self.menu[child].create_param_read_frame(self.origin)
break
elif None in self.menu[child].chunks:
frame = self.menu[child].create_param_read_frame(self.origin)
break
elif oldest_time is None or time.time() - self.menu[child].obtained_time > oldest_time:
oldest_time = time.time() - self.menu[child].obtained_time
oldest_child = child
if frame is None and oldest_child is not None:
frame = self.menu[oldest_child].create_param_read_frame(self.origin)
# TODO: determine the parameter in the current folder which was not updated in the longest time
if frame:
# TODO: replace this with two callbacks: menu_id.write_crsf and menu_ui.debug
self.menu_ui.write_crsf(frame)
self.last_read_frame = frame.payload[0]
def process_frame(self, frame):
'''Process frame from this device'''
if frame.type == CRSF.MSG_TYPE_PARAM_ENTRY:
# TODO: process "param entry" frame
param_num = frame.payload[0]
if self.last_read_frame == param_num:
self.last_read_time -= self.POLL_PERIOD_S * self.POLL_RESPONSE_SPEED_UP_FACTOR
if 0 <= param_num < len(self.menu):
if isinstance(self.menu[param_num], CRSFParam):
self.menu[param_num].process_frame(frame)
else:
# TODO: report error: unexpected parameter number