Added GPS Telemetry packet support. Added JSON logging to UBlox lib.

rx/WenetPackets.py

@@ -4,6 +4,7 @@
 #
 import struct
 import traceback
+import datetime
 
 WENET_IMAGE_UDP_PORT 		= 7890
 WENET_TELEMETRY_UDP_PORT 	= 7891
@@ -12,12 +13,15 @@ WENET_TELEMETRY_UDP_PORT 	= 7891
 
 class WENET_PACKET_TYPES:
 	TEXT_MESSAGE			= 0x00
-	TELEMETRY 				= 0x01
+	GPS_TELEMETRY 			= 0x01
+	IMU_TELEMETRY			= 0x02
 	# Your packet types here!
+	IMAGE_TELEMETRY			= 0x54
 	SSDV 					= 0x55
 	IDLE					= 0x56
 
-
+class WENET_PACKET_LENGTHS:
+	GPS_TELEMETRY 		= 35
 
 def decode_packet_type(packet):
 	# Convert packet to a list of integers before parsing.
@@ -30,8 +34,12 @@ def packet_to_string(packet):
 
 	if packet_type == WENET_PACKET_TYPES.TEXT_MESSAGE:
 		return text_message_string(packet)
-	elif packet_type == WENET_PACKET_TYPES.TELEMETRY:
+	elif packet_type == WENET_PACKET_TYPES.GPS_TELEMETRY:
-		return telemetry_string(packet)
+		return gps_telemetry_string(packet)
+	elif packet_type == WENET_PACKET_TYPES.IMU_TELEMETRY:
+		return imu_telemetry_string(packet)
+	elif packet_type == WENET_PACKET_TYPES.IMAGE_TELEMETRY:
+		return image_telemetry_string(packet)
 	elif packet_type == WENET_PACKET_TYPES.SSDV:
 		return ssdv_packet_string(packet)
 	else:
@@ -116,7 +124,136 @@ def text_message_string(packet):
 		return "Text Message #%d: \t%s" % (message['id'],message['text'])
 
 #
-# GPS/IMU Telemetry
+# GPS Telemetry Decoder
 #
