diff --git a/Utils/UBX_RAWX_Aligner.py b/Utils/UBX_RAWX_Aligner.py new file mode 100644 index 00000000..c18713d8 --- /dev/null +++ b/Utils/UBX_RAWX_Aligner.py @@ -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(' largest_rawx_alignment): + largest_rawx_alignment = abs(rcvTow - rcvTowRounded) # Record the largest alignment change + fileBytes = struct.pack(' 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!') diff --git a/examples/Callbacks/CallbackExample10_ESF_RAW/CallbackExample10_ESF_RAW.ino b/examples/Callbacks/CallbackExample10_ESF_RAW/CallbackExample10_ESF_RAW.ino new file mode 100644 index 00000000..67632a20 --- /dev/null +++ b/examples/Callbacks/CallbackExample10_ESF_RAW/CallbackExample10_ESF_RAW.ino @@ -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 //Needed for I2C to GPS + +#include //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. +} diff --git a/examples/Callbacks/CallbackExample11_ESF_RAW_In_Loop/CallbackExample11_ESF_RAW_In_Loop.ino b/examples/Callbacks/CallbackExample11_ESF_RAW_In_Loop/CallbackExample11_ESF_RAW_In_Loop.ino new file mode 100644 index 00000000..c8fd7ca8 --- /dev/null +++ b/examples/Callbacks/CallbackExample11_ESF_RAW_In_Loop/CallbackExample11_ESF_RAW_In_Loop.ino @@ -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 //Needed for I2C to GPS + +#include //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 +} diff --git a/examples/ZED-F9P/Example21_GetHighPrecisionECEFUsingDouble/Example21_GetHighPrecisionECEFUsingDouble.ino b/examples/ZED-F9P/Example21_GetHighPrecisionECEFUsingDouble/Example21_GetHighPrecisionECEFUsingDouble.ino new file mode 100644 index 00000000..538e33d0 --- /dev/null +++ b/examples/ZED-F9P/Example21_GetHighPrecisionECEFUsingDouble/Example21_GetHighPrecisionECEFUsingDouble.ino @@ -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 // 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 //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 + } +} diff --git a/keywords.txt b/keywords.txt index 206b7513..752e31d0 100644 --- a/keywords.txt +++ b/keywords.txt @@ -484,8 +484,6 @@ assumeAutoESFMEAS KEYWORD2 flushESFMEAS KEYWORD2 logESFMEAS KEYWORD2 -getEsfRawDataInfo KEYWORD2 -getESFRAW KEYWORD2 setAutoESFRAW KEYWORD2 setAutoESFRAWrate KEYWORD2 setAutoESFRAWcallback KEYWORD2 @@ -585,6 +583,12 @@ getMagAcc KEYWORD2 getHeadVehValid KEYWORD2 getPositionAccuracy KEYWORD2 +getHighResECEFX KEYWORD2 +getHighResECEFY KEYWORD2 +getHighResECEFZ KEYWORD2 +getHighResECEFXHp KEYWORD2 +getHighResECEFYHp KEYWORD2 +getHighResECEFZHp KEYWORD2 getTimeOfWeekFromHPPOSLLH KEYWORD2 getHighResLongitude KEYWORD2 diff --git a/library.properties b/library.properties index b202b648..f0c174ff 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SparkFun u-blox GNSS Arduino Library -version=2.2.13 +version=2.2.14 author=SparkFun Electronics maintainer=SparkFun Electronics sentence=Library for I2C, Serial and SPI Communication with u-blox GNSS modules

diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp index 6033a73b..a7e37911 100644 --- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp +++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp @@ -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 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].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 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 @@ -10233,7 +10231,7 @@ bool SFE_UBLOX_GNSS::getVehAtt(uint16_t maxWait) bool SFE_UBLOX_GNSS::getNAVATT(uint16_t maxWait) { 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 return false; @@ -10373,7 +10371,7 @@ bool SFE_UBLOX_GNSS::setAutoNAVATTcallbackPtr(void (*callbackPointerPtr)(UBX_NAV bool SFE_UBLOX_GNSS::assumeAutoNAVATT(bool enabled, bool implicitUpdate) { 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 return false; @@ -14738,92 +14736,23 @@ void SFE_UBLOX_GNSS::logESFMEAS(bool enabled) // ***** ESF RAW automatic support -bool SFE_UBLOX_GNSS::getEsfRawDataInfo(uint16_t maxWait) -{ - return (getESFRAW(maxWait)); -} - -bool SFE_UBLOX_GNSS::getESFRAW(uint16_t maxWait) -{ - 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; +// ESF RAW messages are output only. They cannot be polled. - 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. +// Enable or disable automatic ESF RAW message generation by the GNSS. bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, uint16_t maxWait) { return setAutoESFRAWrate(enable ? 1 : 0, true, maxWait); } -// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo -// works. +// Enable or disable automatic ESF RAW message generation by the GNSS. bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, bool implicitUpdate, uint16_t maxWait) { return setAutoESFRAWrate(enable ? 1 : 0, implicitUpdate, maxWait); } -// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo -// works. +// Enable or disable automatic ESF RAW message generation by the GNSS. +// 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) { 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.implicitUpdate = implicitUpdate; } - packetUBXESFRAW->moduleQueried.moduleQueried.bits.all = false; // Mark data as stale 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) { // 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->callbackPointerPtr = NULL; packetUBXESFRAW->callbackData = NULL; - packetUBXESFRAW->moduleQueried.moduleQueried.all = 0; return (true); } @@ -14946,7 +14873,6 @@ void SFE_UBLOX_GNSS::flushESFRAW() { if (packetUBXESFRAW == NULL) 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 @@ -17527,6 +17453,108 @@ uint32_t SFE_UBLOX_GNSS::getPositionAccuracy(uint16_t maxWait) 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 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); } -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) { sensorData->data.all = ubxDataStruct.data[sensor].data.all; diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.h b/src/SparkFun_u-blox_GNSS_Arduino_Library.h index 763b86f7..e608fe82 100644 --- a/src/SparkFun_u-blox_GNSS_Arduino_Library.h +++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.h @@ -1283,8 +1283,6 @@ class SFE_UBLOX_GNSS void flushESFMEAS(); // Mark all the data as read/stale 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, 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 @@ -1408,6 +1406,12 @@ class SFE_UBLOX_GNSS // 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, + 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 @@ -1464,7 +1468,6 @@ class SFE_UBLOX_GNSS 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, 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 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); diff --git a/src/u-blox_structs.h b/src/u-blox_structs.h index 80ca38a3..bd7c875c 100644 --- a/src/u-blox_structs.h +++ b/src/u-blox_structs.h @@ -49,6 +49,10 @@ #define DEF_NUM_SENS 7 // The maximum number of ESF sensors #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 struct ubxAutomaticFlags { @@ -2257,7 +2261,10 @@ typedef struct // UBX-ESF-RAW (0x10 0x03): Raw sensor measurements // 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 { @@ -2276,28 +2283,14 @@ typedef struct typedef struct { 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; -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 { ubxAutomaticFlags automaticFlags; UBX_ESF_RAW_data_t data; - UBX_ESF_RAW_moduleQueried_t moduleQueried; void (*callbackPointer)(UBX_ESF_RAW_data_t); void (*callbackPointerPtr)(UBX_ESF_RAW_data_t *); UBX_ESF_RAW_data_t *callbackData;