diff options
-rwxr-xr-x | ubxtool | 88 |
1 files changed, 80 insertions, 8 deletions
@@ -25,6 +25,9 @@ usage: ubxtool [OPTIONS] [server[:port[:device]]] # # To read GPS messages a log file: # ubxtool -v 2 -f test/daemon/ublox-neo-m8n.log +# +# References: +# [1] IS-GPS-200J from __future__ import absolute_import, print_function, division @@ -126,6 +129,28 @@ opts = { # I'd like to use pypy module bitstring or bitarray, but # people complain when non stock python modules are used here. +def unpack_s11(word, pos): + """Grab a signed 11 bits from offset pos of word""" + + bytes = bytearray(2) + bytes[0] = (word >> pos) & 0xff + bytes[1] = (word >> (pos + 8)) & 0x07 + if 0x04 & bytes[1]: + # extend the sign + bytes[1] |= 0xf8 + u = struct.unpack_from('<h', bytes, 0) + return u[0] + + +def unpack_s11s(word): + """Grab the weird split signed 11 bits from word""" + + newword = (word >> 22) & 0xff + newword <<= 3 + newword |= (word >> 8) & 0x07 + return unpack_s11(newword, 0) + + def unpack_s16(word, pos): """Grab a signed two bytes from offset pos of word""" @@ -163,6 +188,35 @@ def unpack_s22(word, pos): return u[0] +def unpack_s24(word, pos): + """Grab a signed 24 bits from offset pos of word""" + + bytes = bytearray(4) + bytes[0] = (word >> pos) & 0xff + bytes[1] = (word >> (pos + 8)) & 0xff + bytes[2] = (word >> (pos + 16)) & 0xff + bytes[3] = 0 + if 0x80 & bytes[2]: + # extend the sign + bytes[3] = 0xff + + u = struct.unpack_from('<l', bytes, 0) + return u[0] + + +def unpack_u24(word, pos): + """Grab an unsigned 24 bits from offset pos of word""" + + bytes = bytearray(4) + bytes[0] = (word >> pos) & 0xff + bytes[1] = (word >> (pos + 8)) & 0xff + bytes[2] = (word >> (pos + 16)) & 0xff + bytes[3] = 0 + + u = struct.unpack_from('<L', bytes, 0) + return u[0] + + def unpack_s8(word, pos): """Grab a signed byte from offset pos of word""" @@ -3272,29 +3326,38 @@ class ubx(object): # decode GPS subframe 5, pages 1 to 24, # and subframe 4, pages 2 to 5, and 7 to 10 + # [1] Section 20.3.3.5, Figure 20-1 Sheet 4, and Table 20-VI. def almanac(self, words): """Decode GPS Almanac""" - # Note: not really tested! + # Note: not really tested! What to test against? # e = Eccentricity # toa = Almanac reference time - # deltan = Mean Motion Difference From Computed Value + # Deltan = Mean Motion Difference From Computed Value # Omegadot = Rate of Right Ascension # sqrtA = Square Root of the Semi-Major Axis # deltai # Omega0 = Longitude of Ascending Node of Orbit Plane at Weekly Epoch # omega = Argument of Perigee # M0 = Mean Anomaly at Reference Time - # af0 - # af1 + # af0 = SV Clock Bias Correction Coefficient + # af1 = SV Clock Drift Correction Coefficient s = " Almanac" - s += ("\n e %e toa %u deltai %e Omegadot %e SVH x%x\n" % + s += ("\n e %e toa %u deltai %e Omegadot %e" + "\n SVH x%x sqrtA %.6f Omega0 %e omega %e" + "\n M0 %e af0 %e af1 %e" % (unpack_u16(words[2], 6) * (2 ** -21), unpack_u8(words[3], 22) * (2 ** 12), unpack_s16(words[3], 6) * (2 ** -19), unpack_s16(words[4], 14) * (2 ** -38), - unpack_u8(words[4], 6))) + unpack_u8(words[4], 6), + unpack_u24(words[5], 6) * (2 ** -11), + unpack_s24(words[6], 6) * (2 ** -23), + unpack_s24(words[7], 6) * (2 ** -23), + unpack_s24(words[8], 6) * (2 ** -23), + unpack_s11s(words[9]) * (2 ** -38), + unpack_s11(words[9], 11) * (2 ** -38))) return s cnav_msgids = { @@ -3315,6 +3378,7 @@ class ubx(object): } # map subframe 4 SV ID to Page number + # IS-GPS-200J Table 20-V sbfr4_svid_page = { 57: 1, 25: 2, @@ -3344,6 +3408,7 @@ class ubx(object): } # map subframe 5 SV ID to Page number + # IS-GPS-200J Table 20-V sbfr5_svid_page = { 1: 1, 2: 2, @@ -3356,7 +3421,7 @@ class ubx(object): 9: 9, 10: 10, 11: 11, - 21: 12, + 12: 12, 13: 13, 14: 14, 15: 15, @@ -3410,6 +3475,7 @@ class ubx(object): return " Bad Length %s" % m_len u = struct.unpack_from('<BBBBBBBB', buf, 0) + svId = u[1] s = (' gnssId %u svId %3u reserved1 %u freqId %u numWords %u\n' ' reserved2 %u version %u reserved3 %u\n' % u) s += ' dwrd' @@ -3510,7 +3576,12 @@ class ubx(object): # except for pages 13, 18, 15 # as of 2018, dataid is always 1. svid = (words[2] >> 22) & 0x3f - page = index_s(svid, self.sbfr4_svid_page) + if 0 < svid: + page = index_s(svid, self.sbfr4_svid_page) + else: + # page of zero means the svId that sent it. + page = "%d/Self" % svId + s += ("\n dataid %u svid %u (page %s)\n" % (words[2] >> 28, svid, page)) @@ -3554,6 +3625,7 @@ class ubx(object): elif 5 == subframe: svid = (words[2] >> 22) & 0x3f page = index_s(svid, self.sbfr5_svid_page) + s += ("\n dataid %u svid %u (page %s)\n" % (words[2] >> 28, svid, page)) |