GPS reloaded

3.11.2006 11:26 | Přečteno: 2455× | Linux | poslední úprava: 13.11.2007 16:43

Nedávno jsem tu nadával na "sekačkovou" gépéesku od Mountfieldu. Poté jsem vezl rodinu do olomoucké ZOO, a protože jsem tam ještě nebyl, nahrál jsem si pomocí gpsbabel routu, spočítanou a stáhnutou z http://www.mapy.cz/ v .GPX formátu. Navigace samozřejmě nesrovnatelná s moderními harddiskovými navigátory, ale přesto překvapivě použitelná. Sice žádné barvičky, animace, náhledy zatáček, a hlasová upozornění, ale vzdálenost do příštího waypointu a směr odbočení je vidět velmi dobře, a většinou to pro navigaci naprosto postačuje.

Akorát se mi vůbec nelíbil gpsbabel, tak jsem si napsal vlastní utilitu pro upload routy. Když se vykostí všechny zvrhlosti které gpsbabel obsahuje, je samotný protokol překvapivě jednoduchý, i když ne moc efektivní. gps.py má mnohem jednodušší syntax, generuje z .GPX souborů lepší názvy waypointů než nicneříkající WP00n, a když je route delší než 125 waypointů (což je při úrovni detailů odboček na mapy.cz docela často), tak routu inteligentně zahustí, aby se do chudáčka vešla. Jako bonus umí i upload .LOC souborů z geocaching.com, a taky download waypointů, routes, a tracklogů. Třeba bude užitečná i pro další lidi (pokud jsem při copypaste nezmršil formátování). Uvítám zpětnou vazbu (je to kewl, to a to děláš blbě, neumí to svahilsky..). Za reakce obsahující klíčová slova "konfigurace", "manpage", "usb" či "dokumentace" děkuji obzvlášť uctivě a předem.

$ cat ~/bin/gps.py
#! /usr/bin/python
import os, termios, sys, time, re, math
from string import join, split, upper
from struct import pack, unpack

GPS_WPT, GPS_ROU, GPS_TRK = 7, 4, 6

sym = { 10: 'House',
        18: 'Dot',
        173: 'Hotel',
        8218: 'Shop',
        8223: 'Street',
        8234: 'Building',
        8255: 'Cache',
        8256: 'Found' }

sym_r = {}
for k, v in sym.items(): sym_r[v] = k
del k, v

def to_deg(semi, s):
        d = abs(semi)*180.0/0x80000000
        return '%d%c%06.3f' % (int(d), s[semi < 0], (d - int(d))*60)

def to_frac(s):
        if re.search('^[+-]?\d+(\.\d+)?$', s):
                s = float(s)
        else:
                s, x, f = re.search('^(\d+)([NSEW])(\d+\.\d+)$', s).groups()
                s = (int(s) + float(f) / 60) * (1, -1)[x in 'SW']
        return int(s / 180 * 0x80000000)

def compress(i, max = 10):
        i = re.sub('[^\w\d +]+', '-', i)
        i = re.sub(' +([ -])', '\\1', i)
        i = re.sub('([ -]) +', '\\1', i)
        while len(i) > max:
                m = re.search('.*[aeiouy]', i)
                if not m: break
                i = i[:m.end()-1] + i[m.end():]
        return upper(i[:max])

class ProtocolError(Exception):
        pass

