Ticket #130: gps-logging-improvements.diff

File gps-logging-improvements.diff, 12.0 KB (added by fso@…, 4 years ago)

ogpsd logging improvements

  • conf/example/frameworkd.conf 

    diff --git a/conf/example/frameworkd.conf b/conf/example/frameworkd.conf
index ec6e84a..89fdbb4 100644
    a b device = UBXDevice 
    5353channel = SerialChannel 
    5454path = /dev/ttyACM0 
    5555log_level = INFO 
     56# print the status of every channel (SV tracked, etc) every X seconds. 
     57# 0 to disable. 
     58svprint_interval = 60 
     59 
     60# Logging of UBX packet processing is managed separately as it can be very 
     61# verbose. 
     62[ogpsd.ubx] 
     63log_level = INFO 
    5664 
    5765[opreferencesd] 
    5866log_level = DEBUG 

  • framework/subsystems/ogpsd/ubx.py 

    diff --git a/framework/subsystems/ogpsd/ubx.py b/framework/subsystems/ogpsd/ubx.py
index b6718cc..f4857b0 100644
    a b import struct 
    1414import dbus 
    1515from gpsdevice import GPSDevice 
    1616import calendar 
     17import time 
     18 
     19from framework.config import config 
    1720 
    1821import logging 
    1922logger = logging.getLogger('ogpsd') 
     23# Separate logger for ubx packet parsing for extra control. 
     24ubxpkt_logger = logging.getLogger('ogpsd.ubx') 
    2025 
     26CONFIG_MODULE = "ogpsd" 
    2127DBUS_INTERFACE = "org.freesmartphone.GPS" 
    2228 
    2329SYNC1=0xb5 
    … … MSGFMT = { 
    257263# TIM - Timekeeping 
    258264} 
    259265 
     266# self.gpsfixstatus value constants. 
     267STATUS_NO_FIX = 1 
     268STATUS_2D_FIX = 2 
     269STATUS_3D_FIX = 3 
     270 
     271# status to readable names 
     272status_to_name = { 
     273    STATUS_NO_FIX: "No", 
     274    STATUS_2D_FIX: "2D", 
     275    STATUS_3D_FIX: "3D" 
     276} 
     277 
     278# GPSfix field values (from UBX spec) 
     279GPSFIX_NONE = 0x0 
     280GPSFIX_DEAD_RECKONING = 0x01 
     281GPSFIX_2D = 0x02 
     282GPSFIX_3D = 0x03 
     283GPSFIX_GPS_DEAD_RECKONING = 0x04 
     284GPSFIX_TIME_ONLY = 0x05 
     285 
     286# GPSfix to gpsfixstatus mapping 
     287fix_to_status = { 
     288    GPSFIX_NONE: STATUS_NO_FIX, 
     289    GPSFIX_DEAD_RECKONING: STATUS_2D_FIX, 
     290    GPSFIX_2D: STATUS_2D_FIX, 
     291    GPSFIX_3D: STATUS_3D_FIX, 
     292    GPSFIX_GPS_DEAD_RECKONING: STATUS_2D_FIX, 
     293    GPSFIX_TIME_ONLY: STATUS_NO_FIX 
     294} 
     295 
     296# GPSfix to readable names 
     297fix_to_name = { 
     298    GPSFIX_NONE: "None", 
     299    GPSFIX_DEAD_RECKONING: "Dead reckoning only", 
     300    GPSFIX_2D: "2D", 
     301    GPSFIX_3D: "3D", 
     302    GPSFIX_GPS_DEAD_RECKONING: "GPS + Dead reckoning combined", 
     303    GPSFIX_TIME_ONLY: "Time only" 
     304} 
     305 
     306# Signal Quality Indicator values (from UBX spec) 
     307sq_mapping = { 
     308    0: "Idle", 
     309    1: "Searching", 
     310    2: "Searching", 
     311    3: "Unusable signal", 
     312    4: "Code lock", 
     313    5: "Code and Carrier lock", 
     314    6: "Code and Carrier lock", 
     315    7: "Receiving data" 
     316} 
     317 
    260318MSGFMT_INV = dict( [ [(CLIDPAIR[clid], le),v + [clid]] for (clid, le),v in MSGFMT.items() ] ) 
    261319 
    262320class UBXDevice( GPSDevice ): 
    … … class UBXDevice( GPSDevice ): 
    267325        self.buffer = "" 
    268326        self.gpschannel = gpschannel 
    269327        self.gpschannel.setCallback( self.parse ) 
     328        self._svcache = {} 
     329        self._svchn_map = {} 
     330        self._svprint_interval = 0 
     331        self._lastsvprint = 0 
    270332 
    271333        self.ack = {"CFG-PRT" : 0} 
    272334        self.ubx = {} 
    … … class UBXDevice( GPSDevice ): 
    295357        self.send("CFG-MSG", 3, {"Class" : CLIDPAIR["NAV-DOP"][0] , "MsgID" : CLIDPAIR["NAV-DOP"][1] , "Rate" : 1 }) 
    296358        # Send NAV SVINFO 
    297359        self.send("CFG-MSG", 3, {"Class" : CLIDPAIR["NAV-SVINFO"][0] , "MsgID" : CLIDPAIR["NAV-SVINFO"][1] , "Rate" : 5 }) 
     360        # Setup SV status printing 
     361        self._svprint_interval = config.getInt( CONFIG_MODULE, 
     362                "svprint_interval", 60 ) 
     363        self._lastsvprint = time.time() 
    298364 
    299365    def deconfigure( self ): 
    300366        # Disable UBX packets 
    … … class UBXDevice( GPSDevice ): 
    311377        # Reset 
    312378        self.gpsfixstatus = 0 
    313379        self.buffer = "" 
     380        self._svcache = {} 
     381        self._svchn_map = {} 
     382        self._lastsvprint = 0 
    314383        self._reset() 
    315384 
    316385    def parse( self, data ): 
    … … class UBXDevice( GPSDevice ): 
    319388            # Find the beginning of a UBX message 
    320389            start = self.buffer.find( chr( SYNC1 ) + chr( SYNC2 ) ) 
    321390            if start != 0: 
    322                 logger.info( "Discarded data not UBX \"%s\"" % repr(self.buffer[:start]) ) 
     391                ubxpkt_logger.debug( "Discarded data not UBX \"%s\"" % repr(self.buffer[:start]) ) 
    323392            self.buffer = self.buffer[start:] 
    324393            # Minimum packet length is 8 
    325394            if len(self.buffer) < 8: 
    … … class UBXDevice( GPSDevice ): 
    330399                return 
    331400 
    332401            if self.checksum(self.buffer[2:length+6]) != struct.unpack("<BB", self.buffer[length+6:length+8]): 
    333                 logger.warning( "UBX packed class 0x%x, id 0x%x, length %i failed checksum" % (cl, id, length) ) 
     402                ubxpkt_logger.warning( "UBX packed class 0x%x, id 0x%x, length %i failed checksum" % (cl, id, length) ) 
    334403                self.buffer = self.buffer[2:] 
    335404                continue 
    336405 
    … … class UBXDevice( GPSDevice ): 
    341410            self.buffer = self.buffer[length+8:] 
    342411 
    343412    def send( self, clid, length, payload ): 
    344         logger.debug( "Sending UBX packet of type %s: %s" % ( clid, payload ) ) 
     413        ubxpkt_logger.debug( "Sending UBX packet of type %s: %s" % ( clid, payload ) ) 
    345414 
    346415        stream = struct.pack("<BBBBH", SYNC1, SYNC2, CLIDPAIR[clid][0], CLIDPAIR[clid][1], length) 
    347416        if length > 0: 
    … … class UBXDevice( GPSDevice ): 
    356425                payload_base = payload[0] 
    357426                payload_rep = payload[1:] 
    358427                if (length - fmt_base[0])%fmt_rep[0] != 0: 
    359                     logger.error( "Cannot send: Variable length message class \ 
     428                    ubxpkt_logger.error( "Cannot send: Variable length message class \ 
    360429                        0x%x, id 0x%x has wrong length %i" % ( cl, id, length ) ) 
    361430                    return 
    362431            stream = stream + struct.pack(fmt_base[1], *[payload_base[i] for i in fmt_base[2]]) 
    … … class UBXDevice( GPSDevice ): 
    389458                fmt_rep = format[3:] 
    390459                # Check if the length matches 
    391460                if (length - fmt_base[0])%fmt_rep[0] != 0: 
    392                     logger.error( "Variable length message class 0x%x, id 0x%x \ 
     461                    ubxpkt_logger.error( "Variable length message class 0x%x, id 0x%x \ 
    393462                        has wrong length %i" % ( cl, id, length ) ) 
    394463                    return 
    395464                data.append(dict(zip(fmt_base[2], struct.unpack(fmt_base[1], payload[:fmt_base[0]])))) 
    … … class UBXDevice( GPSDevice ): 
    398467                    data.append(dict(zip(fmt_rep[2], struct.unpack(fmt_rep[1], payload[offset:offset+fmt_rep[0]])))) 
    399468 
    400469            except KeyError: 
    401                 logger.info( "Unknown message class 0x%x, id 0x%x, length %i" % ( cl, id, length ) ) 
     470                ubxpkt_logger.info( "Unknown message class 0x%x, id 0x%x, length %i" % ( cl, id, length ) ) 
    402471                return 
    403472 
    404         logger.debug( "Got UBX packet of type %s: %s" % (format[-1] , data ) ) 
     473        ubxpkt_logger.debug( "Got UBX packet of type %s: %s" % (format[-1] , data ) ) 
    405474        methodname = "handle_"+format[-1].replace("-", "_") 
    406475        try: 
    407476            method = getattr( self, methodname ) 
    … … class UBXDevice( GPSDevice ): 
    427496 
    428497    def handle_NAV_STATUS( self, data ): 
    429498        data = data[0] 
    430         fixtranstbl = [ 1, 1, 2, 3, 2, 1 ] 
    431         self.gpsfixstatus = fixtranstbl[ data["GPSfix"] ] 
    432499        if data["Flags"]&0x01 == 0: 
    433             self.gpsfixstatus = 1 
     500            newstatus = STATUS_NO_FIX 
     501        else: 
     502            newstatus = fix_to_status.get(data["GPSfix"], STATUS_NO_FIX) 
     503        if newstatus != self.gpsfixstatus: 
     504            logger.info( "New fix state from GPS: %s" % 
     505                fix_to_name.get(data["GPSfix"], "None") ) 
     506        self.gpsfixstatus = newstatus 
    434507        self._updateFixStatus( self.gpsfixstatus ) 
    435508 
    436509    def handle_NAV_POSLLH( self, data ): 
    437510        scaling = 10000000.0 
    438         if self.gpsfixstatus == 3: 
     511        if self.gpsfixstatus == STATUS_3D_FIX: 
    439512            valid = 7 
    440         elif self.gpsfixstatus == 2: 
     513        elif self.gpsfixstatus == STATUS_2D_FIX: 
    441514            valid = 3 
    442515        else: 
    443516            valid = 0 
    … … class UBXDevice( GPSDevice ): 
    446519                data["LON"]/scaling, data["HEIGHT"]/1000.0 ) 
    447520 
    448521    def handle_NAV_DOP( self, data ): 
    449         if self.gpsfixstatus == 3: 
     522        if self.gpsfixstatus == STATUS_3D_FIX: 
    450523            valid = 7 
    451         elif self.gpsfixstatus == 2: 
     524        elif self.gpsfixstatus == STATUS_2D_FIX: 
    452525            valid = 2 
    453526        else: 
    454527            valid = 0 
    … … class UBXDevice( GPSDevice ): 
    457530                data["HDOP"]/100.0, data["VDOP"]/100.0 ) 
    458531 
    459532    def handle_NAV_VELNED( self, data ): 
    460         if self.gpsfixstatus == 3: 
     533        if self.gpsfixstatus == STATUS_3D_FIX: 
    461534            valid = 7 
    462         elif self.gpsfixstatus == 2: 
     535        elif self.gpsfixstatus == STATUS_2D_FIX: 
    463536            valid = 3 
    464537        else: 
    465538            valid = 0 
    … … class UBXDevice( GPSDevice ): 
    472545        base = data[0] 
    473546        data = data[1:] 
    474547        for sat in data: 
     548            self._cacheSV(sat) 
    475549            in_use = bool(sat["Flags"] & 0x01) 
    476550            # Don't include satellites that are below the horizon 
    477551            # (Gypsy interface requires positive elevation) 
    478552            if sat["Elev"] > 0: 
    479553                satellites.append( (sat["SVID"], in_use, sat["Elev"], sat["Azim"], sat["CNO"]) ) 
    480554        self._updateSatellites( satellites ) 
     555        if not self._svprint_interval: 
     556            return 
     557        print_age = time.time() - self._lastsvprint 
     558        if not self._lastsvprint or print_age > self._svprint_interval: 
     559            logger.info( "SV State (%s Fix)" % 
     560                    status_to_name[self.gpsfixstatus]) 
     561            logger.info( "========" ) 
     562            t = [ (d[0], sv) for sv, d in self._svcache.items()] 
     563            for ch, sv in sorted(t): 
     564                self._logSV("", sv, True, False) 
     565            logger.info( "========" ) 
     566            self._lastsvprint = time.time() 
     567 
     568    def _cacheSV(self, sv_data): 
     569        id = sv_data["SVID"] 
     570        if id == 0: 
     571            return 
     572        in_use = bool(sv_data["Flags"] & 0x01) 
     573        has_orbit = bool(sv_data["Flags"] & 0x04) 
     574        has_ephm = bool(sv_data["Flags"] & 0x08) 
     575        qi = sq_mapping.get(sv_data["QI"], "Unknown") 
     576        data_tuple = (sv_data["chn"], in_use, has_orbit, has_ephm, qi, 
     577                sv_data["CNO"], sv_data["Elev"], sv_data["Azim"]) 
     578        txt = None 
     579        check_channel = False 
     580        if id not in self._svcache: 
     581            txt = "New" 
     582            check_channel = True 
     583        elif data_tuple[:4] != self._svcache[id][:4]: 
     584            txt = "Update" 
     585            if self._svcache[id][0] != data_tuple[0]: 
     586                txt = "Channel Change" 
     587                check_channel = True 
     588        if check_channel: 
     589            old_sv = self._svchn_map.get(data_tuple[0], None) 
     590            sv = self._svcache.get(old_sv, (0,)) 
     591            if old_sv and sv[0] == data_tuple[0]: 
     592                del self._svcache[old_sv] 
     593                logger.debug("Removed SV%02d from Ch%02d" % (old_sv, data_tuple[0])) 
     594        self._svcache[id] = data_tuple 
     595        self._svchn_map[data_tuple[0]] = id 
     596        if txt: 
     597            self._logSV(txt, id) 
     598 
     599    def _logSV(self, prefix, svid, full_data=False, log_debug=True): 
     600        chn, in_use, has_orbit, has_ephm, qi, cno, elev, azim = self._svcache[svid] 
     601        a_txt = in_use and "*" or "-" 
     602        o_txt = has_orbit and "O" or "-" 
     603        e_txt = has_ephm and "E" or "-" 
     604        data = "%s SV%02d %s%s%s on Ch%02d (%s)" % (prefix, svid, a_txt, o_txt, 
     605                e_txt, chn, qi) 
     606        if full_data: 
     607            data += " at %s dbHz, Elev %s, Azim %s" % (cno, elev, azim) 
     608        if log_debug: 
     609            logger.debug( data ) 
     610        else: 
     611            logger.info ( data ) 
    481612 
    482613    def handle_NAV_TIMEUTC( self, data ): 
    483614        data = data[0] 
    … … class UBXDevice( GPSDevice ): 
    495626    # Ignore ACK packets for now 
    496627    def handle_ACK_ACK( self, data ): 
    497628        data = data[0] 
    498         logger.debug("Got ACK %s" % data ) 
     629        ubxpkt_logger.debug("Got ACK %s" % data ) 
    499630        if (data["ClsID"], data["MsgID"]) == CLIDPAIR["CFG-PRT"]: 
    500631          self.ack["CFG-PRT"] = 1 
    501632