diff --git a/conf/example/frameworkd.conf b/conf/example/frameworkd.conf index ec6e84a..89fdbb4 100644 --- a/conf/example/frameworkd.conf +++ b/conf/example/frameworkd.conf @@ -53,6 +53,14 @@ device = UBXDevice channel = SerialChannel path = /dev/ttyACM0 log_level = INFO +# print the status of every channel (SV tracked, etc) every X seconds. +# 0 to disable. +svprint_interval = 60 + +# Logging of UBX packet processing is managed separately as it can be very +# verbose. +[ogpsd.ubx] +log_level = INFO [opreferencesd] log_level = DEBUG diff --git a/framework/subsystems/ogpsd/ubx.py b/framework/subsystems/ogpsd/ubx.py index b6718cc..f4857b0 100644 --- a/framework/subsystems/ogpsd/ubx.py +++ b/framework/subsystems/ogpsd/ubx.py @@ -14,10 +14,16 @@ import struct import dbus from gpsdevice import GPSDevice import calendar +import time + +from framework.config import config import logging logger = logging.getLogger('ogpsd') +# Separate logger for ubx packet parsing for extra control. +ubxpkt_logger = logging.getLogger('ogpsd.ubx') +CONFIG_MODULE = "ogpsd" DBUS_INTERFACE = "org.freesmartphone.GPS" SYNC1=0xb5 @@ -257,6 +263,58 @@ MSGFMT = { # TIM - Timekeeping } +# self.gpsfixstatus value constants. +STATUS_NO_FIX = 1 +STATUS_2D_FIX = 2 +STATUS_3D_FIX = 3 + +# status to readable names +status_to_name = { + STATUS_NO_FIX: "No", + STATUS_2D_FIX: "2D", + STATUS_3D_FIX: "3D" +} + +# GPSfix field values (from UBX spec) +GPSFIX_NONE = 0x0 +GPSFIX_DEAD_RECKONING = 0x01 +GPSFIX_2D = 0x02 +GPSFIX_3D = 0x03 +GPSFIX_GPS_DEAD_RECKONING = 0x04 +GPSFIX_TIME_ONLY = 0x05 + +# GPSfix to gpsfixstatus mapping +fix_to_status = { + GPSFIX_NONE: STATUS_NO_FIX, + GPSFIX_DEAD_RECKONING: STATUS_2D_FIX, + GPSFIX_2D: STATUS_2D_FIX, + GPSFIX_3D: STATUS_3D_FIX, + GPSFIX_GPS_DEAD_RECKONING: STATUS_2D_FIX, + GPSFIX_TIME_ONLY: STATUS_NO_FIX +} + +# GPSfix to readable names +fix_to_name = { + GPSFIX_NONE: "None", + GPSFIX_DEAD_RECKONING: "Dead reckoning only", + GPSFIX_2D: "2D", + GPSFIX_3D: "3D", + GPSFIX_GPS_DEAD_RECKONING: "GPS + Dead reckoning combined", + GPSFIX_TIME_ONLY: "Time only" +} + +# Signal Quality Indicator values (from UBX spec) +sq_mapping = { + 0: "Idle", + 1: "Searching", + 2: "Searching", + 3: "Unusable signal", + 4: "Code lock", + 5: "Code and Carrier lock", + 6: "Code and Carrier lock", + 7: "Receiving data" +} + MSGFMT_INV = dict( [ [(CLIDPAIR[clid], le),v + [clid]] for (clid, le),v in MSGFMT.items() ] ) class UBXDevice( GPSDevice ): @@ -267,6 +325,10 @@ class UBXDevice( GPSDevice ): self.buffer = "" self.gpschannel = gpschannel self.gpschannel.setCallback( self.parse ) + self._svcache = {} + self._svchn_map = {} + self._svprint_interval = 0 + self._lastsvprint = 0 self.ack = {"CFG-PRT" : 0} self.ubx = {} @@ -295,6 +357,10 @@ class UBXDevice( GPSDevice ): self.send("CFG-MSG", 3, {"Class" : CLIDPAIR["NAV-DOP"][0] , "MsgID" : CLIDPAIR["NAV-DOP"][1] , "Rate" : 1 }) # Send NAV SVINFO self.send("CFG-MSG", 3, {"Class" : CLIDPAIR["NAV-SVINFO"][0] , "MsgID" : CLIDPAIR["NAV-SVINFO"][1] , "Rate" : 5 }) + # Setup SV status printing + self._svprint_interval = config.getInt( CONFIG_MODULE, + "svprint_interval", 60 ) + self._lastsvprint = time.time() def deconfigure( self ): # Disable UBX packets @@ -311,6 +377,9 @@ class UBXDevice( GPSDevice ): # Reset self.gpsfixstatus = 0 self.buffer = "" + self._svcache = {} + self._svchn_map = {} + self._lastsvprint = 0 self._reset() def parse( self, data ): @@ -319,7 +388,7 @@ class UBXDevice( GPSDevice ): # Find the beginning of a UBX message start = self.buffer.find( chr( SYNC1 ) + chr( SYNC2 ) ) if start != 0: - logger.info( "Discarded data not UBX \"%s\"" % repr(self.buffer[:start]) ) + ubxpkt_logger.debug( "Discarded data not UBX \"%s\"" % repr(self.buffer[:start]) ) self.buffer = self.buffer[start:] # Minimum packet length is 8 if len(self.buffer) < 8: @@ -330,7 +399,7 @@ class UBXDevice( GPSDevice ): return if self.checksum(self.buffer[2:length+6]) != struct.unpack(" 0: @@ -356,7 +425,7 @@ class UBXDevice( GPSDevice ): payload_base = payload[0] payload_rep = payload[1:] if (length - fmt_base[0])%fmt_rep[0] != 0: - logger.error( "Cannot send: Variable length message class \ + ubxpkt_logger.error( "Cannot send: Variable length message class \ 0x%x, id 0x%x has wrong length %i" % ( cl, id, length ) ) return stream = stream + struct.pack(fmt_base[1], *[payload_base[i] for i in fmt_base[2]]) @@ -389,7 +458,7 @@ class UBXDevice( GPSDevice ): fmt_rep = format[3:] # Check if the length matches if (length - fmt_base[0])%fmt_rep[0] != 0: - logger.error( "Variable length message class 0x%x, id 0x%x \ + ubxpkt_logger.error( "Variable length message class 0x%x, id 0x%x \ has wrong length %i" % ( cl, id, length ) ) return data.append(dict(zip(fmt_base[2], struct.unpack(fmt_base[1], payload[:fmt_base[0]])))) @@ -398,10 +467,10 @@ class UBXDevice( GPSDevice ): data.append(dict(zip(fmt_rep[2], struct.unpack(fmt_rep[1], payload[offset:offset+fmt_rep[0]])))) except KeyError: - logger.info( "Unknown message class 0x%x, id 0x%x, length %i" % ( cl, id, length ) ) + ubxpkt_logger.info( "Unknown message class 0x%x, id 0x%x, length %i" % ( cl, id, length ) ) return - logger.debug( "Got UBX packet of type %s: %s" % (format[-1] , data ) ) + ubxpkt_logger.debug( "Got UBX packet of type %s: %s" % (format[-1] , data ) ) methodname = "handle_"+format[-1].replace("-", "_") try: method = getattr( self, methodname ) @@ -427,17 +496,21 @@ class UBXDevice( GPSDevice ): def handle_NAV_STATUS( self, data ): data = data[0] - fixtranstbl = [ 1, 1, 2, 3, 2, 1 ] - self.gpsfixstatus = fixtranstbl[ data["GPSfix"] ] if data["Flags"]&0x01 == 0: - self.gpsfixstatus = 1 + newstatus = STATUS_NO_FIX + else: + newstatus = fix_to_status.get(data["GPSfix"], STATUS_NO_FIX) + if newstatus != self.gpsfixstatus: + logger.info( "New fix state from GPS: %s" % + fix_to_name.get(data["GPSfix"], "None") ) + self.gpsfixstatus = newstatus self._updateFixStatus( self.gpsfixstatus ) def handle_NAV_POSLLH( self, data ): scaling = 10000000.0 - if self.gpsfixstatus == 3: + if self.gpsfixstatus == STATUS_3D_FIX: valid = 7 - elif self.gpsfixstatus == 2: + elif self.gpsfixstatus == STATUS_2D_FIX: valid = 3 else: valid = 0 @@ -446,9 +519,9 @@ class UBXDevice( GPSDevice ): data["LON"]/scaling, data["HEIGHT"]/1000.0 ) def handle_NAV_DOP( self, data ): - if self.gpsfixstatus == 3: + if self.gpsfixstatus == STATUS_3D_FIX: valid = 7 - elif self.gpsfixstatus == 2: + elif self.gpsfixstatus == STATUS_2D_FIX: valid = 2 else: valid = 0 @@ -457,9 +530,9 @@ class UBXDevice( GPSDevice ): data["HDOP"]/100.0, data["VDOP"]/100.0 ) def handle_NAV_VELNED( self, data ): - if self.gpsfixstatus == 3: + if self.gpsfixstatus == STATUS_3D_FIX: valid = 7 - elif self.gpsfixstatus == 2: + elif self.gpsfixstatus == STATUS_2D_FIX: valid = 3 else: valid = 0 @@ -472,12 +545,70 @@ class UBXDevice( GPSDevice ): base = data[0] data = data[1:] for sat in data: + self._cacheSV(sat) in_use = bool(sat["Flags"] & 0x01) # Don't include satellites that are below the horizon # (Gypsy interface requires positive elevation) if sat["Elev"] > 0: satellites.append( (sat["SVID"], in_use, sat["Elev"], sat["Azim"], sat["CNO"]) ) self._updateSatellites( satellites ) + if not self._svprint_interval: + return + print_age = time.time() - self._lastsvprint + if not self._lastsvprint or print_age > self._svprint_interval: + logger.info( "SV State (%s Fix)" % + status_to_name[self.gpsfixstatus]) + logger.info( "========" ) + t = [ (d[0], sv) for sv, d in self._svcache.items()] + for ch, sv in sorted(t): + self._logSV("", sv, True, False) + logger.info( "========" ) + self._lastsvprint = time.time() + + def _cacheSV(self, sv_data): + id = sv_data["SVID"] + if id == 0: + return + in_use = bool(sv_data["Flags"] & 0x01) + has_orbit = bool(sv_data["Flags"] & 0x04) + has_ephm = bool(sv_data["Flags"] & 0x08) + qi = sq_mapping.get(sv_data["QI"], "Unknown") + data_tuple = (sv_data["chn"], in_use, has_orbit, has_ephm, qi, + sv_data["CNO"], sv_data["Elev"], sv_data["Azim"]) + txt = None + check_channel = False + if id not in self._svcache: + txt = "New" + check_channel = True + elif data_tuple[:4] != self._svcache[id][:4]: + txt = "Update" + if self._svcache[id][0] != data_tuple[0]: + txt = "Channel Change" + check_channel = True + if check_channel: + old_sv = self._svchn_map.get(data_tuple[0], None) + sv = self._svcache.get(old_sv, (0,)) + if old_sv and sv[0] == data_tuple[0]: + del self._svcache[old_sv] + logger.debug("Removed SV%02d from Ch%02d" % (old_sv, data_tuple[0])) + self._svcache[id] = data_tuple + self._svchn_map[data_tuple[0]] = id + if txt: + self._logSV(txt, id) + + def _logSV(self, prefix, svid, full_data=False, log_debug=True): + chn, in_use, has_orbit, has_ephm, qi, cno, elev, azim = self._svcache[svid] + a_txt = in_use and "*" or "-" + o_txt = has_orbit and "O" or "-" + e_txt = has_ephm and "E" or "-" + data = "%s SV%02d %s%s%s on Ch%02d (%s)" % (prefix, svid, a_txt, o_txt, + e_txt, chn, qi) + if full_data: + data += " at %s dbHz, Elev %s, Azim %s" % (cno, elev, azim) + if log_debug: + logger.debug( data ) + else: + logger.info ( data ) def handle_NAV_TIMEUTC( self, data ): data = data[0] @@ -495,7 +626,7 @@ class UBXDevice( GPSDevice ): # Ignore ACK packets for now def handle_ACK_ACK( self, data ): data = data[0] - logger.debug("Got ACK %s" % data ) + ubxpkt_logger.debug("Got ACK %s" % data ) if (data["ClsID"], data["MsgID"]) == CLIDPAIR["CFG-PRT"]: self.ack["CFG-PRT"] = 1