Go to Qiita Advent Calendar 2024 Top
LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@nak435

AF-GYSFDMAXBから読み込んだGPS情報をPCで描画してみた

Posted at

動機

たまたま久しぶりにAF-GYSFDMAXBに触れる機会があったので、PCにつないでGPS情報を描画してみようと思い立った。

環境

Windows、Mac、Linux での動作を前提に、言語はPythonを使用。
ライブラリは、自由な座標に文字や図形を描画できるPygameと、GYSFDMAXBとUART通信するためにpySerialを使用した。

・ライブラリのインストール

$ pip install pygame
$ pip install pyserial

AF-GYSFDMAXBの接続

適当なUSBシリアル変換にAF-GYSFDMAXB（5V, GND, TXD, RXD）を接続、USB側をPCのUSBポートに接続する。特殊なチップを用いたUSBシリアル変換でない限り、OS標準のドライバで対応できるはず。
PCに接続すると、WindowsならCOMx、Macなら/dev/cu.usbmodemXX等でUSBシリアル変換を認識する。

serial.jpeg

プログラムを起動

後述するpythonプログラムを起動する。引数にCOMポートを指定する。

$ python gsh.py COMポート
例
#Windows
$ python gsh.py COM3

#Mac
$ python gsh.py /dev/cu.usbmodem1201

#Linux
$ python gsh.py /dev/ttyAMA0

画面

Pygameで描画しているので、基本的にどのプラットホームでも見た目は同じ。

しばらく眺めていると、信号の強弱の変化や衛星の位置が動く様子が見られ、なんか面白い

scr-m.png

捕捉している衛星の情報を三重円に描画

  • 丸の中の数字が衛星番号を表す
  • 緑色の丸：測位に利用している衛星、
    灰色の丸：測位に利用していないが 捕捉している衛星を示す
    丸無し：信号強度がほぼ0dB
  • 丸の大きさが信号強度（SN比）を示す。大きいほど強い
  • 北を0度とした衛星の方位角と、外周円が0度〜中心（天頂）を90度とした衛星の仰角が示す位置に緑色/灰色の丸を描画した
  • マウスを当てるとツールチップを表示：『#衛星番号，仰角，方位角，信号強度』

地表における移動の真方位と速度を描画

  • 移動の方向と速度を表示

屋内で測位しているので、本来は移動していないはずだが・・・

ウインドウ内のキー操作

  • スペース キーを押すと、経度・緯度の表示単位を切り替える
  • Mキーを押すと、Google Mapを開き、現在位置を表示する
  • ESCキーかQキーを押すとアプリを終了する

検証

政府が公開している「衛星配置表示アプリ」のWeb版で確認した衛星の位置とほぼ同じ位置に描画できている。

gnss.png

なお、衛星番号196以降の新しい衛星は、GYSFDMAXBでは捕捉できない

プログラムコード

綺麗に書けていませんが、ご自由に改変して結構です。

表示するには ここを クリック
gsh.py
# -*- coding:utf-8 -*-
import sys
import platform
import time
import serial
import math
import webbrowser
import threading
import pygame
from pygame.locals import *

if len(sys.argv) < 2:
    print(f"Usage: {sys.argv[0]} com-port")
    sys.exit()

COMPORT = sys.argv[1]

pf = platform.system()
FONTNAME = 'meiryoui' if pf == 'Windows' else 'pingfangsc' if pf == 'Darwin' else 'ipaexgothic'

PRINT_NMEA = False #True
WIDTH, HEIGHT = 600, 400
LEFT, RIGHT, RADIUS = 412, 235, 150

def byte2string(bData):
    try:
        return bData.decode('utf-8')
    except:
        return ''.join(map(chr, bData))

DISP_UNIT = 0
def conv_unit(val, dir):
    if DISP_UNIT == 0: return nmea2dd(val, dir)
    if DISP_UNIT == 1: return nmea2dms(val, dir)
    if DISP_UNIT == 2: return nmea2dm(val, dir)
    if DISP_UNIT == 3: return nmea2s(val, dir)
    return 'UNIT ERROR'
def nmea2dd(val, dir): #0
    ##NMEA format to DD format decimal (-999.99999)
    val = float(val)
    d = val // 100
    m = ((val / 100 - d) * 100) // 60
    s = (((val / 100 - d) * 100 - m) * 60) / 3600
    dms = d + m + s
    if dir == 'S' or dir == 'W': dms = -dms
    return "%.5f" % (dms)