class Garmin:
        def __init__(self, dev = '/dev/ttyS0'):
                self._fh = os.open(dev, os.O_RDWR)
                _, _, cflag, _, _, _, cc = termios.tcgetattr(self._fh)
                cflag = cflag & ~termios.CSIZE | termios.CS8
                cflag = cflag & ~termios.PARENB & ~termios.CSTOPB
                cc[termios.VMIN] = 1
                cc[termios.VTIME] = 0
                termios.tcsetattr(self._fh, termios.TCSAFLUSH, [
                        0, 0, cflag, 0, termios.B9600, termios.B9600, cc])

        def _send(self, type, data):
                chk = [0]
                def esc(c):
                        chk[0] += c
                        return c != 0x10 and chr(c) or '\x10\x10'
                os.write(self._fh, '\x10' + esc(type) + esc(len(data)))
                os.write(self._fh, join(map(lambda x: esc(ord(x)), data), ''))
                os.write(self._fh, esc(-chk[0] & 0xff) + '\x10\x03')

        def _recv(self):
                chk = [0]
                def read():
                        c = ord(os.read(self._fh, 1))
                        if c == 0x10:
                                c = ord(os.read(self._fh, 1))
                                if c != 0x10:
                                        chk[0] = c
                                        return None
                        chk[0] += c
                        return c
                while read() != None: pass
                t = chk[0]; n = read(); data = ''
                while 1:
                        c = read()
                        if c == None: raise ProtocolError, 'Unexpected dle'
                        if len(data) == n: break
                        data += chr(c)
                if chk[0] & 0xff: raise ProtocolError, 'Checksum error'
                if read() != None or chk[0] != 3:
                        raise ProtocolError, 'Missing dle + eot'
                return t, data

        def send(self, t, data = ''):
                self._send(t, data)
                if self._recv() != (6, pack('<H', t)):
                        raise ProtocolError, 'Expected ack'

        def recv(self):
                t, data = self._recv()
                self._send(6, pack('<H', t))
                return t, data

        def download(self, what):
                self.send(10, pack('<H', what))
                while 1:
                        t, data = self.recv()
                        if t == 12: break
                        if t == 35 or t == 30:
                                s, x, y, a = unpack('<4xH18xiif12x', data[:48])
                                x = to_deg(x, 'NS')
                                y = to_deg(y, 'EW')
                                a = a > 0 and a < 1e9 and str(int(a)) or '?'
                                s = sym.get(s) or '?%d' % s
                                n = split(data[48:], '\0')
                                n = n[0] + (n[5] and ' & ' + n[5])
                                yield x, y, a, s, n
                        elif t == 34:
                                x, y, t, a, _ = unpack('<iiIf4xb3x', data)
                                x = to_deg(x, 'NS')
                                y = to_deg(y, 'EW')
                                a = str(int(a))
                                t = t != 0xffffffff and time.strftime(
                                        '%y%m%d-%H:%M:%S',
                                        time.gmtime(t + 631065600)) or '?'
                                yield x, y, a, t
                        elif t == 29: yield 'ROUTE', split(data, '\0')[0]
                        elif t == 99: yield 'TRACK', split(data[2:], '\0')[0]

        def upload(self, what, wpt, title):
                print 'uploading %s(%d)' % (title, len(wpt))
                is_rou = what == GPS_ROU; t = 0
                self.send(27, pack('<H', len(wpt) * (1 + is_rou)))
                if is_rou:
                        title = re.sub('(.*/)?(.+)\..*', '\\2', title)
                        self.send(29, compress(title, 13) + '\0')
                for x, y, a, s, name in wpt:
                        if t == 30: self.send(98)
                        t = is_rou and 30 or 35
                        self.send(t, '\0\xff\xff\x60' + pack('<H18xiif12x',
                                sym_r[s], to_frac(x), to_frac(y),
                                a != '?' and float(a) or 0.0) +
                                compress(name) + '\0'*6)
                self.send(12, pack('<H', what))

def load_gpx(f):
        def dif(a, b): return b[0] - a[0], b[1] - a[1]
        def mag(x): return math.sqrt(x[0]*x[0] + x[1]*x[1])
        lst = [(to_frac(x), to_frac(y) * 0.65,
                (x, y, '?', 'Dot', cmt or n)) for x, y, n, cmt in re.findall(
                '<rtept lat="(.+?)" lon="(.+?)"><name>(.+?)</name><cmt>(.*?)</cmt>',
                open(f).read())]
        while len(lst) > 125:
                ms = mi = i = 0
                while i + 3 <= len(lst):
                        a = dif(lst[i], lst[i + 1])
                        b = dif(lst[i + 1], lst[i + 2]); i += 1
                        s = (a[0]*b[0] + a[1]*b[1]) / mag(a) / mag(b)
                        if s > ms: ms = s; mi = i
                del lst[mi]
        return [i[2] for i in lst]

if __name__ == '__main__':
        c = Garmin()
        for i in sys.argv[1:]:
                if i[:2] == '-d':
                        for i in c.download(
                                i == '-dr' and GPS_ROU or \
                                i == '-dt' and GPS_TRK or GPS_WPT):
                                print join(i)
                        continue
                if i[-4:] == '.gpx':
                        c.upload(GPS_ROU, load_gpx(i), i)
                        continue
                c.upload(GPS_WPT, [
                        (x, y, '?', 'Cache', wpt) for wpt, x, y in re.findall(
                                '<name id="(.+?)".*?<coord lat="(.+?)" lon="(.+?)"',
                                open(i).read(), re.DOTALL)
                ] or [split(l)[:5] for l in open(i).readlines()], i)

       