-def telemetry_string(packet):
+# Refer Telemetry format documented in:
+# https://docs.google.com/document/d/12230J1X3r2-IcLVLkeaVmIXqFeo3uheurFakElIaPVo/edit?usp=sharing
+#
+# The above 
+
+def gps_weeksecondstoutc(gpsweek, gpsseconds, leapseconds):
+    """ Convert time in GPS time (GPS Week, seconds-of-week) to a UTC timestamp """
+    epoch = datetime.datetime.strptime("1980-01-06 00:00:00","%Y-%m-%d %H:%M:%S")
+    elapsed = datetime.timedelta(days=(gpsweek*7),seconds=(gpsseconds+leapseconds))
+    timestamp = epoch + elapsed
+    return timestamp.isoformat()
+
+def gps_telemetry_decoder(packet):
+	""" Extract GPS telemetry data from a packet, and return it as a python dictionary. """
+	# We need the packet as a string, convert to a string in case we were passed a list of bytes.
+	packet = str(bytearray(packet))
+	gps_data = {}
+
+	# Some basic sanity checking of the packet before we attempt to decode.
+	if len(packet) < WENET_PACKET_LENGTHS.GPS_TELEMETRY:
+		return {'error': 'GPS Telemetry Packet has invalid length.'}
+	elif len(packet) > WENET_PACKET_LENGTHS.GPS_TELEMETRY:
+		# If the packet is too big (which it will be, as it's padded with 0x55's), clip it. 
+		packet = packet[:WENET_PACKET_LENGTHS.GPS_TELEMETRY]
+	else:
+		pass
+
+	# Wrap the next bit in exception handling.
+	try:
+		# Unpack the packet into a list.
+		data = struct.unpack('>BHIBffffffBBB', packet)
+
+		gps_data['week'] 	= data[1]
+		gps_data['iTOW'] 	= data[2]/1000.0 # iTOW provided as milliseconds, convert to seconds.
+		gps_data['leapS'] 	= data[3]
+		gps_data['latitude'] = data[4]
+		gps_data['longitude'] = data[5]
+		gps_data['altitude'] = data[6]
+		gps_data['ground_speed'] = data[7]
+		gps_data['heading'] = data[8]
+		gps_data['ascent_rate'] = data[9]
+		gps_data['numSV'] 	= data[10]
+		gps_data['gpsFix']	= data[11]
+		gps_data['dynamic_model'] = data[12]
+
+		# Perform some post-processing on the data, to make some of the fields easier to read.
+
+		# Produce a human-readable timestamp, in UTC time.
+		gps_data['timestamp'] = gps_weeksecondstoutc(gps_data['week'], gps_data['iTOW'], gps_data['leapS'])
+
+		# Produce a human-readable indication of GPS Fix state.
+		if gps_data['gpsFix'] == 0:
+			gps_data['gpsFix_str'] = 'No Fix'
+		elif gps_data['gpsFix'] == 2:
+			gps_data['gpsFix_str'] = '2D Fix'
+		elif gps_data['gpsFix'] == 3:
+			gps_data['gpsFix_str'] = '3D Fix'
+		elif gps_data['gpsFix'] == 5:
+			gps_data['gpsFix_str'] = 'Time Only'
+		else:
+			gps_data['gpsFix_str'] = 'Unknown (%d)' % gps_data['gpsFix']
+
+		# Produce a human-readable indication of the current dynamic model.
+		if gps_data['dynamic_model'] == 0:
+			gps_data['dynamic_model_str'] = 'Portable'
+		elif gps_data['dynamic_model'] == 1:
+			gps_data['dynamic_model_str'] = 'Not Used'
+		elif gps_data['dynamic_model'] == 2:
+			gps_data['dynamic_model_str'] = 'Stationary'
+		elif gps_data['dynamic_model'] == 3:
+			gps_data['dynamic_model_str'] = 'Pedestrian'
+		elif gps_data['dynamic_model'] == 4:
+			gps_data['dynamic_model_str'] = 'Automotive'
+		elif gps_data['dynamic_model'] == 5:
+			gps_data['dynamic_model_str'] = 'Sea'
+		elif gps_data['dynamic_model'] == 6:
+			gps_data['dynamic_model_str'] = 'Airborne 1G'
+		elif gps_data['dynamic_model'] == 7:
+			gps_data['dynamic_model_str'] = 'Airborne 2G'
+		elif gps_data['dynamic_model'] == 8:
+			gps_data['dynamic_model_str'] = 'Airborne 4G'
+		else:
+			gps_data['dynamic_model_str'] = 'Unknown'
+
+		gps_data['error'] = 'None'
+
+		return gps_data
+
+	except:
+		traceback.print_exc()
+		print(packet)
+		return {'error': 'Could not decode GPS telemetry packet.'}
+
+
+def gps_telemetry_string(packet):
+	gps_data = gps_telemetry_decoder(packet)
+	if gps_data['error'] != 'None':
+		return "GPS: ERROR Could not decode."
+	else:
+		gps_data_string = "GPS: %s Lat/Lon: %.5f,%.5f Alt: %dm, Speed: H %dkph V %.1fm/s, Heading: %d deg, Fix: %s, SVs: %d, Model: %s " % (
+			gps_data['timestamp'],
+			gps_data['latitude'],
+			gps_data['longitude'],
+			int(gps_data['altitude']),
+			int(gps_data['ground_speed']),
+			gps_data['ascent_rate'],
+			int(gps_data['heading']),
+			gps_data['gpsFix_str'],
+			gps_data['numSV'],
+			gps_data['dynamic_model_str']
+			)
+		return gps_data_string
+
+#
+# IMU Telemetry Decoder
+#
+def imu_telemetry_decoder(packet):
+	return "Not Implemented."
+
+def imu_telemetry_string(packet):
+	return "Not Implemented."
+
+
+#
+# Image (Combined GPS/IMU) Telemetry Decoder
+#
+def image_telemetry_decoder(packet):
+	return "Not Implemented."
+
+def image_telemetry_string(packet):
 	return "Not Implemented."

