-
Notifications
You must be signed in to change notification settings - Fork 10
/
PacketTX.py
559 lines (425 loc) · 19 KB
/
PacketTX.py
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
#!/usr/bin/env python2.7
#
# Wenet Packet Transmitter Class
#
# Copyright (C) 2018 Mark Jessop <vk5qi@rfhead.net>
# Released under GNU GPL v3 or later
#
# Frames packets (preamble, unique word, checksum)
# and transmits them out of a serial port.
#
#
# NOTE: The RPi UART isn't spot on with its baud rate.
# Recent firmware updates have improved the accuracy slightly, but it's still
# a bit off. Consequently, 115200 baud is actually around 115177 baud.
#
import serial
import sys
import os
import datetime
import crcmod
import json
import socket
import struct
import traceback
from time import sleep
from threading import Thread
import numpy as np
from ldpc_encoder import *
from queue import Queue
class PacketTX(object):
""" Packet Transmitter Class
The core of the Wenet transmitter stack.
This class handles framing, FEC, and transmission of packets via a
serial port.
Intended to be used with the David Rowe's fsk_demod software, with receiver
glue code available in the 'rx' directory of this repository.
Packet framing is as follows:
Preamble: 16 repeats of 0x55. May not be required, but helps with timing estimation on the demod.
Unique Word: 0xABCDEF01 Used for packet detection in the demod chain.
Packet: 256 bytes of arbitrary binary data.
Checksum: CRC16 checksum.
Parity bits: 516 bits (zero-padded to 65 bytes) of LDPC parity bits, using a r=0.8 Repeat-accumulate code, developed by
Bill Cowley, VK5DSP. See ldpc_enc.c for more details.
Packets are transmitted from two queues, named 'telemetry' and 'ssdv'.
The 'telemetry' queue is intended for immediate transmission of low-latency telemetry packets,
for example, GPS or IMU data. Care must be taken to not over-use this queue, at the detriment of image transmission.
The 'ssdv' queue is used for transmission of large amounts of image (SSDV) data, and up to 4096 packets can be queued for transmit.
"""
# Transmit Queues.
ssdv_queue = Queue(4096) # Up to 1MB of 256 byte packets
telemetry_queue = Queue(256) # Keep this queue small. It's up to the user not to over-use this queue.
# Framing parameters
unique_word = b"\xab\xcd\xef\x01"
preamble = b"\x55"*16
# Idle sequence, transmitted if there is nothing in the transmit queues.
idle_sequence = b"\x56"*256
# Transmit thread active flag.
transmit_active = False
# Internal counter for text messages.
text_message_count = 0
image_telem_count = 0
# WARNING: 115200 baud is ACTUALLY 115386.834 baud, as measured using a freq counter.
def __init__(self,
serial_port="/dev/ttyAMA0",
serial_baud=115200,
payload_length=256,
fec=True,
debug = False,
callsign="N0CALL",
udp_listener = None,
log_file = None):
# Instantiate our low-level transmit interface, be it a serial port, or the BinaryDebug class.
if debug == True:
self.s = BinaryDebug()
self.debug = True
else:
self.debug = False
self.s = serial.Serial(serial_port,serial_baud)
self.payload_length = payload_length
self.callsign = callsign.encode('ascii')
self.fec = fec
self.crc16 = crcmod.predefined.mkCrcFun('crc-ccitt-false')
self.idle_message = self.frame_packet(self.idle_sequence,fec=fec)
if log_file != None:
self.log_file = open(log_file,'a')
self.log_file.write("Started Transmitting at %s\n" % datetime.datetime.utcnow().isoformat())
else:
self.log_file = None
# Startup the UDP listener, if enabled.
self.listener_thread = None
self.udp = None
self.udp_listener_running = False
if udp_listener != None:
self.udp_port = udp_listener
self.start_udp()
def start_tx(self):
self.transmit_active = True
txthread = Thread(target=self.tx_thread)
txthread.start()
def frame_packet(self,packet, fec=False):
# Ensure payload size is equal to the desired payload length
if len(packet) > self.payload_length:
packet = packet[:self.payload_length]
if len(packet) < self.payload_length:
packet = packet + b"\x55"*(self.payload_length - len(packet))
crc = struct.pack("<H",self.crc16(packet))
if fec:
parity = ldpc_encode(packet + crc)
return self.preamble + self.unique_word + packet + crc + parity
else:
return self.preamble + self.unique_word + packet + crc
def set_idle_message(self, message):
temp_msg = b"\x00" + b"DE %s: \t%s" % (self.callsign, message.encode('ascii'))
self.idle_message = self.frame_packet(temp_msg,fec=self.fec)
def generate_idle_message(self):
# Append a \x00 control code before the data
return b"\x00" + b"DE %s: \t%s" % (self.callsign,self.idle_message)
def tx_thread(self):
""" Main Transmit Thread.
Checks telemetry and image queues in order, and transmits a packet.
"""
while self.transmit_active:
if self.telemetry_queue.qsize()>0:
packet = self.telemetry_queue.get_nowait()
self.s.write(packet)
elif self.ssdv_queue.qsize()>0:
packet = self.ssdv_queue.get_nowait()
self.s.write(packet)
else:
if not self.debug:
self.s.write(self.idle_message)
else:
# TODO: Tune this value.
sleep(0.05)
print("Closing Thread")
self.s.close()
def close(self):
self.transmit_active = False
self.udp_listener_running = False
#self.listener_thread.join()
# Deprecated function
def tx_packet(self,packet,blocking = False):
self.ssdv_queue.put(self.frame_packet(packet, self.fec))
if blocking:
while not self.ssdv_queue.empty():
sleep(0.01)
# Deprecated function.
def wait(self):
while (not self.ssdv_queue.empty()) and self.transmit_active:
sleep(0.01)
# New packet queueing and queue querying functions (say that 3 times fast)
def queue_image_packet(self,packet):
self.ssdv_queue.put(self.frame_packet(packet, self.fec))
def queue_image_file(self, filename):
""" Read in <filename> and transmit it, 256 bytes at a time.
Intended for transmitting SSDV images.
"""
file_size = os.path.getsize(filename)
try:
f = open(filename,'rb')
for x in range(file_size//256):
data = f.read(256)
self.queue_image_packet(data)
f.close()
return True
except:
return False
def image_queue_empty(self):
return self.ssdv_queue.qsize() == 0
def queue_telemetry_packet(self, packet, repeats = 1):
for n in range(repeats):
self.telemetry_queue.put(self.frame_packet(packet, self.fec))
def telemetry_queue_empty(self):
return self.telemetry_queue.qsize() == 0
#
# Various Telemetry Packet Generation functions
#
def transmit_text_message(self,message, repeats = 1):
""" Generate and Transmit a Text Message Packet
Keyword Arguments:
message: A string, up to 252 characters long, to transmit.
repeats: An optional field, defining the number of time to
transmit the packet. Can be used to increase chances
of receiving the packet, at the expense of higher
channel usage.
"""
# Increment text message counter.
self.text_message_count = (self.text_message_count+1)%65536
# Clip message if required.
if len(message) > 252:
message = message[:252]
packet = b"\x00" + struct.pack(">BH",len(message),self.text_message_count) + message.encode('ascii')
self.queue_telemetry_packet(packet, repeats=repeats)
log_string = "TXing Text Message #%d: %s" % (self.text_message_count,message)
if self.log_file != None:
self.log_file.write(datetime.datetime.now().isoformat() + "," + log_string + "\n")
print(log_string)
def transmit_gps_telemetry(self, gps_data):
""" Generate and Transmit a GPS Telemetry Packet.
Keyword Arguments:
gps_data: A dictionary, as produced by the UBloxGPS class. It must have the following fields:
latitude, longitude, altitude, ground_speed, ascent_rate, heading, gpsFix, numSV,
week, iTOW, leapS, dynamic_model.
The generated packet format is in accordance with the specification in:
https://docs.google.com/document/d/12230J1X3r2-IcLVLkeaVmIXqFeo3uheurFakElIaPVo/edit?usp=sharing
The corresponding decoder for this packet format is within rx/WenetPackets.py, in the function
gps_telemetry_decoder
"""
try:
gps_packet = struct.pack(">BHIBffffffBBB",
1, # Packet ID for the GPS Telemetry Packet.
gps_data['week'],
int(gps_data['iTOW']*1000), # Convert the GPS week value to milliseconds, and cast to an int.
gps_data['leapS'],
gps_data['latitude'],
gps_data['longitude'],
gps_data['altitude'],
gps_data['ground_speed'],
gps_data['heading'],
gps_data['ascent_rate'],
gps_data['numSV'],
gps_data['gpsFix'],
gps_data['dynamic_model']
)
self.queue_telemetry_packet(gps_packet)
except:
traceback.print_exc()
def transmit_orientation_telemetry(self, week, iTOW, leapS, orientation_data):
""" Generate and Transmit an Payload Orientation telemetry packet.
Keyword Arguments:
week: GPS week number
iTOW: GPS time-of-week (Seconds)
leapS: GPS leap-seconds value (necessary to convert GPS time to UTC time)
orientation_data: A dictionary, as produced by the BNO055 Class. It must have the following fields:
The generated packet format is in accordance with the specification in
https://docs.google.com/document/d/12230J1X3r2-IcLVLkeaVmIXqFeo3uheurFakElIaPVo/edit?usp=sharing
The corresponding decoder for this packet format is within rx/WenetPackets.py, in the function
orientation_telemetry_decoder
"""
try:
orientation_packet = struct.pack(">BHIBBBBBBBbfffffff",
2, # Packet ID for the Orientation Telemetry Packet.
week,
int(iTOW*1000), # Convert the GPS week value to milliseconds, and cast to an int.
leapS,
orientation_data['sys_status'],
orientation_data['sys_error'],
orientation_data['sys_cal'],
orientation_data['gyro_cal'],
orientation_data['accel_cal'],
orientation_data['magnet_cal'],
orientation_data['temp'],
orientation_data['euler_heading'],
orientation_data['euler_roll'],
orientation_data['euler_pitch'],
orientation_data['quaternion_x'],
orientation_data['quaternion_y'],
orientation_data['quaternion_z'],
orientation_data['quaternion_w']
)
self.queue_telemetry_packet(orientation_packet)
except:
traceback.print_exc()
return
def transmit_image_telemetry(self, gps_data, orientation_data, image_id=0, callsign='N0CALL', repeats=1):
""" Generate and Transmit an Image telemetry packet.
Keyword Arguments:
gps_data: A dictionary, as produced by the UBloxGPS class. It must have the following fields:
latitude, longitude, altitude, ground_speed, ascent_rate, heading, gpsFix, numSV,
week, iTOW, leapS, dynamic_model.
orientation_data: A dictionary, as produced by the BNO055 Class. It must have the following fields:
image_id: The ID of the image related to the above position and orientation data.
The generated packet format is in accordance with the specification in
https://docs.google.com/document/d/12230J1X3r2-IcLVLkeaVmIXqFeo3uheurFakElIaPVo/edit?usp=sharing
The corresponding decoder for this packet format is within rx/WenetPackets.py, in the function
image_telemetry_decoder
"""
self.image_telem_count = (self.image_telem_count+1)%65536
try:
image_packet = struct.pack(">BH7pBHIBffffffBBBBBBBBBbfffffff",
0x54, # Packet ID for the GPS Telemetry Packet.
self.image_telem_count,
callsign.encode(),
image_id,
gps_data['week'],
int(gps_data['iTOW']*1000), # Convert the GPS week value to milliseconds, and cast to an int.
gps_data['leapS'],
gps_data['latitude'],
gps_data['longitude'],
gps_data['altitude'],
gps_data['ground_speed'],
gps_data['heading'],
gps_data['ascent_rate'],
gps_data['numSV'],
gps_data['gpsFix'],
gps_data['dynamic_model'],
orientation_data['sys_status'],
orientation_data['sys_error'],
orientation_data['sys_cal'],
orientation_data['gyro_cal'],
orientation_data['accel_cal'],
orientation_data['magnet_cal'],
orientation_data['temp'],
orientation_data['euler_heading'],
orientation_data['euler_roll'],
orientation_data['euler_pitch'],
orientation_data['quaternion_x'],
orientation_data['quaternion_y'],
orientation_data['quaternion_z'],
orientation_data['quaternion_w']
)
self.queue_telemetry_packet(image_packet, repeats=repeats)
except:
traceback.print_exc()
def transmit_secondary_payload_packet(self, id=1, data=[], repeats=1):
""" Generate and transmit a packet supplied by a 'secondary' payload.
These will usually be provided via a UDP messaging system, described in the functions
further below.
Keyword Arguments:
id (int): A payload ID number, 0-255.
data (list): The payload contents, as a list of integers. Maximum of 254 bytes.
repeats (int): (Optional) The number of times to transmit this packet.
"""
# Clip the id to 0-255.
_id = int(id) % 256
# Convert the provided data to a string
_data = bytes(bytearray(data))
# Clip to 254 bytes.
if len(_data) > 254:
_data = _data[:254]
_packet = b"\x03" + struct.pack(">B",_id) + _data
self.queue_telemetry_packet(_packet, repeats=repeats)
#
# UDP messaging functions.
#
def handle_udp_packet(self, packet):
''' Process a received UDP packet '''
try:
packet_dict = json.loads(packet.decode())
if packet_dict['type'] == 'WENET_TX_TEXT':
# Transmit an arbitrary text packet.
# We assume the data is a string.
self.transmit_text_message(packet_dict['packet'])
elif packet_dict['type'] == 'WENET_TX_SEC_PAYLOAD':
# This is a 'secondary' payload packet. It needs to have a 'id' field,
# and a 'data' field which contains the packet contents, provided as a *list of integers*.
# The user can optionally provide a 'repeats' integer, which defines the number of times
# to repeat transmission of the packet.
_id = int(packet_dict['id'])
if 'repeats' in packet_dict:
_repeats = int(packet_dict['repeats'])
else:
_repeats = 1
self.transmit_secondary_payload_packet(id=_id, data=packet_dict['packet'], repeats=_repeats)
else:
pass
except Exception as e:
print("Could not parse packet: %s" % str(e))
traceback.print_exc()
def udp_rx_thread(self):
''' Listen for Broadcast UDP packets '''
self.udp = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
self.udp.settimeout(1)
self.udp.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
try:
self.udp.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
except:
pass
self.udp.bind(('',self.udp_port))
print("Started UDP Listener Thread.")
self.udp_listener_running = True
while self.udp_listener_running:
try:
m = self.udp.recvfrom(4096)
except socket.timeout:
m = None
except:
traceback.print_exc()
if m != None:
self.handle_udp_packet(m[0])
print("Closing UDP Listener")
self.udp.close()
def start_udp(self):
if self.listener_thread is None:
self.listener_thread = Thread(target=self.udp_rx_thread)
self.listener_thread.start()
class BinaryDebug(object):
""" Debug binary 'transmitter' Class
Used to write packet data to a file in one-bit-per-char (i.e. 0 = 0x00, 1 = 0x01)
format for use with codec2-dev's fsk modulator.
Useful for debugging, that's about it.
"""
def __init__(self):
self.f = open("debug.bin",'wb')
def write(self,data):
# TODO: Add in RS232 framing
raw_data = np.array([],dtype=np.uint8)
for d in data:
d_array = np.unpackbits(np.fromstring(d,dtype=np.uint8))
raw_data = np.concatenate((raw_data,[0],d_array[::-1],[1]))
self.f.write(raw_data.astype(np.uint8).tostring())
def close(self):
self.f.close()
if __name__ == "__main__":
""" Test script, which transmits a text message repeatedly. """
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("--txport", default="/dev/ttyAMA0", type=str, help="Transmitter serial port. Defaults to /dev/ttyAMA0")
parser.add_argument("--baudrate", default=115200, type=int, help="Transmitter baud rate. Defaults to 115200 baud.")
args = parser.parse_args()
debug_output = False # If True, packet bits are saved to debug.bin as one char per bit.
tx = PacketTX(
debug=debug_output,
serial_port=args.txport,
serial_baud=args.baudrate,
udp_listener=55674)
tx.start_tx()
try:
while True:
# Transmit a text message.
tx.transmit_text_message("This is a test!")
time.sleep(1)
except KeyboardInterrupt:
tx.close()
print("Closing")