kopia lustrzana https://github.com/projecthorus/wenet
Added GPS Telemetry packet support. Added JSON logging to UBlox lib.
Mark Jessop
rodzic
928a2fe8e4
commit
e6f4a7bff0
|
|
@ -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:
|
||||
return telemetry_string(packet)
|
||||
elif packet_type == WENET_PACKET_TYPES.GPS_TELEMETRY:
|
||||
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."
|
||||
|
|
@ -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.")
|
||||
|
||||
|
|
|
|||
|
|
@ -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")
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
debug_ptr = tx.transmit_text_message)
|
||||
update_rate_ms = 1000,
|
||||
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")
|
||||
|
|
|
|||
33
tx/ublox.py
33
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.
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue