Ticket #50: nmea.py

File nmea.py, 9.5 KB (added by rwhitby, 4 years ago)
Line 
1#!/usr/bin/env python
2# -*- coding: UTF-8 -*-
3"""
4Open GPS Daemon - Abstract parser class
5
6NMEA parser taken from pygps written by Russell Nelson
7Copyright, 2001, 2002, Russell Nelson <pygps@russnelson.com>
8Copyright permissions given by the GPL Version 2.  http://www.fsf.org/
9
10(C) 2008 Rod Whitby <rod@whitby.id.au>
11(C) 2008 Daniel Willmann <daniel@totalueberwachung.de>
12(C) 2008 Openmoko, Inc.
13GPLv2
14"""
15
16__version__ = "0.0.0"
17
18import math
19import string
20import time
21import dbus
22from gpsdevice import GPSDevice
23
24import logging
25logger = logging.getLogger('ogpsd')
26
27DBUS_INTERFACE = "org.freesmartphone.GPS"
28
29class NMEADevice( GPSDevice ):
30    def __init__( self, bus, gpschannel ):
31        super( NMEADevice, self ).__init__( bus )
32
33        self.buffer = ""
34        self.gpschannel = gpschannel
35        self.gpschannel.setCallback( self.parse )
36
37        self.prn = range(12)
38        self.elevation = range(12)
39        self.azimuth = range(12)
40        self.ss = range(12)
41        self.used = range(12)
42        self.date = '000000'
43        self.time = '000000'
44        self.timestamp = 0
45        self.mode = 0
46        self.lat = 0.0
47        self.lon = 0.0
48        self.altitude = 0.0
49        self.track = 0.0
50        self.speed = 0.0
51        self.in_view = 0
52
53    def parse( self, data ):
54        self.buffer += data
55
56        while True:
57            try:
58                line, self.buffer = self.buffer.split( "\r\n", 1 )
59            except:
60                break
61            else:
62                result = self.handle_line( line.strip() )
63                if result:
64                    logger.debug( result )
65
66
67    def checksum(self,sentence, cksum):
68        csum = 0
69        for c in sentence:
70            csum = csum ^ ord(c)
71        return "%02X" % csum == cksum
72
73#$GPGGA,000032.997,0000.0000,N,00000.0000,E,0,00,50.0,0.0,M,,.j...«.Ê.ÆV.Ê.ÆV.Ê.|.VÖL²4jj.h..00032.997,V,0000.0000,N,00000.0006
74#Lat: 0.000000 Lon: 0.000000 Alt: 0.000000 Sat: 0 Mod: 1 Time: 11/26/2000 00:00:31
75#$GPGGA,000033.997,0000.0000,N,00000.0000,E,0,00,50.0,0.0,M,,,,0000*3C
76
77    def  do_lat_lon(self, words):
78        if not words[0]:
79            return
80        if words[0][-1] == 'N':
81            words[0] = words[0][:-1]
82            words[1] = 'N'
83        if words[0][-1] == 'S':
84            words[0] = words[0][:-1]
85            words[1] = 'S'
86        if words[2][-1] == 'E':
87            words[2] = words[2][:-1]
88            words[3] = 'E'
89        if words[2][-1] == 'W':
90            words[2] = words[2][:-1]
91            words[3] = 'W'
92        if len(words[0]):
93            lat = string.atof(words[0])
94            frac, intpart = math.modf(lat / 100.0)
95            lat = intpart + frac * 100.0 / 60.0
96            if words[1] == 'S':
97                lat = -lat
98            self.lat = lat
99        if len(words[2]):
100            lon = string.atof(words[2])
101            frac, intpart = math.modf(lon / 100.0)
102            lon = intpart + frac * 100.0 / 60.0
103            if words[3] == 'W':
104                lon = -lon
105            self.lon = lon
106
107#$GPRMC,024932.992,V,4443.7944,N,07456.7103,W,,,270402,,*05
108#$GPRMC,024933.992,V,4443.7944,N,07456.7103,W,,,270402,,*04
109
110#        RMC - Recommended minimum specific GPS/Transit data
111#        RMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68
112#           225446       Time of fix 22:54:46 UTC
113#           A            Navigation receiver warning A = OK, V = warning
114#           4916.45,N    Latitude 49 deg. 16.45 min North
115#           12311.12,W   Longitude 123 deg. 11.12 min West
116#           000.5        Speed over ground, Knots
117#           054.7        Course Made Good, True
118#           191194       Date of fix  19 November 1994
119#           020.3,E      Magnetic variation 20.3 deg East
120#           *68          mandatory checksum
121
122    def processGPRMC(self, words):
123        self.do_lat_lon(words[2:])
124        self.date = words[8]
125        day = string.atoi(self.date[0:2])
126        month = string.atoi(self.date[2:4])
127        year = 2000 + string.atoi(self.date[4:6])
128        self.time = words[0][0:6]
129        hours = string.atoi(self.time[0:2])
130        minutes = string.atoi(self.time[2:4])
131        seconds = string.atoi(self.time[4:6])
132        self.timestamp = int(time.mktime((year,month,day,hours,minutes,seconds,-1,-1,-1)))
133
134        if words[1] == 'V' or words[1] == "A":
135            if words[6]: self.speed = string.atof(words[6])
136            if words[7]: self.track = string.atof(words[7])
137            self._updateCourse( 6, self.timestamp, self.speed, self.track, 0 )
138
139#$GPGGA,024933.992,4443.7944,N,07456.7103,W,0,00,50.0,192.5,M,,,,0000*27
140#$GPGGA,024934.991,4443.7944,N,07456.7103,W,0,00,50.0,192.5,M,,,,0000*23
141
142#        GGA - Global Positioning System Fix Data
143#        GGA,123519,4807.038,N,01131.324,E,1,08,0.9,545.4,M,46.9,M, , *42
144#           123519       Fix taken at 12:35:19 UTC
145#           4807.038,N   Latitude 48 deg 07.038' N
146#           01131.324,E  Longitude 11 deg 31.324' E
147#           1            Fix quality: 0 = invalid
148#                                     1 = GPS fix
149#                                     2 = DGPS fix
150#           08           Number of satellites being tracked
151#           0.9          Horizontal dilution of position
152#           545.4,M      Altitude, Metres, above mean sea level
153#           46.9,M       Height of geoid (mean sea level) above WGS84
154#                        ellipsoid
155#           (empty field) time in seconds since last DGPS update
156#           (empty field) DGPS station ID number
157
158    def processGPGGA(self,words):
159        day = string.atoi(self.date[0:2])
160        month = string.atoi(self.date[2:4])
161        year = 2000 + string.atoi(self.date[4:6])
162        self.time = words[0][0:6]
163        hours = string.atoi(self.time[0:2])
164        minutes = string.atoi(self.time[2:4])
165        seconds = string.atoi(self.time[4:6])
166        self.timestamp = int(time.mktime((year,month,day,hours,minutes,seconds,-1,-1,-1)))
167        self.status = string.atoi(words[5])
168        self.satellites = string.atoi(words[6])
169        if self.status > 0:
170            self.do_lat_lon(words[1:])
171            self.altitude = string.atof(words[8])
172            # FIXME: Mode
173            self._updateFixStatus( self.status + 1 )
174            self._updatePosition( 15, self.timestamp, self.lat, self.lon, self.altitude )
175
176#$GPGSA,A,1,,,,,,,,,,,,,50.0,50.0,50.0*05
177#$GPGSA,A,1,,,,,,,,,,,,,50.0,50.0,50.0*05
178
179#        GSA - GPS DOP and active satellites
180#        GSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
181#           A            Auto selection of 2D or 3D fix (M = manual)
182#           3            3D fix
183#           04,05...     PRNs of satellites used for fix (space for 12)
184#           2.5          PDOP (dilution of precision)
185#           1.3          Horizontal dilution of precision (HDOP)
186#           2.1          Vertical dilution of precision (VDOP)
187#             DOP is an indication of the effect of satellite geometry on
188#             the accuracy of the fix.
189
190    def processGPGSA(self,words):
191        self.mode = string.atof(words[1])
192        for n in range(12):
193            if words[n+2]:
194                self.used[n] = 1
195            else:
196                self.used[n] = 0
197        pdop = string.atof(words[14])
198        hdop = string.atof(words[15])
199        vdop = string.atof(words[16])
200        # FIXME: Mode...
201        self._updateAccuracy( 7, pdop, hdop, vdop )
202        self._updateSatellites(
203            (self.prn[n],self.used[n],self.elevation[n],self.azimuth[n],self.ss[n]) for n in range(12)
204            )           
205
206#$GPGSV,3,1,09,14,77,023,,21,67,178,,29,64,307,,30,42,095,*7E
207#$GPGSV,3,2,09,05,29,057,,11,15,292,,18,08,150,,23,08,143,*7A
208#$GPGSV,3,3,09,09,05,052,*4B
209
210#        GSV - Satellites in view
211#        GSV,2,1,08,01,40,083,46,02,17,308,41,12,07,344,39,14,22,228,45*75
212#           2            Number of sentences for full data
213#           1            sentence 1 of 2
214#           08           Number of satellites in view
215#           01           Satellite PRN number
216#           40           Elevation, degrees
217#           083          Azimuth, degrees
218#           46           Signal strength - higher is better
219#           <repeat for up to 4 satellites per sentence>
220#                There my be up to three GSV sentences in a data packet
221
222    def processGPGSV(self,words):
223        num_sentences = string.atoi(words[0])
224        current_sentence = string.atoi(words[1])
225        self.in_view = string.atoi(words[2])
226
227        f = 3
228        n = (current_sentence - 1) * 4
229
230        # FIXME: Need to clear the rest of the entries up to 12
231
232        while n < self.in_view and f < len(words):
233            if words[f+0]:
234                self.prn[n] = string.atoi(words[f+0])
235            if words[f+1]:
236                self.elevation[n] = string.atoi(words[f+1])
237            if words[f+2]:
238                self.azimuth[n] = string.atoi(words[f+2])
239            if words[f+3]:
240                self.ss[n] = string.atoi(words[f+3])
241            f = f + 4
242            n = n + 1
243
244    def processPGLOR(self,words):
245        # FIXME: Do we do anything with this sentence?
246        pass
247
248    def handle_line(self, line):
249        if line[0] == '$':
250            line = string.split(line[1:-1], '*')
251            if len(line) != 2: return
252#            if not self.checksum(line[0], line[1]):
253#                return "Bad checksum"
254            words = string.split(line[0], ',')
255            methodname = "process"+words[0]
256            try:
257                method = getattr( self, methodname )
258            except AttributeError:
259                logger.error( "Line: %s" % line)
260                return "Unknown sentence"
261            else:
262                try:
263                    method( words[1:] )
264                except Exception, e:
265                    logger.error( "Line: %s" % line)
266                    logger.error( "Error in %s method: %s" % ( methodname, e ) )
267        else:
268            return "Not NMEA"
269
270#vim: expandtab