Komentáře

[Jáchym Čepický]

sháním nějakou utilitku na kloudný upload dat do gps. tohle vypadá zajímavě, ale je to nějak porouchané:

Traceback (most recent call last):
  File "/home/jachym/bin/gps.py", line 159, in ?
    c = Garmin()
  File "/home/jachym/bin/gps.py", line 49, in __init__
    _, _, cflag, _, _, _, cc = termios.tcgetattr(self._fh)
termios.error: (5, 'Input/output error')

atd.

nemohl bys ten soubor někam uložit a dát na něj link?

dík

[zde]

Tohle formátováním určitě nebude. Nepoužíváš už ttyS0 (první sériový port) pro něco jiného? Vypadá to že port otevřít jde (takže existuje), ale tcgetattr() selže. Hláška I/O error je nějak dost divná, tcgetattr() se o žádné I/O pokoušet nemá, jen vrací aktuální parametry sériového rozhraní. Na jakém OS to zkoušíš? Mě to na Archu i FC5 běhalo bez problémů, jinde jsem nezkoušel.

[V/S/7]

Funguje skvěle, ale chtěl jsem předělat jména pro geocaching, taky nějakou kompresí, napadlo mne zkrátit první slovo/ dvě slova bez teček a čárek a pomlček atp. Nahrávat do Garmina toto jméno místo GCXXXXXX. Ale moc se v Pythonu nechytám a ani se mi nepodařilo Tě kontaktovat.

[zde]

Já používám takovouto konvenci:

49.215 16.613333 0708 Dot W6JX-US72 060523 bozecaspar by dape team
49.1968 16.608683 1110 Cache X1Z5-TM31 060818 Kostel sv. Jakuba / St. Jacob church by SHANTI Team
49.214967 16.634817 1110 Cache PDFW-TS22 050622 Husak/ Husovice hill by adp.
49.21095 16.640067 1103 Cache 1116N-US71 070219 Sokobrno by abakus
..

Jmeno waypointu ma tvrdej limit 10 znaku. Aktualni GC waypointy bez predpony GC maji 5 znaku, takze zbyva 5, bez pomlcky 4. Prvni znak davam typ cache (T/M/U/..), druhy znak je velikost (M/S/R/L/..), a pak jsou obtiznosti, prevedene z rozsahu 1-5 na rozsah 1-9 ale bez polovin. Do nadmorske vysky dam datum posledniho nalezu, ale vejde se tam jen mesic a rok, protoze garmin ma limit pro nadmorskou vysku 30000. To co je za nazvem waypointu je uz jen komentar, a do garmina se neposila. Jo a disabled cache posilam jako 'Dot', a found cache jako 'Found', takze je na prvni pohled videt co je co.

[V/S/7]

Vypadá zajímavě, nechal bych se inspirovat...

Můj původní návrh by to převedl nějak takto(jen změna jména):

49.215 16.613333 0708 Dot BOZECASP BY 060523 bozecaspar by dape team

49.1968 16.608683 1110 Cache KOSTEL SV 060818 Kostel sv. Jakuba / St. Jacob church by SHANTI Team

49.214967 16.634817 1110 Cache HUSAK HUSO 050622 Husak/ Husovice hill by adp.

49.21095 16.640067 1103 Cache SOKOBRN BY 070219 Sokobrno by abakus

a pravda obtížnost by se dala uložit do nadmořské výšky. A na typ keše než tradiční i jiná ikonka(Garmin jich tam má spousty)

Pokud je tvůj kód zveřejnitelný tak by jsi ho sem mohl dát, já případně zveřejním tu 3.verzi.

Ale ještě mi to v tom Pythonu moc nemyslí (myslí mi to v c/c++).

[zde]

No, jedinej užitečnej fix co tam byl je oprava parsování route, ale to už jsem opravil i tady, takže myslím že je to zbytečný. Pokud bys chtěl dávat do waypointu něco jiného, např. zkráceninu názvu cache, asi bych to nedával do programu který komunikuje s GPS ale spíš do programu který mu připravuje data.