def nmea2dms(val, dir): #1
    ##NMEA format to DMS format string (999°99'99.9X")
    val = float(val)
    d = val // 100
    m = int((val / 100 - d) * 100)
    s = ((val / 100 - d) * 100 - m) * 60
    return "%d°%d'%4.1f\"%s" % (d, m, s, dir)
def nmea2dm(val, dir): #2
    ##NMEA format to DM format string (999°-99.9999')
    val = float(val)
    d = val // 100
    m = (val / 100 - d) * 100
    if dir == 'S' or dir == 'W': m = -m
    return "%d°%.4f'" % (d, m)
def nmea2s(val, dir): #3
    ##NMEA format to S format decimal (-99999.999)
    val = float(val)
    d = val // 100
    m = ((val / 100 - d) * 100) // 60
    s = (((val / 100.0 - d) * 100.0 - m) * 60) / 3600
    dms = (d + m + s) * 3600
    if dir == 'S' or dir == 'W': dms = -dms
    return '%.3fs' % (dms)

def is_num(s):
    try:
        float(s)
    except ValueError:
        return False
    else:
        return True

gps = serial.Serial(COMPORT, 9600, timeout=1)

use_satellites, satellites_info = [], []
gpsQuality, n_use = 0, 0
latitude, longitude, altitude = 0, 0, 0
latitudeDir, longitudeDir = '', ''
direction, speed = 0, 0
dateTime = ''
gps_updating = False

screen, font18, font14, font10 = None, None, None, None

def render_size(font, text, antialias, color, background=None):
    surface = font.render(text, antialias, color, background)
    size = font.size(text)
    return surface, size

class Satellite_info:
    def __init__(self, prn, elevation, azimuth, snr):
        self.prn, self.elevation, self.azimuth, self.snr = \
            prn, elevation, azimuth, snr
    def __repr__(self):
        return f'<{self.prn}, {self.elevation}, {self.azimuth}, {self.snr}>'
    def isNone(self):
        return self.elevation == 0 and self.azimuth == 0 and self.snr == 0

