kopia lustrzana https://github.com/projecthorus/wenet
409 wiersze
12 KiB
Python
409 wiersze
12 KiB
Python
#!/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 Queue
|
|
import sys
|
|
import os
|
|
import datetime
|
|
import crcmod
|
|
import struct
|
|
import traceback
|
|
from time import sleep
|
|
from threading import Thread
|
|
import numpy as np
|
|
from ldpc_encoder import *
|
|
|
|
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.Queue(4096) # Up to 1MB of 256 byte packets
|
|
telemetry_queue = Queue.Queue(16) # Keep this queue small. It's up to the user not to over-use this queue.
|
|
|
|
# Framing parameters
|
|
unique_word = "\xab\xcd\xef\x01"
|
|
preamble = "\x55"*16
|
|
|
|
# Idle sequence, transmitted if there is nothing in the transmit queues.
|
|
idle_sequence = "\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",
|
|
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
|
|
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
|
|
|
|
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 + "\x55"*(self.payload_length - len(packet))
|
|
|
|
crc = struct.pack("<H",self.crc16(packet))
|
|
|
|
if fec:
|
|
parity = ldpc_encode_string(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 = "\x00" + "DE %s: \t%s" % (self.callsign, message)
|
|
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 "\x00" + "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
|
|
|
|
|
|
# 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():
|
|
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 = "\x00" + struct.pack(">BH",len(message),self.text_message_count) + message
|
|
|
|
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,
|
|
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()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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()
|