400 lines
19 KiB
Python
400 lines
19 KiB
Python
# Checks the integrity of u-blox binary files
|
|
|
|
# Written by: Paul Clark
|
|
# Last update: August 26th 2020
|
|
|
|
# Reads a UBX file and checks the integrity of both UBX and NMEA data
|
|
# Will rewind and re-sync if an error is found
|
|
|
|
# SparkFun code, firmware, and software is released under the MIT License (http://opensource.org/licenses/MIT)
|
|
#
|
|
# The MIT License (MIT)
|
|
#
|
|
# Copyright (c) 2020 SparkFun Electronics
|
|
#
|
|
# Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
# of this software and associated documentation files (the "Software"), to deal
|
|
# in the Software without restriction, including without limitation the rights
|
|
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
# copies of the Software, and to permit persons to whom the Software is
|
|
# furnished to do so, subject to the following conditions:
|
|
#
|
|
# The above copyright notice and this permission notice shall be included in all
|
|
# copies or substantial portions of the Software.
|
|
#
|
|
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
# SOFTWARE.
|
|
|
|
import sys
|
|
import os
|
|
|
|
# Add byte to checksums sum1 and sum2
|
|
def csum(byte, sum1, sum2):
|
|
sum1 = sum1 + byte
|
|
sum2 = sum2 + sum1
|
|
sum1 = sum1 & 0xFF
|
|
sum2 = sum2 & 0xFF
|
|
return sum1,sum2
|
|
|
|
print('UBX Integrity Checker')
|
|
print()
|
|
|
|
filename = ''
|
|
|
|
if filename == '':
|
|
# Check if the bin filename was passed in argv
|
|
if len(sys.argv) > 1: filename = sys.argv[1]
|
|
|
|
# Find first .ubx file in the current directory
|
|
firstfile = ''
|
|
for root, dirs, files in os.walk("."):
|
|
if len(files) > 0:
|
|
if root == ".": # Comment this line to check sub-directories too
|
|
for afile in files:
|
|
if afile[-4:] == '.ubx':
|
|
if firstfile == '': firstfile = os.path.join(root, afile)
|
|
|
|
# Ask user for .bin filename offering firstfile as the default
|
|
if filename == '': filename = input('Enter the UBX filename (default: ' + firstfile + '): ') # Get the filename
|
|
if filename == '': filename = firstfile
|
|
|
|
# Ask user if the data contains NMEA messages
|
|
response = input('Could this file contain any NMEA messages? (Y/n): ') # Get the response
|
|
if (response == '') or (response == 'Y') or (response == 'y'):
|
|
containsNMEA = True
|
|
else:
|
|
containsNMEA = False
|
|
|
|
print()
|
|
print('Processing',filename)
|
|
print()
|
|
filesize = os.path.getsize(filename) # Record the file size
|
|
|
|
# Try to open file for reading
|
|
try:
|
|
fi = open(filename,"rb")
|
|
except:
|
|
raise Exception('Invalid file!')
|
|
|
|
processed = -1 # The nunber of bytes processed
|
|
messages = {} # The collected message types
|
|
longest = 0 # The length of the longest UBX message
|
|
keepGoing = True
|
|
|
|
# Sync 'state machine'
|
|
looking_for_B5_dollar = 0 # Looking for either a UBX 0xB5 or an NMEA '$'
|
|
looking_for_62 = 1 # Looking for a UBX 0x62 header byte
|
|
looking_for_class = 2 # Looking for UBX class byte
|
|
looking_for_ID = 3 # Looking for UBX ID byte
|
|
looking_for_length_LSB = 4 # Looking for UBX length bytes
|
|
looking_for_length_MSB = 5
|
|
processing_payload = 6 # Processing the payload. Keep going until length bytes have been processed
|
|
looking_for_checksum_A = 7 # Looking for UBX checksum bytes
|
|
looking_for_checksum_B = 8
|
|
sync_lost = 9 # Go into this state if sync is lost (bad checksum etc.)
|
|
looking_for_asterix = 10 # Looking for NMEA '*'
|
|
looking_for_csum1 = 11 # Looking for NMEA checksum bytes
|
|
looking_for_csum2 = 12
|
|
looking_for_term1 = 13 # Looking for NMEA terminating bytes (CR and LF)
|
|
looking_for_term2 = 14
|
|
|
|
ubx_nmea_state = sync_lost # Initialize the state machine
|
|
|
|
# Storage for UBX messages
|
|
ubx_length = 0
|
|
ubx_class = 0
|
|
ubx_ID = 0
|
|
ubx_checksum_A = 0
|
|
ubx_checksum_B = 0
|
|
ubx_expected_checksum_A = 0
|
|
ubx_expected_checksum_B = 0
|
|
|
|
# Storage for NMEA messages
|
|
nmea_length = 0
|
|
nmea_char_1 = 0 # e.g. G
|
|
nmea_char_2 = 0 # e.g. P
|
|
nmea_char_3 = 0 # e.g. G
|
|
nmea_char_4 = 0 # e.g. G
|
|
nmea_char_5 = 0 # e.g. A
|
|
nmea_csum = 0
|
|
nmea_csum1 = 0
|
|
nmea_csum2 = 0
|
|
nmea_expected_csum1 = 0
|
|
nmea_expected_csum2 = 0
|
|
|
|
max_nmea_len = 100 # Maximum length for an NMEA message: use this to detect if we have lost sync while receiving an NMEA message
|
|
sync_lost_at = -1 # Record where we lost sync
|
|
rewind_to = -1 # Keep a note of where we should rewind to if sync is lost
|
|
rewind_attempts = 0 # Keep a note of how many rewinds have been attempted
|
|
max_rewinds = 100 # Abort after this many rewinds
|
|
rewind_in_progress = False # Flag to indicate if a rewind is in progress
|
|
resyncs = 0 # Record the number of successful resyncs
|
|
resync_in_progress = False # Flag to indicate if a resync is in progress
|
|
message_start_byte = 0 # Record where the latest message started (for resync reporting)
|
|
|
|
try:
|
|
while keepGoing:
|
|
|
|
# Read one byte from the file
|
|
fileBytes = fi.read(1)
|
|
if (len(fileBytes) == 0):
|
|
print('ERROR: Read zero bytes. End of file?! Or zero file size?!')
|
|
raise Exception('End of file?! Or zero file size?!')
|
|
c = fileBytes[0]
|
|
|
|
processed = processed + 1 # Keep a record of how many bytes have been read and processed
|
|
|
|
# Process data bytes according to ubx_nmea_state
|
|
# For UBX messages:
|
|
# Sync Char 1: 0xB5
|
|
# Sync Char 2: 0x62
|
|
# Class byte
|
|
# ID byte
|
|
# Length: two bytes, little endian
|
|
# Payload: length bytes
|
|
# Checksum: two bytes
|
|
# For NMEA messages:
|
|
# Starts with a '$'
|
|
# The next five characters indicate the message type (stored in nmea_char_1 to nmea_char_5)
|
|
# Message fields are comma-separated
|
|
# Followed by an '*'
|
|
# Then a two character checksum (the logical exclusive-OR of all characters between the $ and the * as ASCII hex)
|
|
# Ends with CR LF
|
|
# Only allow a new file to be opened when a complete packet has been processed and ubx_nmea_state has returned to "looking_for_B5_dollar"
|
|
# Or when a data error is detected (sync_lost)
|
|
|
|
# RXM_RAWX is class 0x02 ID 0x15
|
|
# RXM_SFRBF is class 0x02 ID 0x13
|
|
# TIM_TM2 is class 0x0d ID 0x03
|
|
# NAV_POSLLH is class 0x01 ID 0x02
|
|
# NAV_PVT is class 0x01 ID 0x07
|
|
# NAV-STATUS is class 0x01 ID 0x03
|
|
|
|
if (ubx_nmea_state == looking_for_B5_dollar) or (ubx_nmea_state == sync_lost):
|
|
if (c == 0xB5): # Have we found Sync Char 1 (0xB5) if we were expecting one?
|
|
if (ubx_nmea_state == sync_lost):
|
|
print("UBX Sync Char 1 (0xB5) found at byte "+str(processed)+". Checking for Sync Char 2")
|
|
ubx_nmea_state = looking_for_62 # Now look for Sync Char 2 (0x62)
|
|
message_start_byte = processed # Record the message start byte for resync reporting
|
|
elif (c == 0x24) and (containsNMEA == True): # Have we found an NMEA '$' if we were expecting one?
|
|
if (ubx_nmea_state == sync_lost):
|
|
print("NMEA $ found at byte "+str(processed)+". Attempting to process the message")
|
|
ubx_nmea_state = looking_for_asterix # Now keep going until we receive an asterix
|
|
nmea_length = 0 # Reset nmea_length then use it to check for excessive message length
|
|
nmea_csum = 0 # Reset the nmea_csum. Update it as each character arrives
|
|
nmea_char_1 = 0x30 # Reset the first five NMEA chars to something invalid
|
|
nmea_char_2 = 0x30
|
|
nmea_char_3 = 0x30
|
|
nmea_char_4 = 0x30
|
|
nmea_char_5 = 0x30
|
|
message_start_byte = processed # Record the message start byte for resync reporting
|
|
else:
|
|
#print("Was expecting Sync Char 0xB5 or an NMEA $ but did not receive one!")
|
|
if (c == 0x24):
|
|
print("Warning: * found at byte "+str(processed)+"! Are you sure this file does not contain NMEA messages?")
|
|
sync_lost_at = processed
|
|
ubx_nmea_state = sync_lost
|
|
elif (ubx_nmea_state == looking_for_62):
|
|
if (c == 0x62): # Have we found Sync Char 2 (0x62) when we were expecting one?
|
|
ubx_expected_checksum_A = 0 # Reset the expected checksum
|
|
ubx_expected_checksum_B = 0
|
|
ubx_nmea_state = looking_for_class # Now look for Class byte
|
|
else:
|
|
print("Panic!! Was expecting Sync Char 2 (0x62) but did not receive one!")
|
|
print("Sync lost at byte "+str(processed)+". Attemting to re-sync")
|
|
sync_lost_at = processed
|
|
resync_in_progress = True
|
|
ubx_nmea_state = sync_lost
|
|
elif (ubx_nmea_state == looking_for_class):
|
|
ubx_class = c
|
|
ubx_expected_checksum_A = ubx_expected_checksum_A + c # Update the expected checksum
|
|
ubx_expected_checksum_B = ubx_expected_checksum_B + ubx_expected_checksum_A
|
|
ubx_nmea_state = looking_for_ID # Now look for ID byte
|
|
elif (ubx_nmea_state == looking_for_ID):
|
|
ubx_ID = c
|
|
ubx_expected_checksum_A = ubx_expected_checksum_A + c # Update the expected checksum
|
|
ubx_expected_checksum_B = ubx_expected_checksum_B + ubx_expected_checksum_A
|
|
message_type = '0x%02X 0x%02X'%(ubx_class,ubx_ID) # Record the message type
|
|
ubx_nmea_state = looking_for_length_LSB # Now look for length LSB
|
|
elif (ubx_nmea_state == looking_for_length_LSB):
|
|
ubx_length = c # Store the length LSB
|
|
ubx_expected_checksum_A = ubx_expected_checksum_A + c # Update the expected checksum
|
|
ubx_expected_checksum_B = ubx_expected_checksum_B + ubx_expected_checksum_A
|
|
ubx_nmea_state = looking_for_length_MSB # Now look for length MSB
|
|
elif (ubx_nmea_state == looking_for_length_MSB):
|
|
ubx_length = ubx_length + (c * 256) # Add the length MSB
|
|
ubx_expected_checksum_A = ubx_expected_checksum_A + c # Update the expected checksum
|
|
ubx_expected_checksum_B = ubx_expected_checksum_B + ubx_expected_checksum_A
|
|
if (ubx_length > longest): # Update the longest UBX message length
|
|
longest = ubx_length
|
|
rewind_to = processed # If we lose sync due to dropped bytes then rewind to here
|
|
ubx_nmea_state = processing_payload # Now look for payload bytes (length: ubx_length)
|
|
elif (ubx_nmea_state == processing_payload):
|
|
ubx_length = ubx_length - 1 # Decrement length by one
|
|
ubx_expected_checksum_A = ubx_expected_checksum_A + c # Update the expected checksum
|
|
ubx_expected_checksum_B = ubx_expected_checksum_B + ubx_expected_checksum_A
|
|
if (ubx_length == 0):
|
|
ubx_expected_checksum_A = ubx_expected_checksum_A & 0xff # Limit checksums to 8-bits
|
|
ubx_expected_checksum_B = ubx_expected_checksum_B & 0xff
|
|
ubx_nmea_state = looking_for_checksum_A # If we have received length payload bytes, look for checksum bytes
|
|
elif (ubx_nmea_state == looking_for_checksum_A):
|
|
ubx_checksum_A = c
|
|
ubx_nmea_state = looking_for_checksum_B
|
|
elif (ubx_nmea_state == looking_for_checksum_B):
|
|
ubx_checksum_B = c
|
|
ubx_nmea_state = looking_for_B5_dollar # All bytes received so go back to looking for a new Sync Char 1 unless there is a checksum error
|
|
if ((ubx_expected_checksum_A != ubx_checksum_A) or (ubx_expected_checksum_B != ubx_checksum_B)):
|
|
print("Panic!! UBX checksum error!")
|
|
print("Sync lost at byte "+str(processed)+". Attemting to re-sync.")
|
|
sync_lost_at = processed
|
|
resync_in_progress = True
|
|
ubx_nmea_state = sync_lost
|
|
else:
|
|
# Valid UBX message was received. Check if we have seen this message type before
|
|
if message_type in messages:
|
|
messages[message_type] += 1 # if we have, increment its count
|
|
else:
|
|
messages[message_type] = 1 # if we have not, set its count to 1
|
|
rewind_in_progress = False # Clear rewind_in_progress
|
|
rewind_to = -1
|
|
if (resync_in_progress == True): # Check if we are resyncing
|
|
resync_in_progress = False # Clear the flag now that a valid message has been received
|
|
resyncs += 1 # Increment the number of successful resyncs
|
|
print("Sync successfully re-established at byte "+str(processed)+". The UBX message started at byte "+str(message_start_byte))
|
|
print()
|
|
# NMEA messages
|
|
elif (ubx_nmea_state == looking_for_asterix):
|
|
nmea_length = nmea_length + 1 # Increase the message length count
|
|
if (nmea_length > max_nmea_len): # If the length is greater than max_nmea_len, something bad must have happened (sync_lost)
|
|
print("Panic!! Excessive NMEA message length!")
|
|
print("Sync lost at byte "+str(processed)+". Attemting to re-sync")
|
|
sync_lost_at = processed
|
|
resync_in_progress = True
|
|
ubx_nmea_state = sync_lost
|
|
continue
|
|
# If this is one of the first five characters, store it
|
|
if (nmea_length <= 5):
|
|
if (nmea_length == 1):
|
|
nmea_char_1 = c
|
|
rewind_to = processed # If we lose sync due to dropped bytes then rewind to here
|
|
elif (nmea_length == 2):
|
|
nmea_char_2 = c
|
|
elif (nmea_length == 3):
|
|
nmea_char_3 = c
|
|
elif (nmea_length == 4):
|
|
nmea_char_4 = c
|
|
else: # ubx_length == 5
|
|
nmea_char_5 = c
|
|
message_type = chr(nmea_char_1) + chr(nmea_char_2) + chr(nmea_char_3) + chr(nmea_char_4) + chr(nmea_char_5) # Record the message type
|
|
# Now check if this is an '*'
|
|
if (c == 0x2A):
|
|
# Asterix received
|
|
# Don't exOR it into the checksum
|
|
# Instead calculate what the expected checksum should be (nmea_csum in ASCII hex)
|
|
nmea_expected_csum1 = ((nmea_csum & 0xf0) >> 4) + 0x30 # Convert MS nibble to ASCII hex
|
|
if (nmea_expected_csum1 >= 0x3A): # : follows 9 so add 7 to convert to A-F
|
|
nmea_expected_csum1 += 7
|
|
nmea_expected_csum2 = (nmea_csum & 0x0f) + 0x30 # Convert LS nibble to ASCII hex
|
|
if (nmea_expected_csum2 >= 0x3A): # : follows 9 so add 7 to convert to A-F
|
|
nmea_expected_csum2 += 7
|
|
# Next, look for the first csum character
|
|
ubx_nmea_state = looking_for_csum1
|
|
continue # Don't include the * in the checksum
|
|
# Now update the checksum
|
|
# The checksum is the exclusive-OR of all characters between the $ and the *
|
|
nmea_csum = nmea_csum ^ c
|
|
elif (ubx_nmea_state == looking_for_csum1):
|
|
# Store the first NMEA checksum character
|
|
nmea_csum1 = c
|
|
ubx_nmea_state = looking_for_csum2
|
|
elif (ubx_nmea_state == looking_for_csum2):
|
|
# Store the second NMEA checksum character
|
|
nmea_csum2 = c
|
|
# Now check if the checksum is correct
|
|
if ((nmea_csum1 != nmea_expected_csum1) or (nmea_csum2 != nmea_expected_csum2)):
|
|
# The checksum does not match so sync_lost
|
|
print("Panic!! NMEA checksum error!")
|
|
print("Sync lost at byte "+str(processed)+". Attemting to re-sync")
|
|
sync_lost_at = processed
|
|
resync_in_progress = True
|
|
ubx_nmea_state = sync_lost
|
|
else:
|
|
# Checksum was valid so wait for the terminators
|
|
ubx_nmea_state = looking_for_term1
|
|
elif (ubx_nmea_state == looking_for_term1):
|
|
# Check if this is CR
|
|
if (c != 0x0D):
|
|
print("Panic!! NMEA CR not found!")
|
|
print("Sync lost at byte "+str(processed)+". Attemting to re-sync")
|
|
sync_lost_at = processed
|
|
resync_in_progress = True
|
|
ubx_nmea_state = sync_lost
|
|
else:
|
|
ubx_nmea_state = looking_for_term2
|
|
elif (ubx_nmea_state == looking_for_term2):
|
|
# Check if this is LF
|
|
if (c != 0x0A):
|
|
print("Panic!! NMEA LF not found!")
|
|
print("Sync lost at byte "+str(processed)+". Attemting to re-sync")
|
|
sync_lost_at = processed
|
|
resync_in_progress = True
|
|
ubx_nmea_state = sync_lost
|
|
else:
|
|
# Valid NMEA message was received. Check if we have seen this message type before
|
|
if message_type in messages:
|
|
messages[message_type] += 1 # if we have, increment its count
|
|
else:
|
|
messages[message_type] = 1 # if we have not, set its count to 1
|
|
# LF was received so go back to looking for B5 or a $
|
|
ubx_nmea_state = looking_for_B5_dollar
|
|
rewind_in_progress = False # Clear rewind_in_progress
|
|
rewind_to = -1
|
|
if (resync_in_progress == True): # Check if we are resyncing
|
|
resync_in_progress = False # Clear the flag now that a valid message has been received
|
|
resyncs += 1 # Increment the number of successful resyncs
|
|
print("Sync successfully re-established at byte "+str(processed)+". The NMEA message started at byte "+str(message_start_byte))
|
|
print()
|
|
|
|
# Check if the end of the file has been reached
|
|
if (processed >= filesize - 1): keepGoing = False
|
|
|
|
# Check if we should attempt to rewind
|
|
# Don't rewind if we have not yet seen a valid message
|
|
# Don't rewind if a rewind is already in progress
|
|
if (ubx_nmea_state == sync_lost) and (len(messages) > 0) and (rewind_in_progress == False) and (rewind_to >= 0):
|
|
rewind_attempts += 1 # Increment the number of rewind attempts
|
|
if (rewind_attempts > max_rewinds): # Only rewind up to max_rewind times
|
|
print("Panic! Maximum rewind attempts reached! Aborting...")
|
|
keepGoing = False
|
|
else:
|
|
print("Sync has been lost. Currently processing byte "+str(processed)+". Rewinding to byte "+str(rewind_to))
|
|
fi.seek(rewind_to) # Rewind the file
|
|
processed = rewind_to - 1 # Rewind processed too! (-1 is needed as processed is incremented at the start of the loop)
|
|
rewind_in_progress = True # Flag that a rewind is in progress
|
|
|
|
|
|
finally:
|
|
fi.close() # Close the file
|
|
|
|
# Print the file statistics
|
|
print()
|
|
processed += 1
|
|
print('Processed',processed,'bytes')
|
|
print('File size was',filesize)
|
|
if (processed != filesize):
|
|
print('FILE SIZE MISMATCH!!')
|
|
print('Longest UBX message was %i data bytes'%longest)
|
|
if len(messages) > 0:
|
|
print('Message types and totals were:')
|
|
for key in messages.keys():
|
|
print('Message type:',key,' Total:',messages[key])
|
|
if (resyncs > 0):
|
|
print('Number of successful resyncs:',resyncs)
|
|
print()
|
|
print('Bye!')
|