def read_gps():
    global gps_updating
    global gpsQuality, n_use, dateTime, satellites_info, dateTime, satellites_info, use_satellites
    global latitude, longitude, altitude, latitudeDir, longitudeDir, direction, speed
    while True:
        buf = byte2string(gps.readline())
        if len(buf) == 0 or buf[0] != '$': continue
        sentence = buf[0:-2]

        if PRINT_NMEA: print(sentence) #受信したNMEAデータを見る
        part = sentence.split(',')
        gps_updating = True
        if part[0] == '$GPGGA':
            gpsQuality = int(part[6]) if is_num(part[6]) else 0
            if gpsQuality > 6: gpsQuality = 7
            if gpsQuality == 0: continue #is invalid
            n_use, latitude, longitude, altitude = int(part[7]), part[2], part[4], part[9]
            if not is_num(latitude) or not is_num(longitude): continue
            latitudeDir, longitudeDir = part[3], part[5]
        if part[0] == '$GPGSA':
            use_satellites = []
            for n in range(12):
                if not is_num(part[3 + n]): continue
                use_satellites.append(int(part[3 + n]))
        if part[0] == '$GPRMC':
            dmy, hms = part[9], part[1]
            if len(dmy) != 6 or len(hms) != 10: continue
            if not is_num(dmy) or not is_num(hms): continue
            year, month, mday, hour, minute, second = \
                int(dmy[4:6]) + 2000, int(dmy[2:4]), int(dmy[0:2]), \
                int(hms[0:2]), int(hms[2:4]), int(hms[4:6])
            # UTC to JST
            secs = time.mktime((year, month, mday, hour, minute, second, 0, 1, 0))
            secs += 9 * 3600 # +09:00
            local = time.localtime(secs)
            dateTime = f"{local.tm_year:04}/{local.tm_mon:02}/{local.tm_mday:02} {local.tm_hour:02}:{local.tm_min:02}:{local.tm_sec:02}"
        if part[0] == '$GPGSV':
            if n_use == 0: continue
            if part[2] == '1': satellites_info = []
            part[-1] = part[-1].split('*')[0]
            for n in range((len(part) // 4) - 1):
                if not is_num(part[4 + (4 * n) + 0]): continue
                prn = int(part[4 + (4 * n) + 0])
                elevation = int(part[4 + (4 * n) + 1]) if is_num(part[4 + (4 * n) + 1]) else 0
                azimuth = int(part[4 + (4 * n) + 2]) if is_num(part[4 + (4 * n) + 2]) else 0
                snr = float(part[4 + (4 * n) + 3]) if is_num(part[4 + (4 * n) + 3]) else 0
                satellite = Satellite_info(prn, elevation, azimuth, snr)
                if satellite.isNone(): continue
                satellites_info.append(satellite)

        if part[0] == '$GPVTG':
            if not is_num(part[1]) or not is_num(part[7]): continue
            direction, speed = float(part[1]), float(part[7])
        if part[0] == '$GPZDA':
            if PRINT_NMEA: print()
            gps_updating = False

def draw_text():
    text = font18.render(f'時刻：{dateTime}', True, "white")
    screen.blit(text, (5, 5))
    text = font14.render('(JST)', True, "white")
    screen.blit(text, (252, 9))
    GPSQuality = ["0:未測位", "1:GPS fix", "2:D-GPS fix", "3:Not applicable", "4:RTK Fixed", "5:RTK Float", "6:Dead reckoning", "Other"]
    text, size = render_size(font18, f'測位品質：{GPSQuality[gpsQuality]}', True, "white")
    screen.blit(text, (WIDTH - size[0] - 5, 5))
    text = font18.render(f'測位衛星：{n_use}個 {use_satellites}', True, "white")
    screen.blit(text, (5, 30))
    text = font18.render(f"緯度：{conv_unit(latitude, latitudeDir)}", True, "white")
    screen.blit(text, (5, 55))
    text = font18.render(f"経度：{conv_unit(longitude, longitudeDir)}", True, "white")
    screen.blit(text, (5, 80))
    text = font18.render(f"高度：{altitude}", True, "white")
    screen.blit(text, (5, 105))
    text = font18.render(f'衛星情報：{len(satellites_info)}個', True, "white")
    screen.blit(text, (176, 105))
    text, size = render_size(font18, '移動の方向と速度', True, "white")
    screen.blit(text, (95 - size[0] / 2, 166))

class SatelliteCircle():
    def __init__(self, satellite, font, tipfont):
        self.satellite = satellite

        elevation = (90 - self.satellite.elevation) * 3.3/2
        azimuth = (self.satellite.azimuth - 90) / 180 * math.pi
        self.x = LEFT + round(elevation * math.cos(azimuth))
        self.y = RIGHT + round(elevation * math.sin(azimuth))
        self.r = self.satellite.snr / 2
        r = self.r if self.r > 10 else 10
        self.rect = pygame.Rect(self.x - r, self.y - r, r * 2, r * 2)

        self.bcolour = "limegreen" if self.satellite.prn in use_satellites and self.satellite.snr > 0 else "silver"
        self.fbcolour = "red"
        self.current = False
        self.textsurface = font.render(f'{self.satellite.prn}', False, "black" if self.satellite.snr > 0 else "silver")
        tiptext = f'#{self.satellite.prn}, {self.satellite.elevation}°, {self.satellite.azimuth}°, {self.satellite.snr}dB'
        self.tiptextsurface = tipfont.render(tiptext, False, "black", "yellow")

    def drawCircle(self, display):
        color = self.fbcolour if self.current else self.bcolour
        pygame.draw.circle(display, color, (self.x, self.y), self.r)
        display.blit(self.textsurface, self.textsurface.get_rect(center = self.rect.center))

    def showTip(self, display):
        if self.current:
            x, y = pygame.mouse.get_pos()
            x += 16
            rect = self.tiptextsurface.get_rect()
            if x + rect[2] > WIDTH - 5:
                x -= (x + rect[2]) - (WIDTH - 5)
            display.blit(self.tiptextsurface, (x, y))

    def focusCheck(self, mousepos, mouseclick):
        self.current = self.rect.collidepoint(mousepos)
        return mouseclick if self.current else True

def draw_satellites():
    satellites = []
    for satellite in satellites_info:
        if satellite.isNone(): continue
        satelliteCircle = SatelliteCircle(satellite, font10, font18)
        satelliteCircle.drawCircle(screen)
        satellites.append(satelliteCircle)
    for satellite in satellites:
        mouse_pos, mouse_click = pygame.mouse.get_pos(), False
        _ = satellite.focusCheck(mouse_pos, mouse_click)
        satellite.showTip(screen)

def draw_dir_speed():
    r, d = 70, 12
    angle = (direction - 90) / 180 * math.pi
    x = 95 + round(r * math.cos(angle))
    y = 290 + round(r * math.sin(angle))
    pygame.draw.line(screen, "red", (95, 290), (x, y), width=3)

    d2 = d / 2
    t = math.tan(d2 / (r - d))
    a1, a2 = angle - t, angle + t
    r2 = math.sqrt((r - d) * (r - d) + (d2 * d2))
    x1, y1 = 95 + round(r2 * math.cos(a1)), 290 + round(r2 * math.sin(a1))
    x2, y2 = 95 + round(r2 * math.cos(a2)), 290 + round(r2 * math.sin(a2))
    pygame.draw.polygon(screen, "red", [(x, y), (x1, y1), (x2, y2)])

    text, size = render_size(font18, '%.1f°,' % (direction), True, "white")
    text = font18.render('%.1f°, %.1fkm/h' % (direction, speed), True, "white")
    screen.blit(text, (95 - size[0], 366))

def main():
    global screen, font18, font14, font10, DISP_UNIT
    pygame.init()
    screen = pygame.display.set_mode((WIDTH, HEIGHT))
    pygame.display.set_caption("GPS-GYSFDMAXB")
    font18 = pygame.font.SysFont(FONTNAME, 18)
    font14 = pygame.font.SysFont(FONTNAME, 14)
    font10 = pygame.font.SysFont(FONTNAME, 10)
    clock = pygame.time.Clock()

    gps_thread = threading.Thread(target=read_gps, daemon=True)
    gps_thread.start()

    done = False
    while not done:
        clock.tick(60)

        screen.fill("black")
        pygame.draw.circle(screen, "white", (LEFT, RIGHT), RADIUS, width=1)
        pygame.draw.circle(screen, "white", (LEFT, RIGHT), RADIUS / 3, width=1)
        pygame.draw.circle(screen, "white", (LEFT, RIGHT), RADIUS / 3 * 2, width=1)
        pygame.draw.line(screen, "white", (LEFT - RADIUS - 10, RIGHT), (LEFT + RADIUS + 9, RIGHT), width=1)
        pygame.draw.line(screen, "white", (LEFT, RIGHT - RADIUS - 10), (LEFT, RIGHT + RADIUS + 9), width=1)
        Noth, size = render_size(font18, 'N', True, "white")
        screen.blit(Noth, (LEFT - size[0] / 2 + 1, RIGHT - RADIUS - 12 - size[1] / 2))
        r = 70
        pygame.draw.circle(screen, "white", (95, 290), r, width=1)
        pygame.draw.line(screen, "white", (95, 290 + r - 1), (95, 290 - r - 3), width=1)
        pygame.draw.line(screen, "white", (95 + r - 1, 290), (95 - r, 290), width=1)
        screen.blit(Noth, (95 - size[0] / 2 + 1, 290 - r - 12 - size[1] / 2))

        if not gps_updating:
            draw_text()
            draw_satellites()
            draw_dir_speed()
            pygame.display.update()

        for event in pygame.event.get():
            if event.type == QUIT:
                done = True

            if event.type == KEYUP and (event.key == K_ESCAPE or event.key == K_q):
                done = True

            if event.type == KEYUP and event.key == K_m: #Map
                if latitude != 0 and longitude != 0:
                    url = f'https://www.google.com/maps?q={nmea2dd(latitude, latitudeDir)},{nmea2dd(longitude, longitudeDir)}'
                    if pf == 'Linux':
                        webbrowser.Mozilla('firefox').open(url, new=2, autoraise=True)
                    else:
                        webbrowser.open(url, new=2, autoraise=True)

            if event.type == KEYUP and event.key == K_SPACE: #change unit
                DISP_UNIT += 1
                DISP_UNIT %= 3 #or 4

    pygame.quit()

if __name__ == "__main__":
    main()

漢字を表示するため、Windowsではmeiryouiフォント、Macではpingfangscフォントを使用した。文字化けする場合は、18行目のフォント名を変更してみてほしい。

  • 次の環境で動作確認済み
platform python pygame pyserial
Windows 11 23H2 3.9.13 2.6 3.5
M1 Mac 14.6.1 3.11.9 2.5.2 3.5
Ubuntu 24.04.1 3.12.3 2.6.0 3.5
Raspberry Pi 4 Bookworm 3.11.2 2.6.0 3.5

※ Raspberry Pi OS 12.6（Bookworm）と Ubuntu では Pygame で使える日本語フォントが無かったため、次に示す ipaexフォントをインストールした。

$ sudo apt install -y fonts-ipaexfont

参考情報


  • Pygameで使える日本語フォントを調べてみた


  • NMEAデータは自分で解析せずとも、pythonライブラリが使えます

以上

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?