ubxtool: Better decode of GPS subframe 5.

Some of it is correct, some not, not sure what is going on.

1 files changed, 80 insertions, 8 deletions

diff --git a/ubxtool b/ubxtool
index 51cd8f33..e604b1b9 100755
--- a/ubxtool
+++ b/ubxtool
@@ -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))