def test_gnss_nav_msg_offline(ubx_offline_config):
"""Parse navigation message of recorded .ubx file with ubx-rxm-sfrbx messages."""
logger = getLogger()
logger.info(f"Reading record-file: {ubx_offline_config.record_file}")
record_path = os.path.join(ubx_offline_config.folder,
ubx_offline_config.record_file)
idx = 0
cond = True
assert os.path.isfile(
record_path), f"File: {record_path} is no valid file."
ubx_nav_msg_parser = UbxNavMessageParserHandle()
with open(record_path, "rb") as rec: # read log-file
while cond:
try:
idx += 1
ubr = UBXReader(stream=rec, ubxonly=False, decodenavdata=False)
try:
(_, parsed_data) = ubr.read()
except UBXParseError as e:
warnings.warn(str(e))
continue
if not parsed_data:
break
elif parsed_data.identity == "RXM-SFRBX":
# print_ubx_rxm_sfrbx(ubx_msg=parsed_data)
ubx_nav_msg_parser.add_subframe(ubx_msg=parsed_data)
except UBXMessageError as e:
logger.warning(f"UBXMessage Error: {e}")
except StopIteration as _:
logger.info(f"Iterator exhausted.")
cond = False
for gnss_id in ubx_nav_msg_parser.gnss_msgs.keys():
for sv_id in ubx_nav_msg_parser.gnss_msgs[gnss_id].keys():
for nav_msg_id, gnss_msg in ubx_nav_msg_parser.gnss_msgs[gnss_id][
sv_id].items():
print(
f"{gnss_id} - {sv_id} - {nav_msg_id}: {gnss_msg.ephemeris}\n{gnss_msg.almanac}"
)
pass
class UBXStreamer:
"""
UBXStreamer class.
"""
def __init__(self, port, baudrate, timeout=5):
"""
Constructor.
"""
self._serial_object = None
self._serial_thread = None
self._ubxreader = None
self._connected = False
self._reading = False
self._port = port
self._baudrate = baudrate
self._timeout = timeout
def __del__(self):
"""
Destructor.
"""
self.stop_read_thread()
self.disconnect()
def connect(self):
"""
Open serial connection.
"""
try:
self._serial_object = Serial(
self._port, self._baudrate, timeout=self._timeout
)
self._ubxreader = UBXReader(BufferedReader(self._serial_object), False)
self._connected = True
except (SerialException, SerialTimeoutException) as err:
print(f"Error connecting to serial port {err}")
def disconnect(self):
"""
Close serial connection.
"""
if self._connected and self._serial_object:
try:
self._serial_object.close()
except (SerialException, SerialTimeoutException) as err:
print(f"Error disconnecting from serial port {err}")
self._connected = False
def start_read_thread(self):
"""
Start the serial reader thread.
"""
if self._connected:
self._reading = True
self._serial_thread = Thread(target=self._read_thread, daemon=True)
self._serial_thread.start()
def stop_read_thread(self):
"""
Stop the serial reader thread.
"""
if self._serial_thread is not None:
self._reading = False
def send(self, data):
"""
Send data to serial connection.
"""
self._serial_object.write(data)
def flush(self):
"""
Flush input buffer
"""
self._serial_object.reset_input_buffer()
def waiting(self):
"""
Check if any messages remaining in the input buffer
"""
return self._serial_object.in_waiting
def _read_thread(self):
"""
THREADED PROCESS
Reads and parses UBX message data from stream
"""
while self._reading and self._serial_object:
if self._serial_object.in_waiting:
try:
(raw_data, parsed_data) = self._ubxreader.read()
# if raw_data:
# print(raw_data)
if parsed_data:
print(parsed_data)
except (
ube.UBXStreamError,
ube.UBXMessageError,
ube.UBXTypeError,
ube.UBXParseError,
) as err:
print(f"Something went wrong {err}")
continue
class UBXStreamer:
"""
UBXStreamer class.
"""
def __init__(self, port, baudrate, timeout=5):
"""
Constructor.
"""
self._serial_object = None
self._serial_thread = None
self._ubxreader = None
self._connected = False
self._reading = False
self._port = port
self._baudrate = baudrate
self._timeout = timeout
def __del__(self):
"""
Destructor.
"""
self.stop_read_thread()
self.disconnect()
def connect(self):
"""
Open serial connection.
"""
try:
self._serial_object = Serial(self._port,
self._baudrate,
timeout=self._timeout)
self._ubxreader = UBXReader(BufferedReader(self._serial_object),
False)
self._connected = True
except (SerialException, SerialTimeoutException) as err:
print(f"Error connecting to serial port {err}")
def disconnect(self):
"""
Close serial connection.
"""
if self._connected and self._serial_object:
try:
self._serial_object.close()
except (SerialException, SerialTimeoutException) as err:
print(f"Error disconnecting from serial port {err}")
self._connected = False
def start_read_thread(self):
"""
Start the serial reader thread.
"""
if self._connected:
self._reading = True
self._serial_thread = Thread(target=self._read_thread, daemon=True)
self._serial_thread.start()
def stop_read_thread(self):
"""
Stop the serial reader thread.
"""
if self._serial_thread is not None:
self._reading = False
def flush(self):
"""
Flush input buffer
"""
self._serial_object.reset_input_buffer()
def waiting(self):
"""
Check if any messages remaining in the input buffer
"""
return self._serial_object.in_waiting
def _read_thread(self):
"""
THREADED PROCESS
Reads and parses UBX message data from stream
"""
start_time = time.time()
header = ['TS', 'time', 'lat', 'lon', 'speed', 'fix']
global gnss_points_df
gnss_points_df = pd.DataFrame(columns=header)
while self._reading and self._serial_object:
if self._serial_object.in_waiting:
start_time = time.time()
try:
(raw_data, parsed_data) = self._ubxreader.read()
# if raw_data:
# print(raw_data)
if parsed_data:
print(parsed_data)
print(parsed_data.identity)
if parsed_data.identity == 'NAV-PVT':
utc_time = datetime(1980, 1, 6) + timedelta(
seconds=(parsed_data.iTOW / 1000) - (35 - 19))
utc_time = utc_time.replace(
year=parsed_data.year,
month=parsed_data.month,
day=parsed_data.day)
ts = time.mktime(utc_time.timetuple(
)) * 1000.0 + utc_time.microsecond / 1000.0
if parsed_data.fixType == 3:
fix = "3d"
elif parsed_data.fixType == 2:
fix = "2d"
else:
fix = "none"
loc_str = [
ts,
utc_time.strftime('%Y-%m-%d %H:%M:%S.%f '),
parsed_data.lat / 10**7,
parsed_data.lon / 10**7, parsed_data.gSpeed,
fix
]
print(loc_str)
gnss_points_df.loc[ts] = loc_str
except (
ube.UBXStreamError,
ube.UBXMessageError,
ube.UBXTypeError,
ube.UBXParseError,
) as err:
print(f"Something went wrong {err}")
continue
else:
count_time = time.time() - start_time
if count_time > self._timeout:
print("\n\nStopping reader thread...")
self.stop_read_thread()
print("Disconnecting from serial port...")
self.disconnect()
break
class UBXStreamer:
"""
UBXStreamer class.
"""
def __init__(self, port, baudrate, timeout=5):
"""
Constructor.
"""
self._serial_object = None
self._serial_thread = None
self._ubxreader = None
self._connected = False
self._reading = False
self._port = port
self._baudrate = baudrate
self._timeout = timeout
def __del__(self):
"""
Destructor.
"""
self.stop_read_thread()
self.disconnect()
def connect(self):
"""
Open serial connection.
"""
try:
self._serial_object = Serial(self._port,
self._baudrate,
timeout=self._timeout)
self._ubxreader = UBXReader(BufferedReader(self._serial_object),
False)
self._connected = True
except (SerialException, SerialTimeoutException) as err:
print(f"Error connecting to serial port {err}")
def disconnect(self):
"""
Close serial connection.
"""
if self._connected and self._serial_object:
try:
self._serial_object.close()
except (SerialException, SerialTimeoutException) as err:
print(f"Error disconnecting from serial port {err}")
self._connected = False
def start_read_thread(self):
"""
Start the serial reader thread.
"""
if self._connected:
self._reading = True
self._serial_thread = Thread(target=self._read_thread, daemon=True)
self._serial_thread.start()
def stop_read_thread(self):
"""
Stop the serial reader thread.
"""
if self._serial_thread is not None:
self._reading = False
def send(self, data):
"""
Send data to serial connection.
"""
self._serial_object.write(data)
def flush(self):
"""
Flush input buffer
"""
self._serial_object.reset_input_buffer()
def waiting(self):
"""
Check if any messages remaining in the input buffer
"""
return self._serial_object.in_waiting
def _read_thread(self):
"""
THREADED PROCESS
Reads and parses UBX message data from stream
"""
while self._reading and self._serial_object:
if self._serial_object.in_waiting:
try:
(raw_data, parsed_data) = self._ubxreader.read()
#if raw_data:
# print(raw_data)
if parsed_data:
print(parsed_data)
except (
ube.UBXStreamError,
ube.UBXMessageError,
ube.UBXTypeError,
ube.UBXParseError,
) as err:
print(f"Something went wrong {err}")
continue
def ubx_proto_only(self):
"""
Creates a CFG-PRT configuration message and
sends it to the receiver.
"""
NMEA = b"\x02\x00"
UBX = b"\x01\x00"
BOTH = b"\x03\x00"
try:
msg = UBXMessage(
"CFG",
"CFG-PRT",
SET,
portID=3,
baudRate=0,
inProtoMask=b"\x07\x00",
outProtoMask=UBX,
)
print(f"Sending {msg}")
self.send(msg.serialize())
except (ube.UBXMessageError, ube.UBXTypeError,
ube.UBXParseError) as err:
print(f"Something went wrong {err}")
def set_nav(self):
"""
Set the current Navigation configuration
"""
transaction = 0 # Immediate application
layers = 3 # Volatile RAM only.
# 2 - Battery Backed
# 3 - Flash
cfgData = [
#("CFG_SFIMU_IMU_AUTO_MNTALG_ENA", 1), # Enable auto-IMU mounting alignment
# Start driving at a minimum speed of 30 km/h and do
# a series of approximately 10 left and right turns (at
# least 90 degrees).
("CFG_SFIMU_IMU_AUTO_MNTALG_YAW", 158),
("CFG_SFIMU_IMU_AUTO_MNTALG_PITCH", 86),
("CFG_SFIMU_IMU_AUTO_MNTALG_ROLL", 191),
("CFG_RATE_MEAS", 100), # Measurement rate once every x ms
("CFG_RATE_NAV", 1), # 1 navigation solution for every measurement
("CFG_RATE_NAV_PRIO", 10), # 30 Hz navigation priority messages
("CFG_MSGOUT_UBX_NAV_STATUS_USB", 1), # Navigation Status
("CFG_MSGOUT_UBX_NAV_PVT_USB", 1), # PVT solution
("CFG_MSGOUT_UBX_NAV_HPPOSECEF_USB",
1), # High Precision ECEF position
("CFG_MSGOUT_UBX_NAV_POSECEF_USB",
1), # Regular precision ECEF position
("CFG_MSGOUT_UBX_NAV_VELECEF_USB", 1), # Velocity in ECEF
("CFG_MSGOUT_UBX_NAV_HPPOSLLH_USB",
1), # High precision LLH position
("CFG_MSGOUT_UBX_NAV_POSLLH_USB",
1), # High precision LLH position
("CFG_MSGOUT_UBX_NAV_ATT_USB", 1), # Attitude of the vehicle
("CFG_MSGOUT_UBX_NAV_COV_USB", 1), # Covariance matricies
("CFG_MSGOUT_UBX_NAV_TIMEGPS_USB",
1), # GPS Time more explicitly called out
("CFG_MSGOUT_UBX_ESF_INS_USB",
1), # Compensated angular rate and acceleration
("CFG_MSGOUT_UBX_ESF_STATUS_USB",
1), # External Sensor Fusion status
("CFG_MSGOUT_UBX_ESF_ALG_USB", 1), #Alignment of the sensors
("CFG_MSGOUT_UBX_ESF_MEAS_USB", 1), # Measurements of the sensors
# Disable all the other default interfaces so we can actually get our 30Hz!
("CFG_MSGOUT_NMEA_ID_GGA_I2C", 0),
("CFG_MSGOUT_NMEA_ID_GGA_SPI", 0),
("CFG_MSGOUT_NMEA_ID_GGA_UART1", 0),
("CFG_MSGOUT_NMEA_ID_GGA_UART2", 0),
("CFG_MSGOUT_NMEA_ID_GGA_USB", 0),
("CFG_MSGOUT_NMEA_ID_GLL_I2C", 0),
("CFG_MSGOUT_NMEA_ID_GLL_SPI", 0),
("CFG_MSGOUT_NMEA_ID_GLL_UART1", 0),
("CFG_MSGOUT_NMEA_ID_GLL_UART2", 0),
("CFG_MSGOUT_NMEA_ID_GLL_USB", 0),
("CFG_MSGOUT_NMEA_ID_GSA_I2C", 0),
("CFG_MSGOUT_NMEA_ID_GSA_SPI", 0),
("CFG_MSGOUT_NMEA_ID_GSA_UART1", 0),
("CFG_MSGOUT_NMEA_ID_GSA_UART2", 0),
("CFG_MSGOUT_NMEA_ID_GSA_USB", 0),
("CFG_MSGOUT_NMEA_ID_GSV_I2C", 0),
("CFG_MSGOUT_NMEA_ID_GSV_SPI", 0),
("CFG_MSGOUT_NMEA_ID_GSV_UART1", 0),
("CFG_MSGOUT_NMEA_ID_GSV_UART2", 0),
("CFG_MSGOUT_NMEA_ID_GSV_USB", 0),
("CFG_MSGOUT_NMEA_ID_RMC_I2C", 0),
("CFG_MSGOUT_NMEA_ID_RMC_SPI", 0),
("CFG_MSGOUT_NMEA_ID_RMC_UART1", 0),
("CFG_MSGOUT_NMEA_ID_RMC_UART2", 0),
("CFG_MSGOUT_NMEA_ID_RMC_USB", 0),
("CFG_MSGOUT_NMEA_ID_VTG_I2C", 0),
("CFG_MSGOUT_NMEA_ID_VTG_SPI", 0),
("CFG_MSGOUT_NMEA_ID_VTG_UART1", 0),
("CFG_MSGOUT_NMEA_ID_VTG_UART2", 0),
("CFG_MSGOUT_NMEA_ID_VTG_USB", 0),
]
msg = UBXMessage.config_set(layers, transaction, cfgData)
ubp.send(msg.serialize())
class UBXStreamer:
"""
UBXStreamer class.
"""
def __init__(self, port, baudrate, timeout=5):
"""
Constructor.
"""
self._serial_object = None
self._serial_thread = None
self._ubxreader = None
self._connected = False
self._reading = False
self._port = port
self._baudrate = baudrate
self._timeout = timeout
def __del__(self):
"""
Destructor.
"""
self.stop_read_thread()
self.disconnect()
def connect(self):
"""
Open serial connection.
"""
try:
self._serial_object = Serial(
self._port, self._baudrate, timeout=self._timeout
)
self._ubxreader = UBXReader(BufferedReader(self._serial_object), False)
self._connected = True
except (SerialException, SerialTimeoutException) as err:
print(f"Error connecting to serial port {err}")
def disconnect(self):
"""
Close serial connection.
"""
if self._connected and self._serial_object:
try:
self._serial_object.close()
except (SerialException, SerialTimeoutException) as err:
print(f"Error disconnecting from serial port {err}")
self._connected = False
def start_read_thread(self):
"""
Start the serial reader thread.
"""
if self._connected:
self._reading = True
self._serial_thread = Thread(target=self._read_thread, daemon=True)
self._serial_thread.start()
def stop_read_thread(self):
"""
Stop the serial reader thread.
"""
if self._serial_thread is not None:
self._reading = False
def send(self, data):
"""
Send data to serial connection.
"""
self._serial_object.write(data)
def flush(self):
"""
Flush input buffer
"""
self._serial_object.reset_input_buffer()
def waiting(self):
"""
Check if any messages remaining in the input buffer
"""
return self._serial_object.in_waiting
def _read_thread(self):
"""
THREADED PROCESS
Reads and parses UBX message data from stream
"""
while self._reading and self._serial_object:
if self._serial_object.in_waiting:
try:
(_, parsed_data) = self._ubxreader.read()
if parsed_data:
print(parsed_data)
except (
ube.UBXStreamError,
ube.UBXMessageError,
ube.UBXTypeError,
ube.UBXParseError,
) as err:
print(f"Something went wrong {err}")
continue
def poll_uart(self, layer=0):
"""
Poll the current BBR UART1/2 configuration
"""
position = 0
keys = ["CFG_UART1_BAUDRATE", "CFG_UART2_BAUDRATE"]
msg = UBXMessage.config_poll(layer, position, keys)
ubp.send(msg.serialize())
def set_uart(self, layers=1):
"""
Set the current BBR UART1/2 configuration
"""
transaction = 0
cfgData = [("CFG_UART1_BAUDRATE", 115200), ("CFG_UART2_BAUDRATE", 57600)]
msg = UBXMessage.config_set(layers, transaction, cfgData)
ubp.send(msg.serialize())
def unset_uart(self, layers=1):
"""
Unset (del) the current BBR UART1/2 configuration
"""
transaction = 0
keys = ["CFG_UART1_BAUDRATE", "CFG_UART2_BAUDRATE"]
msg = UBXMessage.config_del(layers, transaction, keys)
ubp.send(msg.serialize())
class UBXStreamer:
"""
UBXStreamer class.
"""
def __init__(self, port, baudrate, timeout=5):
"""
Constructor.
"""
self._serial_object = None
self._serial_thread = None
self._ubxreader = None
self._connected = False
self._reading = False
self._port = port
self._baudrate = baudrate
self._timeout = timeout
self.almanac_raw = {i: Almanac_Raw() for i in range(32)} # Almanac_Raw object for each GPS Satellite
self.almanac_parsed = {i: None for i in range(32)} # Almanac_Parsed object for each GPS Satellite
self.ephemeris_raw = {i: Ephemeris_Raw() for i in range(32)} # Ephemeris_Raw object for each GPS Satellite
self.ephemeris_parsed = {i: None for i in range(32)} # Ephemeris_Parsed object for each GPS Satellite
self.pseudorange = {i: None for i in range(32)} # Pseudorange for each GPS Satellite
self.snr = {i: None for i in range(32)} # Signal to Noise Ration for each GPS Satellite
self.sat_position = {i: None for i in range(32)} # Satellite position in the sky for each GPS Satellite
self.clockBias_dist = 0 # Clock bias (as a distance in meters)
self.receiver_time = 0 # Receiver time in seconds
def connect(self):
"""
Open serial connection.
"""
try:
self._serial_object = Serial(self._port,
self._baudrate,
timeout=self._timeout)
self._ubxreader = UBXReader(BufferedReader(self._serial_object), False)
self._connected = True
except (SerialException, SerialTimeoutException) as err:
print(f"Error connecting to serial port {err}")
def disconnect(self):
"""
Close serial connection.
"""
if self._connected and self._serial_object:
try:
self._serial_object.close()
except (SerialException, SerialTimeoutException) as err:
print(f"Error disconnecting from serial port {err}")
self._connected = False
def start_read_thread(self):
"""
Start the serial reader thread.
"""
if self._connected:
self._reading = True
self._serial_thread = Thread(target=self._read_thread, daemon=False)
self._serial_thread.start()
def stop_read_thread(self):
"""
Stop the serial reader thread.
"""
if self._serial_thread is not None:
self._reading = False
self._serial_thread.join()
def send(self, data1, data2, data3=None):
"""
Send data to serial connection.
The number of data (i.e. UBX messages) in parameters here is arbitrary.
There is no theoretical limit, it's up to the user to define how many messages will be sent.
"""
for data in (data1, data2, data3):
if data is not None:
self._serial_object.write(data)
sleep(1) # Arbitrary timeout to account for delay in getting a response to requests.
# Could theoretically be lower
def _read_thread(self):
"""
THREADED PROCESS
Reads and parses UBX message data from the stream.
In addition to parsing messages, this method will do further processing for specific messages: UBX-NAV-CLOCK,
UBX-RXM-RAW, UBX-AID-ALM, and UBX-AID-EPH. Theoretically, this part could be done elsewhere.
"""
while self._reading and self._serial_object:
if self._serial_object.in_waiting:
try:
(raw_data, parsed_data) = self._ubxreader.read()
if parsed_data is not None:
print(parsed_data)
if parsed_data:
"""
'parsed_data' is an object which takes all the fields of the UBX message
(as defined in the U-blox documentation) as attributes.
"""
if parsed_data.identity == "NAV-CLOCK":
self.clockBias_dist = parsed_data.clkB * (10 ** -9) * C # Multiplying to account for units.
self.receiver_time = parsed_data.iTOW * (10 ** -3)
print("Receiver clock bias: ", self.clockBias_dist)
print("GPS System Time: ", self.receiver_time)
print("\n")
if parsed_data.identity == "RXM-RAW":
self.receiver_time = parsed_data.iTOW * (10 ** -3)
numSV = parsed_data.numSV
"""
The number of attributes for RXM-RAW depends on the amount of satellites detected.
As such, there is a repeated block of attributes indexed from 1 to numSV
(= number of satellites detected).
"""
if numSV != 0:
for i in range(1, numSV + 1):
prMes_num = "prMes_0" + str(i) if i < 10 else "prMes_" + str(i)
sv_num = "sv_0" + str(i) if i < 10 else "sv_" + str(i)
snr_num = "cno_0" + str(i) if i < 10 else "cno_" + str(i)
"""
The satellite identifier 'sv' is indexed from 1, the corresponding python
dictionary is indexed from 0.
"""
self.pseudorange[int(getattr(parsed_data, sv_num)) - 1] = \
getattr(parsed_data, prMes_num)
self.snr[int(getattr(parsed_data, sv_num)) - 1] = getattr(parsed_data, snr_num)
if parsed_data.identity == "AID-ALM":
self.almanac_raw[parsed_data.svid - 1].set_data(raw_data) # Fills up the ephemeris class
if not self.almanac_raw[parsed_data.svid - 1].sf_empty:
self.almanac_parsed[parsed_data.svid - 1] = \
Almanac_Parsed(self.almanac_raw[parsed_data.svid - 1])
self.almanac_parsed[parsed_data.svid - 1].special_print()
print("\n")
if parsed_data.identity == "AID-EPH" and parsed_data.how != 0:
print('svid: ', parsed_data.svid)
self.ephemeris_raw[parsed_data.svid - 1].set_data(raw_data) # Fills up the ephemeris class
if not self.ephemeris_raw[parsed_data.svid - 1].sf_empty:
self.ephemeris_parsed[parsed_data.svid - 1] = \
Ephemeris_Parsed(self.ephemeris_raw[parsed_data.svid - 1])
self.ephemeris_parsed[parsed_data.svid - 1].special_print()
if parsed_data.identity == "AID-EPH" and parsed_data.how != 0 and \
self.ephemeris_parsed[parsed_data.svid - 1] is not None:
X, Y, Z = get_wgs84_sat_position(self.ephemeris_parsed[parsed_data.svid - 1],
self.receiver_time,
self.pseudorange[parsed_data.svid - 1])
self.sat_position[parsed_data.svid - 1] = XYZPosition(X, Y, Z)
print("XYZ: ", (X, Y, Z))
Lat, Long, Alt = xyz_to_latlongalt(X, Y, Z)
print("LatLongAlt: ", (Lat, Long, Alt))
print("\n")
except (ube.UBXStreamError, ube.UBXMessageError, ube.UBXTypeError,
ube.UBXParseError) as err:
print(f"Something went wrong {err}")
continue