rx/rx_ssdv.py

@@ -92,7 +92,7 @@ while True:
 			print("Text Message #%d: \t%s" % (message['id'],message['text']))
 			current_text_message = message['id']
 
-	elif packet_type == WENET_PACKET_TYPES.TELEMETRY:
+	elif packet_type == WENET_PACKET_TYPES.GPS_TELEMETRY:
 		broadcast_telemetry_packet(data)
 		print("Telemetry Packet: Not Implemented yet.")
 

rx/telemetry_console.py

@@ -8,6 +8,7 @@
 import json
 import socket
 import datetime
+import traceback
 from WenetPackets import *
 
 def process_udp(udp_packet):
@@ -48,6 +49,7 @@ def udp_rx_thread():
             try:
                 process_udp(m[0])
             except:
+                traceback.print_exc()
                 pass
     
     print("Closing UDP Listener")

tx/PacketTX.py

@@ -20,6 +20,7 @@ import sys
 import os
 import crcmod
 import struct
+import traceback
 from time import sleep
 from threading import Thread
 import numpy as np
@@ -165,10 +166,8 @@ class PacketTX(object):
 
 	def queue_image_file(self, filename):
 		""" Read in <filename> and transmit it, 256 bytes at a time.
-			Intended for transmitting SSDV packets.
+			Intended for transmitting SSDV images.
-
 		"""
-
 		file_size = os.path.getsize(filename)
 		try:
 			f = open(filename,'rb')
@@ -180,7 +179,6 @@ class PacketTX(object):
 		except:
 			return False
 
-
 	def image_queue_empty(self):
 		return self.ssdv_queue.qsize() == 0
 
@@ -191,7 +189,22 @@ class PacketTX(object):
 	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.
@@ -203,6 +216,42 @@ class PacketTX(object):
 		self.queue_telemetry_packet(packet, repeats=repeats)
 		print("TXing Text Message #%d: %s" % (self.text_message_count,message))
 
+	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()
 
 
 class BinaryDebug(object):

tx/tx_picam_gps.py

@@ -39,27 +39,30 @@ time.sleep(1)
 # Initialise a couple of global variables.
 max_altitude = 0
 
+def handle_gps_data(gps_data):
+	""" Handle GPS data passed to us from the UBloxGPS instance """
+	global max_altitude, tx
+
+	# Immediately transmit a GPS packet.
+	tx.transmit_gps_telemetry(gps_data)
+
+	if gps_data['altitude'] > max_altitude:
+		max_altitude = gps_data['altitude']
+
+
 # Try and start up the GPS rx thread.
 try:
 	gps = ublox.UBloxGPS(port=args.gps, 
 		dynamic_model = ublox.DYNAMIC_MODEL_AIRBORNE1G, 
-		update_rate_ms = 500,
+		update_rate_ms = 1000,
-		debug_ptr = tx.transmit_text_message)
+		debug_ptr = tx.transmit_text_message,
+		callback = handle_gps_data,
+		log_file = 'gps_data.log'
+		)
 except Exception as e:
 	tx.transmit_text_message("ERROR: Could not Open GPS - %s" % str(e), repeats=5)
 	gps = None
 
-
-def poll_gps():
-	global gps, max_altitude
-
-	if gps != None:
-		gps_state = gps.read_state()
-
-		if gps_state['altitude'] > max_altitude:
-			max_altitude = gps_state['altitude']
-
-
 # Define our post-processing callback function, which gets called by WenetPiCam
 # after an image has been captured.
 def post_process_image(filename):
