Paul
@@ -0,0 +1,515 @@
|
|||||||
|
# Aligns the rcvTow in RAWX messages in u-blox UBX binary files to the nearest decimalPlaces seconds
|
||||||
|
|
||||||
|
# Written by: Paul Clark
|
||||||
|
# Last update: August 17th 2022
|
||||||
|
|
||||||
|
# SparkFun code, firmware, and software is released under the MIT License (http://opensource.org/licenses/MIT)
|
||||||
|
#
|
||||||
|
# The MIT License (MIT)
|
||||||
|
#
|
||||||
|
# Copyright (c) 2022 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
|
||||||
|
import struct
|
||||||
|
|
||||||
|
# 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 RAWX Aligner')
|
||||||
|
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
|
||||||
|
|
||||||
|
repairFile = True
|
||||||
|
if (filename[-4] == '.'):
|
||||||
|
repairFilename = filename[:-4] + '.aligned' + filename[-4:]
|
||||||
|
else:
|
||||||
|
repairFilename = filename + '.aligned'
|
||||||
|
|
||||||
|
decimalPlaces = 0 # Default to whole seconds
|
||||||
|
if len(sys.argv) > 2: decimalPlaces = sys.argv[2]
|
||||||
|
|
||||||
|
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!')
|
||||||
|
|
||||||
|
# Try to open repair file for write and read
|
||||||
|
if (repairFile):
|
||||||
|
try:
|
||||||
|
fo = open(repairFilename,"w+b")
|
||||||
|
except:
|
||||||
|
raise Exception('Could not open aligned file!')
|
||||||
|
|
||||||
|
processed = -1 # The number of bytes processed
|
||||||
|
messages = {} # The collected message types
|
||||||
|
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_length_LSB = 0
|
||||||
|
ubx_length_MSB = 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
|
||||||
|
longest_UBX = 0 # The length of the longest UBX message
|
||||||
|
longest_UBX_candidate = 0 # Candidate for the length of the longest valid UBX message
|
||||||
|
|
||||||
|
# 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
|
||||||
|
longest_NMEA = 0 # The length of the longest valid NMEA message
|
||||||
|
|
||||||
|
max_nmea_len = 128 # 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)
|
||||||
|
|
||||||
|
rewind_repair_file_to = 0 # Keep a note of where to rewind the repair file to if sync is lost
|
||||||
|
repaired_file_bytes = 0 # Keep a note of how many bytes have been written to the repair file
|
||||||
|
|
||||||
|
repair_file_rawx_payload_start = 0 # Keep a note of where the RAWX payload starts (i.e. where the rcvTow R8 starts)
|
||||||
|
largest_rawx_alignment = 0.0 # Keep note of the largest alignment change
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
# Write the byte to the repair file if desired
|
||||||
|
if (repairFile):
|
||||||
|
fo.write(fileBytes)
|
||||||
|
repaired_file_bytes = repaired_file_bytes + 1
|
||||||
|
|
||||||
|
# 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_length_LSB = 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_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_length_MSB = 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
|
||||||
|
longest_UBX_candidate = ubx_length + 8 # Update the longest UBX message length candidate. Include the header, class, ID, length and checksum bytes
|
||||||
|
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)
|
||||||
|
|
||||||
|
if (message_type == '0x02 0x15'): # Is this RAWX? If so, record the start of the payload
|
||||||
|
repair_file_rawx_payload_start = repaired_file_bytes
|
||||||
|
|
||||||
|
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
|
||||||
|
if (longest_UBX_candidate > longest_UBX): # Update the longest UBX message length
|
||||||
|
longest_UBX = longest_UBX_candidate
|
||||||
|
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()
|
||||||
|
if (repairFile):
|
||||||
|
fo.seek(rewind_repair_file_to) # Rewind the repaired file
|
||||||
|
repaired_file_bytes = rewind_repair_file_to
|
||||||
|
fi.seek(message_start_byte) # Copy the valid message into the repair file
|
||||||
|
repaired_bytes_to_write = processed - message_start_byte
|
||||||
|
fileBytes = fi.read(repaired_bytes_to_write)
|
||||||
|
fo.write(fileBytes)
|
||||||
|
repaired_file_bytes = repaired_file_bytes + repaired_bytes_to_write
|
||||||
|
|
||||||
|
# Note: alignment is skipped if resyncing is in progress
|
||||||
|
# You may need to run the code twice to align any skipped alignments
|
||||||
|
|
||||||
|
else:
|
||||||
|
if (repairFile):
|
||||||
|
rewind_repair_file_to = repaired_file_bytes # Rewind repair file to here if sync is lost
|
||||||
|
|
||||||
|
if (message_type == '0x02 0x15'): # Is this RAWX? If so, do the alignment
|
||||||
|
|
||||||
|
ubx_expected_checksum_A = 0 # Reuse the expected checksum
|
||||||
|
ubx_expected_checksum_B = 0
|
||||||
|
ubx_expected_checksum_A = ubx_expected_checksum_A + ubx_class
|
||||||
|
ubx_expected_checksum_B = ubx_expected_checksum_B + ubx_expected_checksum_A
|
||||||
|
ubx_expected_checksum_A = ubx_expected_checksum_A + ubx_ID
|
||||||
|
ubx_expected_checksum_B = ubx_expected_checksum_B + ubx_expected_checksum_A
|
||||||
|
ubx_expected_checksum_A = ubx_expected_checksum_A + ubx_length_LSB
|
||||||
|
ubx_expected_checksum_B = ubx_expected_checksum_B + ubx_expected_checksum_A
|
||||||
|
ubx_expected_checksum_A = ubx_expected_checksum_A + ubx_length_MSB
|
||||||
|
ubx_expected_checksum_B = ubx_expected_checksum_B + ubx_expected_checksum_A
|
||||||
|
|
||||||
|
fo.seek(repair_file_rawx_payload_start) # Rewind the repair file
|
||||||
|
|
||||||
|
fileBytes = fo.read(8) # Read the rcvTow R8
|
||||||
|
rcvTow = struct.unpack('<d', fileBytes)[0] # Unpack the R8 (Little-endian)
|
||||||
|
rcvTowRounded = round(rcvTow, decimalPlaces) # Round to decimalPlaces
|
||||||
|
if (abs(rcvTow - rcvTowRounded) > largest_rawx_alignment):
|
||||||
|
largest_rawx_alignment = abs(rcvTow - rcvTowRounded) # Record the largest alignment change
|
||||||
|
fileBytes = struct.pack('<d', rcvTowRounded)
|
||||||
|
fo.seek(repair_file_rawx_payload_start) # Rewind the repair file
|
||||||
|
fo.write(fileBytes) # Write the rounded TOW to the repair file
|
||||||
|
for i in range(8): # Update the checksum
|
||||||
|
ubx_expected_checksum_A = ubx_expected_checksum_A + fileBytes[i]
|
||||||
|
ubx_expected_checksum_B = ubx_expected_checksum_B + ubx_expected_checksum_A
|
||||||
|
|
||||||
|
bytesToRead = (ubx_length_MSB * 256) + ubx_length_LSB - 8
|
||||||
|
fileBytes = fo.read(bytesToRead)
|
||||||
|
for i in range(bytesToRead): # Update the checksum
|
||||||
|
ubx_expected_checksum_A = ubx_expected_checksum_A + fileBytes[i]
|
||||||
|
ubx_expected_checksum_B = ubx_expected_checksum_B + ubx_expected_checksum_A
|
||||||
|
ubx_expected_checksum_A = ubx_expected_checksum_A & 0xff # Limit checksums to 8-bits
|
||||||
|
ubx_expected_checksum_B = ubx_expected_checksum_B & 0xff
|
||||||
|
fileBytes = struct.pack('BB', ubx_expected_checksum_A, ubx_expected_checksum_B)
|
||||||
|
fo.write(fileBytes) # Write the updated checksum
|
||||||
|
|
||||||
|
# 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
|
||||||
|
if (message_type == "PUBX,"): # Remove the comma from PUBX
|
||||||
|
message_type = "PUBX"
|
||||||
|
# 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
|
||||||
|
if (nmea_length > longest_NMEA): # Update the longest NMEA message length
|
||||||
|
longest_NMEA = nmea_length
|
||||||
|
# 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()
|
||||||
|
if (repairFile):
|
||||||
|
fo.seek(rewind_repair_file_to) # Rewind the repaired file
|
||||||
|
repaired_file_bytes = rewind_repair_file_to
|
||||||
|
fi.seek(message_start_byte) # Copy the valid message into the repair file
|
||||||
|
repaired_bytes_to_write = processed - message_start_byte
|
||||||
|
fileBytes = fi.read(repaired_bytes_to_write)
|
||||||
|
fo.write(fileBytes)
|
||||||
|
repaired_file_bytes = repaired_file_bytes + repaired_bytes_to_write
|
||||||
|
else:
|
||||||
|
if (repairFile):
|
||||||
|
rewind_repair_file_to = repaired_file_bytes # Rewind repair file to here if sync is lost
|
||||||
|
|
||||||
|
# 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
|
||||||
|
|
||||||
|
if (repairFile):
|
||||||
|
fo.close()
|
||||||
|
|
||||||
|
# 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 valid UBX message was %i bytes'%longest_UBX)
|
||||||
|
if (containsNMEA == True):
|
||||||
|
print('Longest valid NMEA message was %i characters'%longest_NMEA)
|
||||||
|
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()
|
||||||
|
if (repairFile):
|
||||||
|
print('Aligned data written to:', repairFilename)
|
||||||
|
print('Largest alignment change:', largest_rawx_alignment)
|
||||||
|
if (resyncs > 0):
|
||||||
|
print('Note: alignment is skipped during resyncing')
|
||||||
|
print('You may need to run the code twice to align any skipped alignments')
|
||||||
|
print()
|
||||||
|
print('Bye!')
|
||||||
@@ -0,0 +1,196 @@
|
|||||||
|
/*
|
||||||
|
Callback Example: ESF RAW (100Hz!)
|
||||||
|
By: Paul Clark
|
||||||
|
SparkFun Electronics
|
||||||
|
Date: September 8th, 2022
|
||||||
|
License: MIT. See license file for more information but you can
|
||||||
|
basically do whatever you want with this code.
|
||||||
|
|
||||||
|
This example configures the External Sensor Fusion RAW IMU sensor messages on the NEO-M8U / ZED-F9R and
|
||||||
|
uses callbacks to process and display the ESF data automatically.
|
||||||
|
|
||||||
|
Notes:
|
||||||
|
On the ZED-F9R, each ESF RAW message contains _one_ set of IMU sensor data: seven readings in total (3 x Accel, 3 x Gyro, 1 x Temperature).
|
||||||
|
However, on the NEO-M8U, each message contains _ten_ sets of IMU sensor data, seventy readings in total.
|
||||||
|
The NEO-M8U data is all timestamped and it is possible to reconstruct the full data stream, you just need to do it
|
||||||
|
ten samples at a time...
|
||||||
|
Also, note that the sensor data is 24-bit signed (two's complement). You need to be careful when converting to int32_t.
|
||||||
|
Data will arrive at 100Hz! (10Hz x 10 on the NEO-M8U)
|
||||||
|
400kHz I2C is essential...
|
||||||
|
Serial printing needs to be kept short and the baud rate needs to be at least 230400.
|
||||||
|
|
||||||
|
Please make sure your NEO-M8U is running UDR firmware >= 1.31. Please update using u-center if necessary:
|
||||||
|
https://www.u-blox.com/en/product/neo-m8u-module#tab-documentation-resources
|
||||||
|
|
||||||
|
Feel like supporting open source hardware?
|
||||||
|
Buy a board from SparkFun!
|
||||||
|
NEO-M8U: https://www.sparkfun.com/products/16329
|
||||||
|
|
||||||
|
Hardware Connections:
|
||||||
|
Plug a Qwiic cable into the GPS and a Redboard Qwiic
|
||||||
|
If you don't have a platform with a Qwiic connection use the
|
||||||
|
SparkFun Qwiic Breadboard Jumper (https://www.sparkfun.com/products/14425)
|
||||||
|
Open the serial monitor at 115200 baud to see the output
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <Wire.h> //Needed for I2C to GPS
|
||||||
|
|
||||||
|
#include <SparkFun_u-blox_GNSS_Arduino_Library.h> //http://librarymanager/All#SparkFun_u-blox_GNSS
|
||||||
|
SFE_UBLOX_GNSS myGNSS;
|
||||||
|
|
||||||
|
// Callback: printESFRAWdata will be called when new ESF RAW data arrives
|
||||||
|
// See u-blox_structs.h for the full definition of UBX_ESF_RAW_data_t
|
||||||
|
// _____ You can use any name you like for the callback. Use the same name when you call setAutoESFRAWcallback
|
||||||
|
// / _____ This _must_ be UBX_ESF_RAW_data_t
|
||||||
|
// | / _____ You can use any name you like for the struct
|
||||||
|
// | | /
|
||||||
|
// | | |
|
||||||
|
void printESFRAWdata(UBX_ESF_RAW_data_t *ubxDataStruct)
|
||||||
|
{
|
||||||
|
// ubxDataStruct->numEsfRawBlocks indicates how many sensor readings the UBX_ESF_RAW_data_t contains.
|
||||||
|
// On the ZED-F9R, numEsfRawBlocks will be 7: 3 x Accel, 3 x Gyro, 1 x Temperature.
|
||||||
|
// On the NEO-M8U, numEsfRawBlocks will be 70: 10 sets of sensor data. The sensor time tag (sTag)
|
||||||
|
// indicates the timing of each sample.
|
||||||
|
// Serial output will be approx. 110 bytes depending on how many digits are in the sensor readings.
|
||||||
|
// To keep up, Serial needs to be running at 100k baud minimum. 230400 is recommended.
|
||||||
|
|
||||||
|
uint32_t sTag = 0xFFFFFFFF; // Sensor time tag
|
||||||
|
|
||||||
|
// Only print the first seven sensor readings (on the NEO-M8U)
|
||||||
|
for (uint8_t i = 0; (i < ubxDataStruct->numEsfRawBlocks) && (i < 7); i++)
|
||||||
|
// For fun, and to prove it works, uncomment use this line instead to get the full 100Hz data on the NEO-M8U
|
||||||
|
//for (uint8_t i = 0; i < ubxDataStruct->numEsfRawBlocks; i++)
|
||||||
|
{
|
||||||
|
// Print sTag the first time - and also if it changes
|
||||||
|
if (sTag != ubxDataStruct->data[i].sTag)
|
||||||
|
{
|
||||||
|
sTag = ubxDataStruct->data[i].sTag;
|
||||||
|
Serial.print(F("Time:"));
|
||||||
|
Serial.println(sTag);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Print the sensor data type
|
||||||
|
// From the M8 interface description:
|
||||||
|
// 0: None
|
||||||
|
// 1-4: Reserved
|
||||||
|
// 5: z-axis gyroscope angular rate deg/s * 2^-12 signed
|
||||||
|
// 6: front-left wheel ticks: Bits 0-22: unsigned tick value. Bit 23: direction indicator (0=forward, 1=backward)
|
||||||
|
// 7: front-right wheel ticks: Bits 0-22: unsigned tick value. Bit 23: direction indicator (0=forward, 1=backward)
|
||||||
|
// 8: rear-left wheel ticks: Bits 0-22: unsigned tick value. Bit 23: direction indicator (0=forward, 1=backward)
|
||||||
|
// 9: rear-right wheel ticks: Bits 0-22: unsigned tick value. Bit 23: direction indicator (0=forward, 1=backward)
|
||||||
|
// 10: speed ticks: Bits 0-22: unsigned tick value. Bit 23: direction indicator (0=forward, 1=backward)
|
||||||
|
// 11: speed m/s * 1e-3 signed
|
||||||
|
// 12: gyroscope temperature deg Celsius * 1e-2 signed
|
||||||
|
// 13: y-axis gyroscope angular rate deg/s * 2^-12 signed
|
||||||
|
// 14: x-axis gyroscope angular rate deg/s * 2^-12 signed
|
||||||
|
// 16: x-axis accelerometer specific force m/s^2 * 2^-10 signed
|
||||||
|
// 17: y-axis accelerometer specific force m/s^2 * 2^-10 signed
|
||||||
|
// 18: z-axis accelerometer specific force m/s^2 * 2^-10 signed
|
||||||
|
switch (ubxDataStruct->data[i].data.bits.dataType)
|
||||||
|
{
|
||||||
|
case 5:
|
||||||
|
Serial.print(F("Zgyr:"));
|
||||||
|
break;
|
||||||
|
case 12:
|
||||||
|
Serial.print(F("Temp:"));
|
||||||
|
break;
|
||||||
|
case 13:
|
||||||
|
Serial.print(F("Ygyr:"));
|
||||||
|
break;
|
||||||
|
case 14:
|
||||||
|
Serial.print(F("Xgyr:"));
|
||||||
|
break;
|
||||||
|
case 16:
|
||||||
|
Serial.print(F("Xacc:"));
|
||||||
|
break;
|
||||||
|
case 17:
|
||||||
|
Serial.print(F("Yacc:"));
|
||||||
|
break;
|
||||||
|
case 18:
|
||||||
|
Serial.print(F("Zacc:"));
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Gyro data
|
||||||
|
if ((ubxDataStruct->data[i].data.bits.dataType == 5) || (ubxDataStruct->data[i].data.bits.dataType == 13) || (ubxDataStruct->data[i].data.bits.dataType == 14))
|
||||||
|
{
|
||||||
|
union
|
||||||
|
{
|
||||||
|
int32_t signed32;
|
||||||
|
uint32_t unsigned32;
|
||||||
|
} signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t
|
||||||
|
// The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t
|
||||||
|
signedUnsigned.unsigned32 = ubxDataStruct->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data
|
||||||
|
float rate = signedUnsigned.signed32; // Extract the signed data. Convert to float
|
||||||
|
rate /= 256.0; // Divide by 256 to undo the shift
|
||||||
|
rate *= 0.000244140625; // Convert from deg/s * 2^-12 to deg/s
|
||||||
|
Serial.println(rate);
|
||||||
|
}
|
||||||
|
// Accelerometer data
|
||||||
|
else if ((ubxDataStruct->data[i].data.bits.dataType == 16) || (ubxDataStruct->data[i].data.bits.dataType == 17) || (ubxDataStruct->data[i].data.bits.dataType == 18))
|
||||||
|
{
|
||||||
|
union
|
||||||
|
{
|
||||||
|
int32_t signed32;
|
||||||
|
uint32_t unsigned32;
|
||||||
|
} signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t
|
||||||
|
// The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t
|
||||||
|
signedUnsigned.unsigned32 = ubxDataStruct->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data
|
||||||
|
float force = signedUnsigned.signed32; // Extract the signed data. Convert to float
|
||||||
|
force /= 256.0; // Divide by 256 to undo the shift
|
||||||
|
force *= 0.0009765625; // Convert from m/s^2 * 2^-10 to m/s^2
|
||||||
|
Serial.println(force);
|
||||||
|
}
|
||||||
|
// Gyro Temperature
|
||||||
|
else if (ubxDataStruct->data[i].data.bits.dataType == 12)
|
||||||
|
{
|
||||||
|
union
|
||||||
|
{
|
||||||
|
int32_t signed32;
|
||||||
|
uint32_t unsigned32;
|
||||||
|
} signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t
|
||||||
|
// The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t
|
||||||
|
signedUnsigned.unsigned32 = ubxDataStruct->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data
|
||||||
|
float temperature = signedUnsigned.signed32; // Extract the signed data. Convert to float
|
||||||
|
temperature /= 256.0; // Divide by 256 to undo the shift
|
||||||
|
temperature *= 0.01; // Convert from C * 1e-2 to C
|
||||||
|
Serial.println(temperature);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup()
|
||||||
|
{
|
||||||
|
Serial.begin(230400); // <--- Use >> 100k baud (see notes above)
|
||||||
|
|
||||||
|
while (!Serial); //Wait for user to open terminal
|
||||||
|
Serial.println(F("SparkFun u-blox Example"));
|
||||||
|
|
||||||
|
Wire.begin();
|
||||||
|
Wire.setClock(400000); // <-- Use 400kHz I2C (ESSENTIAL)
|
||||||
|
|
||||||
|
//myGNSS.enableDebugging(); // Uncomment this line to enable debug messages on Serial
|
||||||
|
|
||||||
|
if (myGNSS.begin() == false) //Connect to the u-blox module using Wire port
|
||||||
|
{
|
||||||
|
Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring. Freezing."));
|
||||||
|
while (1);
|
||||||
|
}
|
||||||
|
|
||||||
|
myGNSS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
|
||||||
|
myGNSS.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save (only) the communications port settings to flash and BBR
|
||||||
|
|
||||||
|
myGNSS.setI2CpollingWait(5); //Allow checkUblox to poll I2C data every 5ms to keep up with the ESF RAW messages
|
||||||
|
|
||||||
|
if (myGNSS.setAutoESFRAWcallbackPtr(&printESFRAWdata) == true) // Enable automatic ESF RAW messages with callback to printESFRAWdata
|
||||||
|
Serial.println(F("setAutoESFRAWcallback successful"));
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop()
|
||||||
|
{
|
||||||
|
myGNSS.checkUblox(); // Check for the arrival of new data and process it.
|
||||||
|
myGNSS.checkCallbacks(); // Check if any callbacks are waiting to be processed.
|
||||||
|
}
|
||||||
+209
@@ -0,0 +1,209 @@
|
|||||||
|
/*
|
||||||
|
u-blox Example: ESF RAW (100Hz!)
|
||||||
|
By: Paul Clark
|
||||||
|
SparkFun Electronics
|
||||||
|
Date: September 8th, 2022
|
||||||
|
License: MIT. See license file for more information but you can
|
||||||
|
basically do whatever you want with this code.
|
||||||
|
|
||||||
|
This example configures the External Sensor Fusion RAW IMU sensor messages on the NEO-M8U / ZED-F9R and
|
||||||
|
shows how to access the ESF data in the loop - without using the callback.
|
||||||
|
|
||||||
|
Notes:
|
||||||
|
On the ZED-F9R, each ESF RAW message contains _one_ set of IMU sensor data: seven readings in total (3 x Accel, 3 x Gyro, 1 x Temperature).
|
||||||
|
However, on the NEO-M8U, each message contains _ten_ sets of IMU sensor data, seventy readings in total.
|
||||||
|
The NEO-M8U data is all timestamped and it is possible to reconstruct the full data stream, you just need to do it
|
||||||
|
ten samples at a time...
|
||||||
|
Also, note that the sensor data is 24-bit signed (two's complement). You need to be careful when converting to int32_t.
|
||||||
|
Data will arrive at 100Hz! (10Hz x 10 on the NEO-M8U)
|
||||||
|
400kHz I2C is essential...
|
||||||
|
Serial printing needs to be kept short and the baud rate needs to be at least 230400.
|
||||||
|
|
||||||
|
Please make sure your NEO-M8U is running UDR firmware >= 1.31. Please update using u-center if necessary:
|
||||||
|
https://www.u-blox.com/en/product/neo-m8u-module#tab-documentation-resources
|
||||||
|
|
||||||
|
Feel like supporting open source hardware?
|
||||||
|
Buy a board from SparkFun!
|
||||||
|
NEO-M8U: https://www.sparkfun.com/products/16329
|
||||||
|
|
||||||
|
Hardware Connections:
|
||||||
|
Plug a Qwiic cable into the GPS and a Redboard Qwiic
|
||||||
|
If you don't have a platform with a Qwiic connection use the
|
||||||
|
SparkFun Qwiic Breadboard Jumper (https://www.sparkfun.com/products/14425)
|
||||||
|
Open the serial monitor at 115200 baud to see the output
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <Wire.h> //Needed for I2C to GPS
|
||||||
|
|
||||||
|
#include <SparkFun_u-blox_GNSS_Arduino_Library.h> //http://librarymanager/All#SparkFun_u-blox_GNSS
|
||||||
|
SFE_UBLOX_GNSS myGNSS;
|
||||||
|
|
||||||
|
// Callback: printESFRAWdata will be called when new ESF RAW data arrives
|
||||||
|
// See u-blox_structs.h for the full definition of UBX_ESF_RAW_data_t
|
||||||
|
// _____ You can use any name you like for the callback. Use the same name when you call setAutoESFRAWcallback
|
||||||
|
// / _____ This _must_ be UBX_ESF_RAW_data_t
|
||||||
|
// | / _____ You can use any name you like for the struct
|
||||||
|
// | | /
|
||||||
|
// | | |
|
||||||
|
void printESFRAWdata(UBX_ESF_RAW_data_t *ubxDataStruct)
|
||||||
|
{
|
||||||
|
Serial.println(F("Hey! The ESF RAW callback has been called!"));
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup()
|
||||||
|
{
|
||||||
|
Serial.begin(230400); // <--- Use >> 100k baud (see notes above)
|
||||||
|
|
||||||
|
while (!Serial); //Wait for user to open terminal
|
||||||
|
Serial.println(F("SparkFun u-blox Example"));
|
||||||
|
|
||||||
|
Wire.begin();
|
||||||
|
Wire.setClock(400000); // <-- Use 400kHz I2C (ESSENTIAL)
|
||||||
|
|
||||||
|
//myGNSS.enableDebugging(); // Uncomment this line to enable debug messages on Serial
|
||||||
|
|
||||||
|
if (myGNSS.begin() == false) //Connect to the u-blox module using Wire port
|
||||||
|
{
|
||||||
|
Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring. Freezing."));
|
||||||
|
while (1);
|
||||||
|
}
|
||||||
|
|
||||||
|
myGNSS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
|
||||||
|
myGNSS.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save (only) the communications port settings to flash and BBR
|
||||||
|
|
||||||
|
myGNSS.setI2CpollingWait(5); //Allow checkUblox to poll I2C data every 5ms to keep up with the ESF RAW messages
|
||||||
|
|
||||||
|
if (myGNSS.setAutoESFRAWcallbackPtr(&printESFRAWdata) == true) // Enable automatic ESF RAW messages with callback to printESFRAWdata
|
||||||
|
Serial.println(F("setAutoESFRAWcallback successful"));
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop()
|
||||||
|
{
|
||||||
|
myGNSS.checkUblox(); // Check for the arrival of new data and process it.
|
||||||
|
|
||||||
|
// Check if new ESF RAW data has arrived:
|
||||||
|
// If myGNSS.packetUBXESFRAW->automaticFlags.flags.bits.callbackCopyValid is true, it indicates new ESF RAW data has been received and has been copied.
|
||||||
|
// automaticFlags.flags.bits.callbackCopyValid will be cleared automatically when the callback is called.
|
||||||
|
|
||||||
|
if (myGNSS.packetUBXESFRAW->automaticFlags.flags.bits.callbackCopyValid == true)
|
||||||
|
{
|
||||||
|
// But, we can manually clear the callback flag too. This will prevent the callback from being called!
|
||||||
|
myGNSS.packetUBXESFRAW->automaticFlags.flags.bits.callbackCopyValid = false; // Comment this line if you still want the callback to be called
|
||||||
|
|
||||||
|
// myGNSS.packetUBXESFRAW->callbackData->numEsfRawBlocks indicates how many sensor readings the UBX_ESF_RAW_data_t contains.
|
||||||
|
// On the ZED-F9R, numEsfRawBlocks will be 7: 3 x Accel, 3 x Gyro, 1 x Temperature.
|
||||||
|
// On the NEO-M8U, numEsfRawBlocks will be 70: 10 sets of sensor data. The sensor time tag (sTag)
|
||||||
|
// indicates the timing of each sample.
|
||||||
|
// Serial output will be approx. 110 bytes depending on how many digits are in the sensor readings.
|
||||||
|
// To keep up, Serial needs to be running at 100k baud minimum. 230400 is recommended.
|
||||||
|
|
||||||
|
uint32_t sTag = 0xFFFFFFFF; // Sensor time tag
|
||||||
|
|
||||||
|
// Only print the first seven sensor readings (on the NEO-M8U)
|
||||||
|
for (uint8_t i = 0; (i < myGNSS.packetUBXESFRAW->callbackData->numEsfRawBlocks) && (i < 7); i++)
|
||||||
|
// For fun, and to prove it works, uncomment use this line instead to get the full 100Hz data on the NEO-M8U
|
||||||
|
//for (uint8_t i = 0; i < myGNSS.packetUBXESFRAW->callbackData->numEsfRawBlocks; i++)
|
||||||
|
{
|
||||||
|
// Print sTag the first time - and also if it changes
|
||||||
|
if (sTag != myGNSS.packetUBXESFRAW->callbackData->data[i].sTag)
|
||||||
|
{
|
||||||
|
sTag = myGNSS.packetUBXESFRAW->callbackData->data[i].sTag;
|
||||||
|
Serial.print(F("Time:"));
|
||||||
|
Serial.println(sTag);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Print the sensor data type
|
||||||
|
// From the M8 interface description:
|
||||||
|
// 0: None
|
||||||
|
// 1-4: Reserved
|
||||||
|
// 5: z-axis gyroscope angular rate deg/s * 2^-12 signed
|
||||||
|
// 6: front-left wheel ticks: Bits 0-22: unsigned tick value. Bit 23: direction indicator (0=forward, 1=backward)
|
||||||
|
// 7: front-right wheel ticks: Bits 0-22: unsigned tick value. Bit 23: direction indicator (0=forward, 1=backward)
|
||||||
|
// 8: rear-left wheel ticks: Bits 0-22: unsigned tick value. Bit 23: direction indicator (0=forward, 1=backward)
|
||||||
|
// 9: rear-right wheel ticks: Bits 0-22: unsigned tick value. Bit 23: direction indicator (0=forward, 1=backward)
|
||||||
|
// 10: speed ticks: Bits 0-22: unsigned tick value. Bit 23: direction indicator (0=forward, 1=backward)
|
||||||
|
// 11: speed m/s * 1e-3 signed
|
||||||
|
// 12: gyroscope temperature deg Celsius * 1e-2 signed
|
||||||
|
// 13: y-axis gyroscope angular rate deg/s * 2^-12 signed
|
||||||
|
// 14: x-axis gyroscope angular rate deg/s * 2^-12 signed
|
||||||
|
// 16: x-axis accelerometer specific force m/s^2 * 2^-10 signed
|
||||||
|
// 17: y-axis accelerometer specific force m/s^2 * 2^-10 signed
|
||||||
|
// 18: z-axis accelerometer specific force m/s^2 * 2^-10 signed
|
||||||
|
switch (myGNSS.packetUBXESFRAW->callbackData->data[i].data.bits.dataType)
|
||||||
|
{
|
||||||
|
case 5:
|
||||||
|
Serial.print(F("Zgyr:"));
|
||||||
|
break;
|
||||||
|
case 12:
|
||||||
|
Serial.print(F("Temp:"));
|
||||||
|
break;
|
||||||
|
case 13:
|
||||||
|
Serial.print(F("Ygyr:"));
|
||||||
|
break;
|
||||||
|
case 14:
|
||||||
|
Serial.print(F("Xgyr:"));
|
||||||
|
break;
|
||||||
|
case 16:
|
||||||
|
Serial.print(F("Xacc:"));
|
||||||
|
break;
|
||||||
|
case 17:
|
||||||
|
Serial.print(F("Yacc:"));
|
||||||
|
break;
|
||||||
|
case 18:
|
||||||
|
Serial.print(F("Zacc:"));
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Gyro data
|
||||||
|
if ((myGNSS.packetUBXESFRAW->callbackData->data[i].data.bits.dataType == 5) || (myGNSS.packetUBXESFRAW->callbackData->data[i].data.bits.dataType == 13) || (myGNSS.packetUBXESFRAW->callbackData->data[i].data.bits.dataType == 14))
|
||||||
|
{
|
||||||
|
union
|
||||||
|
{
|
||||||
|
int32_t signed32;
|
||||||
|
uint32_t unsigned32;
|
||||||
|
} signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t
|
||||||
|
// The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t
|
||||||
|
signedUnsigned.unsigned32 = myGNSS.packetUBXESFRAW->callbackData->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data
|
||||||
|
float rate = signedUnsigned.signed32; // Extract the signed data. Convert to float
|
||||||
|
rate /= 256.0; // Divide by 256 to undo the shift
|
||||||
|
rate *= 0.000244140625; // Convert from deg/s * 2^-12 to deg/s
|
||||||
|
Serial.println(rate);
|
||||||
|
}
|
||||||
|
// Accelerometer data
|
||||||
|
else if ((myGNSS.packetUBXESFRAW->callbackData->data[i].data.bits.dataType == 16) || (myGNSS.packetUBXESFRAW->callbackData->data[i].data.bits.dataType == 17) || (myGNSS.packetUBXESFRAW->callbackData->data[i].data.bits.dataType == 18))
|
||||||
|
{
|
||||||
|
union
|
||||||
|
{
|
||||||
|
int32_t signed32;
|
||||||
|
uint32_t unsigned32;
|
||||||
|
} signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t
|
||||||
|
// The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t
|
||||||
|
signedUnsigned.unsigned32 = myGNSS.packetUBXESFRAW->callbackData->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data
|
||||||
|
float force = signedUnsigned.signed32; // Extract the signed data. Convert to float
|
||||||
|
force /= 256.0; // Divide by 256 to undo the shift
|
||||||
|
force *= 0.0009765625; // Convert from m/s^2 * 2^-10 to m/s^2
|
||||||
|
Serial.println(force);
|
||||||
|
}
|
||||||
|
// Gyro Temperature
|
||||||
|
else if (myGNSS.packetUBXESFRAW->callbackData->data[i].data.bits.dataType == 12)
|
||||||
|
{
|
||||||
|
union
|
||||||
|
{
|
||||||
|
int32_t signed32;
|
||||||
|
uint32_t unsigned32;
|
||||||
|
} signedUnsigned; // Avoid any ambiguity casting uint32_t to int32_t
|
||||||
|
// The dataField is 24-bit signed, stored in the 24 LSBs of a uint32_t
|
||||||
|
signedUnsigned.unsigned32 = myGNSS.packetUBXESFRAW->callbackData->data[i].data.bits.dataField << 8; // Shift left by 8 bits to correctly align the data
|
||||||
|
float temperature = signedUnsigned.signed32; // Extract the signed data. Convert to float
|
||||||
|
temperature /= 256.0; // Divide by 256 to undo the shift
|
||||||
|
temperature *= 0.01; // Convert from C * 1e-2 to C
|
||||||
|
Serial.println(temperature);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
myGNSS.checkCallbacks(); // Check if any callbacks are waiting to be processed. There will not be any in this example, unless you commented the line above
|
||||||
|
}
|
||||||
+121
@@ -0,0 +1,121 @@
|
|||||||
|
/*
|
||||||
|
Get the high precision ECEF coordinates using double
|
||||||
|
By: Paul Clark
|
||||||
|
SparkFun Electronics
|
||||||
|
Date: September 8th, 2022
|
||||||
|
License: MIT. See license file for more information but you can
|
||||||
|
basically do whatever you want with this code.
|
||||||
|
|
||||||
|
This example shows how to read the high-precision ECEF
|
||||||
|
positional solution. Please see below for information about the units.
|
||||||
|
|
||||||
|
** This example will only work correctly on platforms which support 64-bit double **
|
||||||
|
|
||||||
|
Feel like supporting open source hardware?
|
||||||
|
Buy a board from SparkFun!
|
||||||
|
ZED-F9P RTK2: https://www.sparkfun.com/products/15136
|
||||||
|
NEO-M8P RTK: https://www.sparkfun.com/products/15005
|
||||||
|
|
||||||
|
Hardware Connections:
|
||||||
|
Plug a Qwiic cable into the GNSS and (e.g.) a Redboard Artemis https://www.sparkfun.com/products/15444
|
||||||
|
or an Artemis Thing Plus https://www.sparkfun.com/products/15574
|
||||||
|
If you don't have a platform with a Qwiic connection use the SparkFun Qwiic Breadboard Jumper (https://www.sparkfun.com/products/14425)
|
||||||
|
Open the serial monitor at 115200 baud to see the output
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <Wire.h> // Needed for I2C to GNSS
|
||||||
|
|
||||||
|
#define myWire Wire // This will work on the Redboard Artemis and the Artemis Thing Plus using Qwiic
|
||||||
|
//#define myWire Wire1 // Uncomment this line if you are using the extra SCL1/SDA1 pins (D17 and D16) on the Thing Plus
|
||||||
|
|
||||||
|
#include <SparkFun_u-blox_GNSS_Arduino_Library.h> //http://librarymanager/All#SparkFun_u-blox_GNSS
|
||||||
|
SFE_UBLOX_GNSS myGNSS;
|
||||||
|
|
||||||
|
long lastTime = 0; //Simple local timer. Limits amount if I2C traffic to u-blox module.
|
||||||
|
|
||||||
|
void setup()
|
||||||
|
{
|
||||||
|
Serial.begin(115200);
|
||||||
|
while (!Serial); //Wait for user to open terminal
|
||||||
|
|
||||||
|
myWire.begin();
|
||||||
|
|
||||||
|
//myGNSS.enableDebugging(Serial); // Uncomment this line to enable debug messages
|
||||||
|
|
||||||
|
if (myGNSS.begin(myWire) == false) //Connect to the u-blox module using Wire port
|
||||||
|
{
|
||||||
|
Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring. Freezing."));
|
||||||
|
while (1)
|
||||||
|
;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check that this platform supports 64-bit (8 byte) double
|
||||||
|
if (sizeof(double) < 8)
|
||||||
|
{
|
||||||
|
Serial.println(F("Warning! Your platform does not support 64-bit double."));
|
||||||
|
Serial.println(F("The ECEF coordinates will be inaccurate."));
|
||||||
|
}
|
||||||
|
|
||||||
|
myGNSS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
|
||||||
|
//myGNSS.saveConfiguration(); //Save the current settings to flash and BBR
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop()
|
||||||
|
{
|
||||||
|
//Query module only every second.
|
||||||
|
//The module only responds when a new position is available.
|
||||||
|
if (millis() - lastTime > 1000)
|
||||||
|
{
|
||||||
|
lastTime = millis(); //Update the timer
|
||||||
|
|
||||||
|
// getHighResECEFX: returns the X coordinate from HPPOSECEF as an int32_t in cm
|
||||||
|
// getHighResECEFXHp: returns the high resolution component of the X coordinate from HPPOSECEF as an int8_t in mm*10^-1 (0.1mm)
|
||||||
|
// getHighResECEFY: returns the Y coordinate from HPPOSECEF as an int32_t in cm
|
||||||
|
// getHighResECEFYHp: returns the high resolution component of the Y coordinate from HPPOSECEF as an int8_t in mm*10^-1 (0.1mm)
|
||||||
|
// getHighResECEFZ: returns the Z coordinate from HPPOSECEF as an int32_t in cm
|
||||||
|
// getHighResECEFZHp: returns the high resolution component of the Z coordinate from HPPOSECEF as an int8_t in mm*10^-1 (0.1mm)
|
||||||
|
// getPositionAccuracy: returns the position accuracy estimate from HPPOSLLH as an uint32_t in mm (note: not 0.1mm)
|
||||||
|
|
||||||
|
// First, let's collect the position data
|
||||||
|
int32_t ECEFX = myGNSS.getHighResECEFX();
|
||||||
|
int8_t ECEFXHp = myGNSS.getHighResECEFXHp();
|
||||||
|
int32_t ECEFY = myGNSS.getHighResECEFY();
|
||||||
|
int8_t ECEFYHp = myGNSS.getHighResECEFYHp();
|
||||||
|
int32_t ECEFZ = myGNSS.getHighResECEFZ();
|
||||||
|
int8_t ECEFZHp = myGNSS.getHighResECEFZHp();
|
||||||
|
uint32_t accuracy = myGNSS.getPositionAccuracy();
|
||||||
|
|
||||||
|
// Defines storage for the ECEF coordinates as double
|
||||||
|
double d_ECEFX;
|
||||||
|
double d_ECEFY;
|
||||||
|
double d_ECEFZ;
|
||||||
|
|
||||||
|
// Assemble the high precision coordinates
|
||||||
|
d_ECEFX = ((double)ECEFX) / 100.0; // Convert from cm to m
|
||||||
|
d_ECEFX += ((double)ECEFXHp) / 10000.0; // Now add the high resolution component ( mm * 10^-1 = m * 10^-4 )
|
||||||
|
d_ECEFY = ((double)ECEFY) / 100.0; // Convert from cm to m
|
||||||
|
d_ECEFY += ((double)ECEFYHp) / 10000.0; // Now add the high resolution component ( mm * 10^-1 = m * 10^-4 )
|
||||||
|
d_ECEFZ = ((double)ECEFZ) / 100.0; // Convert from cm to m
|
||||||
|
d_ECEFZ += ((double)ECEFZHp) / 10000.0; // Now add the high resolution component ( mm * 10^-1 = m * 10^-4 )
|
||||||
|
|
||||||
|
// Print the coordinates with 4 decimal places (0.1mm)
|
||||||
|
Serial.print("X (m): ");
|
||||||
|
Serial.print(d_ECEFX, 4);
|
||||||
|
Serial.print(", Y (m): ");
|
||||||
|
Serial.print(d_ECEFY, 4);
|
||||||
|
Serial.print(", Z (m): ");
|
||||||
|
Serial.print(d_ECEFZ, 4);
|
||||||
|
|
||||||
|
// Now define float storage for the accuracy
|
||||||
|
float f_accuracy;
|
||||||
|
|
||||||
|
// Convert the horizontal accuracy (mm) to a float
|
||||||
|
f_accuracy = accuracy;
|
||||||
|
// Now convert to m
|
||||||
|
f_accuracy = f_accuracy / 1000.0; // Convert from mm to m
|
||||||
|
|
||||||
|
// Finally, do the printing
|
||||||
|
Serial.print(", Accuracy (m): ");
|
||||||
|
Serial.println(f_accuracy, 3); // Print the accuracy with 3 decimal places
|
||||||
|
}
|
||||||
|
}
|
||||||
+6
-2
@@ -484,8 +484,6 @@ assumeAutoESFMEAS KEYWORD2
|
|||||||
flushESFMEAS KEYWORD2
|
flushESFMEAS KEYWORD2
|
||||||
logESFMEAS KEYWORD2
|
logESFMEAS KEYWORD2
|
||||||
|
|
||||||
getEsfRawDataInfo KEYWORD2
|
|
||||||
getESFRAW KEYWORD2
|
|
||||||
setAutoESFRAW KEYWORD2
|
setAutoESFRAW KEYWORD2
|
||||||
setAutoESFRAWrate KEYWORD2
|
setAutoESFRAWrate KEYWORD2
|
||||||
setAutoESFRAWcallback KEYWORD2
|
setAutoESFRAWcallback KEYWORD2
|
||||||
@@ -585,6 +583,12 @@ getMagAcc KEYWORD2
|
|||||||
getHeadVehValid KEYWORD2
|
getHeadVehValid KEYWORD2
|
||||||
|
|
||||||
getPositionAccuracy KEYWORD2
|
getPositionAccuracy KEYWORD2
|
||||||
|
getHighResECEFX KEYWORD2
|
||||||
|
getHighResECEFY KEYWORD2
|
||||||
|
getHighResECEFZ KEYWORD2
|
||||||
|
getHighResECEFXHp KEYWORD2
|
||||||
|
getHighResECEFYHp KEYWORD2
|
||||||
|
getHighResECEFZHp KEYWORD2
|
||||||
|
|
||||||
getTimeOfWeekFromHPPOSLLH KEYWORD2
|
getTimeOfWeekFromHPPOSLLH KEYWORD2
|
||||||
getHighResLongitude KEYWORD2
|
getHighResLongitude KEYWORD2
|
||||||
|
|||||||
+1
-1
@@ -1,5 +1,5 @@
|
|||||||
name=SparkFun u-blox GNSS Arduino Library
|
name=SparkFun u-blox GNSS Arduino Library
|
||||||
version=2.2.13
|
version=2.2.14
|
||||||
author=SparkFun Electronics <techsupport@sparkfun.com>
|
author=SparkFun Electronics <techsupport@sparkfun.com>
|
||||||
maintainer=SparkFun Electronics <sparkfun.com>
|
maintainer=SparkFun Electronics <sparkfun.com>
|
||||||
sentence=Library for I2C, Serial and SPI Communication with u-blox GNSS modules<br/><br/>
|
sentence=Library for I2C, Serial and SPI Communication with u-blox GNSS modules<br/><br/>
|
||||||
|
|||||||
@@ -4246,15 +4246,13 @@ void SFE_UBLOX_GNSS::processUBXpacket(ubxPacket *msg)
|
|||||||
// Parse various byte fields into storage - but only if we have memory allocated for it
|
// Parse various byte fields into storage - but only if we have memory allocated for it
|
||||||
if (packetUBXESFRAW != NULL)
|
if (packetUBXESFRAW != NULL)
|
||||||
{
|
{
|
||||||
for (uint16_t i = 0; (i < DEF_NUM_SENS) && ((i * 8) < (msg->len - 4)); i++)
|
packetUBXESFRAW->data.numEsfRawBlocks = (msg->len - 4) / 8; // Record how many blocks were received. Could be 7 or 70 (ZED-F9R vs. NEO-M8U)
|
||||||
|
for (uint16_t i = 0; (i < (DEF_NUM_SENS * DEF_MAX_NUM_ESF_RAW_REPEATS)) && ((i * 8) < (msg->len - 4)); i++)
|
||||||
{
|
{
|
||||||
packetUBXESFRAW->data.data[i].data.all = extractLong(msg, 4 + (i * 8));
|
packetUBXESFRAW->data.data[i].data.all = extractLong(msg, 4 + (i * 8));
|
||||||
packetUBXESFRAW->data.data[i].sTag = extractLong(msg, 8 + (i * 8));
|
packetUBXESFRAW->data.data[i].sTag = extractLong(msg, 8 + (i * 8));
|
||||||
}
|
}
|
||||||
|
|
||||||
// Mark all datums as fresh (not read before)
|
|
||||||
packetUBXESFRAW->moduleQueried.moduleQueried.all = 0xFFFFFFFF;
|
|
||||||
|
|
||||||
// Check if we need to copy the data for the callback
|
// Check if we need to copy the data for the callback
|
||||||
if ((packetUBXESFRAW->callbackData != NULL) // If RAM has been allocated for the copy of the data
|
if ((packetUBXESFRAW->callbackData != NULL) // If RAM has been allocated for the copy of the data
|
||||||
&& (packetUBXESFRAW->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale
|
&& (packetUBXESFRAW->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale
|
||||||
@@ -10233,7 +10231,7 @@ bool SFE_UBLOX_GNSS::getVehAtt(uint16_t maxWait)
|
|||||||
bool SFE_UBLOX_GNSS::getNAVATT(uint16_t maxWait)
|
bool SFE_UBLOX_GNSS::getNAVATT(uint16_t maxWait)
|
||||||
{
|
{
|
||||||
if (packetUBXNAVATT == NULL)
|
if (packetUBXNAVATT == NULL)
|
||||||
initPacketUBXNAVATT(); // Check that RAM has been allocated for the ESF RAW data
|
initPacketUBXNAVATT(); // Check that RAM has been allocated for the NAV ATT data
|
||||||
if (packetUBXNAVATT == NULL) // Only attempt this if RAM allocation was successful
|
if (packetUBXNAVATT == NULL) // Only attempt this if RAM allocation was successful
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
@@ -10373,7 +10371,7 @@ bool SFE_UBLOX_GNSS::setAutoNAVATTcallbackPtr(void (*callbackPointerPtr)(UBX_NAV
|
|||||||
bool SFE_UBLOX_GNSS::assumeAutoNAVATT(bool enabled, bool implicitUpdate)
|
bool SFE_UBLOX_GNSS::assumeAutoNAVATT(bool enabled, bool implicitUpdate)
|
||||||
{
|
{
|
||||||
if (packetUBXNAVATT == NULL)
|
if (packetUBXNAVATT == NULL)
|
||||||
initPacketUBXNAVATT(); // Check that RAM has been allocated for the ESF RAW data
|
initPacketUBXNAVATT(); // Check that RAM has been allocated for the NAV ATT data
|
||||||
if (packetUBXNAVATT == NULL) // Only attempt this if RAM allocation was successful
|
if (packetUBXNAVATT == NULL) // Only attempt this if RAM allocation was successful
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
@@ -14738,92 +14736,23 @@ void SFE_UBLOX_GNSS::logESFMEAS(bool enabled)
|
|||||||
|
|
||||||
// ***** ESF RAW automatic support
|
// ***** ESF RAW automatic support
|
||||||
|
|
||||||
bool SFE_UBLOX_GNSS::getEsfRawDataInfo(uint16_t maxWait)
|
// ESF RAW messages are output only. They cannot be polled.
|
||||||
{
|
|
||||||
return (getESFRAW(maxWait));
|
|
||||||
}
|
|
||||||
|
|
||||||
bool SFE_UBLOX_GNSS::getESFRAW(uint16_t maxWait)
|
// Enable or disable automatic ESF RAW message generation by the GNSS.
|
||||||
{
|
|
||||||
if (packetUBXESFRAW == NULL)
|
|
||||||
initPacketUBXESFRAW(); // Check that RAM has been allocated for the ESF RAW data
|
|
||||||
if (packetUBXESFRAW == NULL) // Only attempt this if RAM allocation was successful
|
|
||||||
return false;
|
|
||||||
|
|
||||||
if (packetUBXESFRAW->automaticFlags.flags.bits.automatic && packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate)
|
|
||||||
{
|
|
||||||
// The GPS is automatically reporting, we just check whether we got unread data
|
|
||||||
// if (_printDebug == true)
|
|
||||||
// {
|
|
||||||
// _debugSerial->println(F("getEsfRawDataInfo: Autoreporting"));
|
|
||||||
// }
|
|
||||||
checkUbloxInternal(&packetCfg, UBX_CLASS_ESF, UBX_ESF_RAW);
|
|
||||||
return packetUBXESFRAW->moduleQueried.moduleQueried.bits.all;
|
|
||||||
}
|
|
||||||
else if (packetUBXESFRAW->automaticFlags.flags.bits.automatic && !packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate)
|
|
||||||
{
|
|
||||||
// Someone else has to call checkUblox for us...
|
|
||||||
// if (_printDebug == true)
|
|
||||||
// {
|
|
||||||
// _debugSerial->println(F("getEsfRawDataInfo: Exit immediately"));
|
|
||||||
// }
|
|
||||||
return (false);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
// if (_printDebug == true)
|
|
||||||
// {
|
|
||||||
// _debugSerial->println(F("getEsfRawDataInfo: Polling"));
|
|
||||||
// }
|
|
||||||
|
|
||||||
// The GPS is not automatically reporting HNR PVT so we have to poll explicitly
|
|
||||||
packetCfg.cls = UBX_CLASS_ESF;
|
|
||||||
packetCfg.id = UBX_ESF_RAW;
|
|
||||||
packetCfg.len = 0;
|
|
||||||
packetCfg.startingSpot = 0;
|
|
||||||
|
|
||||||
// The data is parsed as part of processing the response
|
|
||||||
sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait);
|
|
||||||
|
|
||||||
if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED)
|
|
||||||
return (true);
|
|
||||||
|
|
||||||
if (retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN)
|
|
||||||
{
|
|
||||||
// if (_printDebug == true)
|
|
||||||
// {
|
|
||||||
// _debugSerial->println(F("getEsfRawDataInfo: data in packetCfg was OVERWRITTEN by another message (but that's OK)"));
|
|
||||||
// }
|
|
||||||
return (true);
|
|
||||||
}
|
|
||||||
|
|
||||||
// if (_printDebug == true)
|
|
||||||
// {
|
|
||||||
// _debugSerial->print(F("getEsfRawDataInfo retVal: "));
|
|
||||||
// _debugSerial->println(statusString(retVal));
|
|
||||||
// }
|
|
||||||
return (false);
|
|
||||||
}
|
|
||||||
|
|
||||||
return (false); // Trap. We should never get here...
|
|
||||||
}
|
|
||||||
|
|
||||||
// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo
|
|
||||||
// works.
|
|
||||||
bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, uint16_t maxWait)
|
bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, uint16_t maxWait)
|
||||||
{
|
{
|
||||||
return setAutoESFRAWrate(enable ? 1 : 0, true, maxWait);
|
return setAutoESFRAWrate(enable ? 1 : 0, true, maxWait);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo
|
// Enable or disable automatic ESF RAW message generation by the GNSS.
|
||||||
// works.
|
|
||||||
bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, bool implicitUpdate, uint16_t maxWait)
|
bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, bool implicitUpdate, uint16_t maxWait)
|
||||||
{
|
{
|
||||||
return setAutoESFRAWrate(enable ? 1 : 0, implicitUpdate, maxWait);
|
return setAutoESFRAWrate(enable ? 1 : 0, implicitUpdate, maxWait);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo
|
// Enable or disable automatic ESF RAW message generation by the GNSS.
|
||||||
// works.
|
// Note: this function can only be used to enable or disable the messages. A rate of zero disables the messages.
|
||||||
|
// A rate of 1 or more causes the messages to be generated at the full 100Hz.
|
||||||
bool SFE_UBLOX_GNSS::setAutoESFRAWrate(uint8_t rate, bool implicitUpdate, uint16_t maxWait)
|
bool SFE_UBLOX_GNSS::setAutoESFRAWrate(uint8_t rate, bool implicitUpdate, uint16_t maxWait)
|
||||||
{
|
{
|
||||||
if (packetUBXESFRAW == NULL)
|
if (packetUBXESFRAW == NULL)
|
||||||
@@ -14848,11 +14777,10 @@ bool SFE_UBLOX_GNSS::setAutoESFRAWrate(uint8_t rate, bool implicitUpdate, uint16
|
|||||||
packetUBXESFRAW->automaticFlags.flags.bits.automatic = (rate > 0);
|
packetUBXESFRAW->automaticFlags.flags.bits.automatic = (rate > 0);
|
||||||
packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate = implicitUpdate;
|
packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate = implicitUpdate;
|
||||||
}
|
}
|
||||||
packetUBXESFRAW->moduleQueried.moduleQueried.bits.all = false; // Mark data as stale
|
|
||||||
return ok;
|
return ok;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Enable automatic navigation message generation by the GNSS.
|
// Enable automatic message generation by the GNSS.
|
||||||
bool SFE_UBLOX_GNSS::setAutoESFRAWcallback(void (*callbackPointer)(UBX_ESF_RAW_data_t), uint16_t maxWait)
|
bool SFE_UBLOX_GNSS::setAutoESFRAWcallback(void (*callbackPointer)(UBX_ESF_RAW_data_t), uint16_t maxWait)
|
||||||
{
|
{
|
||||||
// Enable auto messages. Set implicitUpdate to false as we expect the user to call checkUblox manually.
|
// Enable auto messages. Set implicitUpdate to false as we expect the user to call checkUblox manually.
|
||||||
@@ -14937,7 +14865,6 @@ bool SFE_UBLOX_GNSS::initPacketUBXESFRAW()
|
|||||||
packetUBXESFRAW->callbackPointer = NULL;
|
packetUBXESFRAW->callbackPointer = NULL;
|
||||||
packetUBXESFRAW->callbackPointerPtr = NULL;
|
packetUBXESFRAW->callbackPointerPtr = NULL;
|
||||||
packetUBXESFRAW->callbackData = NULL;
|
packetUBXESFRAW->callbackData = NULL;
|
||||||
packetUBXESFRAW->moduleQueried.moduleQueried.all = 0;
|
|
||||||
return (true);
|
return (true);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -14946,7 +14873,6 @@ void SFE_UBLOX_GNSS::flushESFRAW()
|
|||||||
{
|
{
|
||||||
if (packetUBXESFRAW == NULL)
|
if (packetUBXESFRAW == NULL)
|
||||||
return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
|
return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
|
||||||
packetUBXESFRAW->moduleQueried.moduleQueried.all = 0; // Mark all datums as stale (read before)
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Log this data in file buffer
|
// Log this data in file buffer
|
||||||
@@ -17527,6 +17453,108 @@ uint32_t SFE_UBLOX_GNSS::getPositionAccuracy(uint16_t maxWait)
|
|||||||
return (tempAccuracy);
|
return (tempAccuracy);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Get the current 3D high precision X coordinate
|
||||||
|
// Returns a long representing the coordinate in cm
|
||||||
|
int32_t SFE_UBLOX_GNSS::getHighResECEFX(uint16_t maxWait)
|
||||||
|
{
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL)
|
||||||
|
initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefX == false)
|
||||||
|
getNAVHPPOSECEF(maxWait);
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefX = false; // Since we are about to give this to user, mark this data as stale
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false;
|
||||||
|
|
||||||
|
return (packetUBXNAVHPPOSECEF->data.ecefX);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the current 3D high precision Y coordinate
|
||||||
|
// Returns a long representing the coordinate in cm
|
||||||
|
int32_t SFE_UBLOX_GNSS::getHighResECEFY(uint16_t maxWait)
|
||||||
|
{
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL)
|
||||||
|
initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefY == false)
|
||||||
|
getNAVHPPOSECEF(maxWait);
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefY = false; // Since we are about to give this to user, mark this data as stale
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false;
|
||||||
|
|
||||||
|
return (packetUBXNAVHPPOSECEF->data.ecefY);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the current 3D high precision Z coordinate
|
||||||
|
// Returns a long representing the coordinate in cm
|
||||||
|
int32_t SFE_UBLOX_GNSS::getHighResECEFZ(uint16_t maxWait)
|
||||||
|
{
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL)
|
||||||
|
initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefZ == false)
|
||||||
|
getNAVHPPOSECEF(maxWait);
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefZ = false; // Since we are about to give this to user, mark this data as stale
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false;
|
||||||
|
|
||||||
|
return (packetUBXNAVHPPOSECEF->data.ecefZ);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the high precision component of the ECEF X coordinate
|
||||||
|
// Returns a signed byte representing the component as 0.1*mm
|
||||||
|
int8_t SFE_UBLOX_GNSS::getHighResECEFXHp(uint16_t maxWait)
|
||||||
|
{
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL)
|
||||||
|
initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefXHp == false)
|
||||||
|
getNAVHPPOSECEF(maxWait);
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefXHp = false; // Since we are about to give this to user, mark this data as stale
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false;
|
||||||
|
|
||||||
|
return (packetUBXNAVHPPOSECEF->data.ecefXHp);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the high precision component of the ECEF Y coordinate
|
||||||
|
// Returns a signed byte representing the component as 0.1*mm
|
||||||
|
int8_t SFE_UBLOX_GNSS::getHighResECEFYHp(uint16_t maxWait)
|
||||||
|
{
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL)
|
||||||
|
initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefYHp == false)
|
||||||
|
getNAVHPPOSECEF(maxWait);
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefYHp = false; // Since we are about to give this to user, mark this data as stale
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false;
|
||||||
|
|
||||||
|
return (packetUBXNAVHPPOSECEF->data.ecefYHp);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the high precision component of the ECEF Z coordinate
|
||||||
|
// Returns a signed byte representing the component as 0.1*mm
|
||||||
|
int8_t SFE_UBLOX_GNSS::getHighResECEFZHp(uint16_t maxWait)
|
||||||
|
{
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL)
|
||||||
|
initPacketUBXNAVHPPOSECEF(); // Check that RAM has been allocated for the HPPOSECEF data
|
||||||
|
if (packetUBXNAVHPPOSECEF == NULL) // Bail if the RAM allocation failed
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
if (packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefZHp == false)
|
||||||
|
getNAVHPPOSECEF(maxWait);
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.ecefZHp = false; // Since we are about to give this to user, mark this data as stale
|
||||||
|
packetUBXNAVHPPOSECEF->moduleQueried.moduleQueried.bits.all = false;
|
||||||
|
|
||||||
|
return (packetUBXNAVHPPOSECEF->data.ecefZHp);
|
||||||
|
}
|
||||||
|
|
||||||
// ***** HPPOSLLH Helper Functions
|
// ***** HPPOSLLH Helper Functions
|
||||||
|
|
||||||
uint32_t SFE_UBLOX_GNSS::getTimeOfWeekFromHPPOSLLH(uint16_t maxWait)
|
uint32_t SFE_UBLOX_GNSS::getTimeOfWeekFromHPPOSLLH(uint16_t maxWait)
|
||||||
@@ -18039,22 +18067,6 @@ bool SFE_UBLOX_GNSS::getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *senso
|
|||||||
return (true);
|
return (true);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SFE_UBLOX_GNSS::getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait)
|
|
||||||
{
|
|
||||||
if (packetUBXESFRAW == NULL)
|
|
||||||
initPacketUBXESFRAW(); // Check that RAM has been allocated for the ESF RAW data
|
|
||||||
if (packetUBXESFRAW == NULL) // Bail if the RAM allocation failed
|
|
||||||
return (false);
|
|
||||||
|
|
||||||
if ((packetUBXESFRAW->moduleQueried.moduleQueried.bits.data & (1 << sensor)) == 0)
|
|
||||||
getESFRAW(maxWait);
|
|
||||||
packetUBXESFRAW->moduleQueried.moduleQueried.bits.data &= ~(1 << sensor); // Since we are about to give this to user, mark this data as stale
|
|
||||||
packetUBXESFRAW->moduleQueried.moduleQueried.bits.all = false;
|
|
||||||
sensorData->data.all = packetUBXESFRAW->data.data[sensor].data.all;
|
|
||||||
sensorData->sTag = packetUBXESFRAW->data.data[sensor].sTag;
|
|
||||||
return (true);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool SFE_UBLOX_GNSS::getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, UBX_ESF_RAW_data_t ubxDataStruct, uint8_t sensor)
|
bool SFE_UBLOX_GNSS::getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, UBX_ESF_RAW_data_t ubxDataStruct, uint8_t sensor)
|
||||||
{
|
{
|
||||||
sensorData->data.all = ubxDataStruct.data[sensor].data.all;
|
sensorData->data.all = ubxDataStruct.data[sensor].data.all;
|
||||||
|
|||||||
@@ -1283,8 +1283,6 @@ public:
|
|||||||
void flushESFMEAS(); // Mark all the data as read/stale
|
void flushESFMEAS(); // Mark all the data as read/stale
|
||||||
void logESFMEAS(bool enabled = true); // Log data to file buffer
|
void logESFMEAS(bool enabled = true); // Log data to file buffer
|
||||||
|
|
||||||
bool getEsfRawDataInfo(uint16_t maxWait = defaultMaxWait); // ESF RAW Helper
|
|
||||||
bool getESFRAW(uint16_t maxWait = defaultMaxWait); // ESF RAW
|
|
||||||
bool setAutoESFRAW(bool enabled, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic ESF RAW reports
|
bool setAutoESFRAW(bool enabled, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic ESF RAW reports
|
||||||
bool setAutoESFRAW(bool enabled, bool implicitUpdate, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic ESF RAW reports, with implicitUpdate == false accessing stale data will not issue parsing of data in the rxbuffer of your interface, instead you have to call checkUblox when you want to perform an update
|
bool setAutoESFRAW(bool enabled, bool implicitUpdate, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic ESF RAW reports, with implicitUpdate == false accessing stale data will not issue parsing of data in the rxbuffer of your interface, instead you have to call checkUblox when you want to perform an update
|
||||||
bool setAutoESFRAWrate(uint8_t rate, bool implicitUpdate = true, uint16_t maxWait = defaultMaxWait); // Set the rate for automatic RAW reports
|
bool setAutoESFRAWrate(uint8_t rate, bool implicitUpdate = true, uint16_t maxWait = defaultMaxWait); // Set the rate for automatic RAW reports
|
||||||
@@ -1408,6 +1406,12 @@ public:
|
|||||||
// Helper functions for HPPOSECEF
|
// Helper functions for HPPOSECEF
|
||||||
|
|
||||||
uint32_t getPositionAccuracy(uint16_t maxWait = defaultMaxWait); // Returns the 3D accuracy of the current high-precision fix, in mm. Supported on NEO-M8P, ZED-F9P,
|
uint32_t getPositionAccuracy(uint16_t maxWait = defaultMaxWait); // Returns the 3D accuracy of the current high-precision fix, in mm. Supported on NEO-M8P, ZED-F9P,
|
||||||
|
int32_t getHighResECEFX(uint16_t maxWait = defaultMaxWait); // Returns the ECEF X coordinate (cm)
|
||||||
|
int32_t getHighResECEFY(uint16_t maxWait = defaultMaxWait); // Returns the ECEF Y coordinate (cm)
|
||||||
|
int32_t getHighResECEFZ(uint16_t maxWait = defaultMaxWait); // Returns the ECEF Z coordinate (cm)
|
||||||
|
int8_t getHighResECEFXHp(uint16_t maxWait = defaultMaxWait); // Returns the ECEF X coordinate High Precision Component (0.1 mm)
|
||||||
|
int8_t getHighResECEFYHp(uint16_t maxWait = defaultMaxWait); // Returns the ECEF Y coordinate High Precision Component (0.1 mm)
|
||||||
|
int8_t getHighResECEFZHp(uint16_t maxWait = defaultMaxWait); // Returns the ECEF Z coordinate High Precision Component (0.1 mm)
|
||||||
|
|
||||||
// Helper functions for HPPOSLLH
|
// Helper functions for HPPOSLLH
|
||||||
|
|
||||||
@@ -1464,7 +1468,6 @@ public:
|
|||||||
float getESFyaw(uint16_t maxWait = defaultMaxWait); // Returned as degrees
|
float getESFyaw(uint16_t maxWait = defaultMaxWait); // Returned as degrees
|
||||||
bool getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait = defaultMaxWait);
|
bool getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait = defaultMaxWait);
|
||||||
bool getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *sensorData, UBX_ESF_MEAS_data_t ubxDataStruct, uint8_t sensor);
|
bool getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *sensorData, UBX_ESF_MEAS_data_t ubxDataStruct, uint8_t sensor);
|
||||||
bool getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait = defaultMaxWait);
|
|
||||||
bool getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, UBX_ESF_RAW_data_t ubxDataStruct, uint8_t sensor);
|
bool getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, UBX_ESF_RAW_data_t ubxDataStruct, uint8_t sensor);
|
||||||
bool getSensorFusionStatus(UBX_ESF_STATUS_sensorStatus_t *sensorStatus, uint8_t sensor, uint16_t maxWait = defaultMaxWait);
|
bool getSensorFusionStatus(UBX_ESF_STATUS_sensorStatus_t *sensorStatus, uint8_t sensor, uint16_t maxWait = defaultMaxWait);
|
||||||
bool getSensorFusionStatus(UBX_ESF_STATUS_sensorStatus_t *sensorStatus, UBX_ESF_STATUS_data_t ubxDataStruct, uint8_t sensor);
|
bool getSensorFusionStatus(UBX_ESF_STATUS_sensorStatus_t *sensorStatus, UBX_ESF_STATUS_data_t ubxDataStruct, uint8_t sensor);
|
||||||
|
|||||||
+10
-17
@@ -49,6 +49,10 @@
|
|||||||
#define DEF_NUM_SENS 7 // The maximum number of ESF sensors
|
#define DEF_NUM_SENS 7 // The maximum number of ESF sensors
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef DEF_MAX_NUM_ESF_RAW_REPEATS
|
||||||
|
#define DEF_MAX_NUM_ESF_RAW_REPEATS 10 // The NEO-M8U sends ESF RAW data in blocks / sets of ten readings. (The ZED-F9R sends them one at a time.)
|
||||||
|
#endif
|
||||||
|
|
||||||
// Additional flags and pointers that need to be stored with each message type
|
// Additional flags and pointers that need to be stored with each message type
|
||||||
struct ubxAutomaticFlags
|
struct ubxAutomaticFlags
|
||||||
{
|
{
|
||||||
@@ -2257,7 +2261,10 @@ typedef struct
|
|||||||
|
|
||||||
// UBX-ESF-RAW (0x10 0x03): Raw sensor measurements
|
// UBX-ESF-RAW (0x10 0x03): Raw sensor measurements
|
||||||
// Note: length is variable
|
// Note: length is variable
|
||||||
const uint16_t UBX_ESF_RAW_MAX_LEN = 4 + (8 * DEF_NUM_SENS);
|
// Note: The ZED-F9R sends sets of seven sensor readings one at a time
|
||||||
|
// But the NEO-M8U sends them in sets of ten (i.e. seventy readings per message)
|
||||||
|
// Note: ESF RAW data cannot be polled. It is "Output" only
|
||||||
|
const uint16_t UBX_ESF_RAW_MAX_LEN = 4 + (8 * DEF_NUM_SENS * DEF_MAX_NUM_ESF_RAW_REPEATS);
|
||||||
|
|
||||||
typedef struct
|
typedef struct
|
||||||
{
|
{
|
||||||
@@ -2276,28 +2283,14 @@ typedef struct
|
|||||||
typedef struct
|
typedef struct
|
||||||
{
|
{
|
||||||
uint8_t reserved1[4];
|
uint8_t reserved1[4];
|
||||||
UBX_ESF_RAW_sensorData_t data[DEF_NUM_SENS];
|
UBX_ESF_RAW_sensorData_t data[DEF_NUM_SENS * DEF_MAX_NUM_ESF_RAW_REPEATS];
|
||||||
|
uint8_t numEsfRawBlocks; // Note: this is not contained in the ESF RAW message. It is calculated from the message length.
|
||||||
} UBX_ESF_RAW_data_t;
|
} UBX_ESF_RAW_data_t;
|
||||||
|
|
||||||
typedef struct
|
|
||||||
{
|
|
||||||
union
|
|
||||||
{
|
|
||||||
uint32_t all;
|
|
||||||
struct
|
|
||||||
{
|
|
||||||
uint32_t all : 1;
|
|
||||||
|
|
||||||
uint32_t data : DEF_NUM_SENS;
|
|
||||||
} bits;
|
|
||||||
} moduleQueried;
|
|
||||||
} UBX_ESF_RAW_moduleQueried_t;
|
|
||||||
|
|
||||||
typedef struct
|
typedef struct
|
||||||
{
|
{
|
||||||
ubxAutomaticFlags automaticFlags;
|
ubxAutomaticFlags automaticFlags;
|
||||||
UBX_ESF_RAW_data_t data;
|
UBX_ESF_RAW_data_t data;
|
||||||
UBX_ESF_RAW_moduleQueried_t moduleQueried;
|
|
||||||
void (*callbackPointer)(UBX_ESF_RAW_data_t);
|
void (*callbackPointer)(UBX_ESF_RAW_data_t);
|
||||||
void (*callbackPointerPtr)(UBX_ESF_RAW_data_t *);
|
void (*callbackPointerPtr)(UBX_ESF_RAW_data_t *);
|
||||||
UBX_ESF_RAW_data_t *callbackData;
|
UBX_ESF_RAW_data_t *callbackData;
|
||||||
|
|||||||
Reference in New Issue
Block a user