@@ -115,7 +118,6 @@ picam.run(destination_directory="./tx_images/",
 # Main 'loop'.
 try:
 	while True:
-		poll_gps() # Keep polling the GPS so we keep track of our maximum altitude.
 		time.sleep(1)
 except KeyboardInterrupt:
 	print("Closing")

tx/ublox.py

@@ -12,7 +12,7 @@ Added UBloxGPS abstraction layer class for use with Wenet TX system.
 import struct
 import datetime
 from threading import Thread
-import time, os
+import time, os, json
 
 # protocol constants
 PREAMBLE1 = 0xb5
@@ -917,6 +917,7 @@ class UBloxGPS(object):
         'timestamp':    " ",    # ISO-8601 Compliant Date-code (generate by Python's datetime.isoformat() function)
         'dynamic_model': 0      # Current dynamic model in use.
     }
+    # Lock files for writing and reading to the internal state dictionary.
     state_writelock = False
     state_readlock = False
 
@@ -924,7 +925,8 @@ class UBloxGPS(object):
             callback=None,
             update_rate_ms=500,
             dynamic_model=DYNAMIC_MODEL_AIRBORNE1G,
-            debug_ptr = None):
+            debug_ptr = None,
+            log_file = None):
 
         """ Initialise a UBloxGPS Abstraction layer object.
         
@@ -944,6 +946,9 @@ class UBloxGPS(object):
                     In the wenet payload, we use this to link this object to the PacketTX object to be able to
                     transit debug messages to the ground.
 
+        log_file:   An optional filename in which to log GPS state data. Data will be stored as lines of JSON data.
+                    Data is written whenever the gps_callback function is called.
+
         """
 
         # Copy supplied values.
@@ -955,6 +960,13 @@ class UBloxGPS(object):
         self.debug_ptr = debug_ptr
         self.callback = callback
 
+
+        # Open log file, if one has been given.
+        if log_file != None:
+            self.log_file = open(log_file,'a')
+        else:
+            self.log_file = None
+
         # Attempt to inialise.
         self.gps = UBlox(self.port, self.baudrate, self.timeout)
         self.setup_ublox()
@@ -991,8 +1003,7 @@ class UBloxGPS(object):
         """ Write a debug message.
         If debug_ptr was set to a function during init, this will
         pass the message to that function, else it will just print it.
-        This is used mainly to get updates on image capture into the Wenet downlink.
+        This is used mainly to get error and other state updates into the Wenet downlink.
-
         """
         message = "GPS Debug: " + message
         if self.debug_ptr != None:
@@ -1000,6 +1011,7 @@ class UBloxGPS(object):
         else:
             print(message)
 
+    # Thread-safe read/write access into the internal state dictionary
     def write_state(self, value, parameter):
         """ (Hopefully) thread-safe state dictionary write access """
         while self.state_readlock:
@@ -1022,16 +1034,23 @@ class UBloxGPS(object):
 
         return state_copy
 
+    # Function called whenever we have a new GPS fix.
     def gps_callback(self):
         """ Pass the latest GPS state to an external callback function """
         # Grab latest state.
         latest_state = self.read_state()
 
+        # Write into the log file, if we are using one.
+        if self.log_file != None:
+            self.log_file.write(json.dumps(latest_state) + '\n')
+
+        # If we don't have a callback function to use, return immediately.
         if self.callback == None:
             return
+        else:
+            self.callback(latest_state)
 
-        self.callback(latest_state)
+    # Utility function to convert GPS time to UTC time.
-
     def weeksecondstoutc(self, gpsweek, gpsseconds, leapseconds):
         """ Convert time in GPS time (GPS Week, seconds-of-week) to a UTC timestamp """
         epoch = datetime.datetime.strptime("1980-01-06 00:00:00","%Y-%m-%d %H:%M:%S")
@@ -1111,6 +1130,7 @@ class UBloxGPS(object):
                     # A message with only 0x00 in the payload field is a poll.
                     self.gps.send_message(CLASS_CFG, MSG_CFG_NAV5,'\x00')
 
+                # Send data to the callback function.
                 callback_thread = Thread(target=self.gps_callback)
                 callback_thread.start()
 
@@ -1129,6 +1149,7 @@ class UBloxGPS(object):
         self.rx_running = False
         time.sleep(0.5)
         self.gps.close()
+        self.log_file.close()
 
 if __name__ == "__main__":
     """ Basic test script for the above UBloxGPS class.