ecefllhcov.py

import sys

import gnss
import sbp

from typing import Union

from sbp.client import Handler, Framer
from sbp.client.drivers.file_driver import FileDriver

from sbp.navigation import MsgPosECEF, MsgPosECEFCov, MsgPosECEFCovGnss, MsgPosECEFDepA, MsgPosECEFGnss
from sbp.navigation import MsgPosLLH, MsgPosLLHCov, MsgPosLLHCovGnss, MsgPosLLHDepA, MsgPosLLHGnss

from sbp.navigation import SBP_MSG_POS_ECEF, SBP_MSG_POS_ECEF_COV, SBP_MSG_POS_ECEF_COV_GNSS, SBP_MSG_POS_ECEF_DEP_A, SBP_MSG_POS_ECEF_GNSS

ECEF_MESSAGE_TYPES = [SBP_MSG_POS_ECEF, SBP_MSG_POS_ECEF_COV, SBP_MSG_POS_ECEF_COV_GNSS, SBP_MSG_POS_ECEF_DEP_A, SBP_MSG_POS_ECEF_GNSS]

EcefUnion = Union[MsgPosECEF, MsgPosECEFCov, MsgPosECEFCovGnss, MsgPosECEFDepA, MsgPosECEFGnss]
LlhUnion = Union[MsgPosLLH, MsgPosLLHCov, MsgPosLLHCovGnss, MsgPosLLHDepA, MsgPosLLHGnss]

def llh_to_ecef(msg: EcefUnion) -> LlhUnion:
    if type(msg) == MsgPosECEF:
        (lat, lon, height) = gnss.llh_from_ecef((msg.x, msg.y, msg.z))
        return MsgPosLLH(sender=msg.sender,
                         tow=msg.tow,
                         n_sats=msg.n_sats, 
                         flags=msg.flags,
                         lat=lat,
                         lon=lon,
                         height=height,
                         h_accuracy=0, # TODO: what to put here?
                         v_accuracy=0,
                         )
    if type(msg) == MsgPosECEFCov:
        (lat, lon, height) = gnss.llh_from_ecef((msg.x, msg.y, msg.z))
        return MsgPosLLHCov(sender=msg.sender,
                            tow=msg.tow,
                            n_sats=msg.n_sats, 
                            flags=msg.flags,
                            lat=lat,
                            lon=lon,
                            height=height,
                            cov_n_n=0, # TODO
                            cov_n_e=0,
                            cov_n_d=0,
                            cov_e_e=0,
                            cov_e_d=0,
                            cov_d_d=0,
                            )
    if type(msg) == MsgPosECEFCovGnss:
        (lat, lon, height) = gnss.llh_from_ecef((msg.x, msg.y, msg.z))
        return MsgPosLLHCovGnss(sender=msg.sender,
                                tow=msg.tow,
                                n_sats=msg.n_sats, 
                                flags=msg.flags,
                                lat=lat,
                                lon=lon,
                                height=height,
                                cov_n_n=0, # TODO
                                cov_n_e=0,
                                cov_n_d=0,
                                cov_e_e=0,
                                cov_e_d=0,
                                cov_d_d=0,
                                )
    if type(msg) == MsgPosECEFDepA:
        (lat, lon, height) = gnss.llh_from_ecef((msg.x, msg.y, msg.z))
        return MsgPosLLHDepA(sender=msg.sender,
                             tow=msg.tow,
                             n_sats=msg.n_sats, 
                             flags=msg.flags,
                             lat=lat,
                             lon=lon,
                             height=height,
                             h_accuracy=0, # TODO
                             v_accuracy=0,
                             )
    if type(msg) == MsgPosECEFGnss:
        (lat, lon, height) = gnss.llh_from_ecef((msg.x, msg.y, msg.z))
        return MsgPosLLHGnss(sender=msg.sender,
                             tow=msg.tow,
                             n_sats=msg.n_sats, 
                             flags=msg.flags,
                             lat=lat,
                             lon=lon,
                             height=height,
                             h_accuracy=0, # TODO
                             v_accuracy=0,
                             )


with FileDriver(sys.stdin.buffer) as driver:
    with Handler(Framer(driver.read, None)) as source:
        for msg, _ in source.filter():
            sys.stdout.buffer.write(msg.to_binary())
            if msg.msg_type in ECEF_MESSAGE_TYPES:
                llh_msg = llh_to_ecef(msg)
                sys.stdout.buffer.write(llh_msg.to_binary())

pyproject.toml

[tool.poetry]
name = "ecefllhcov"
version = "0.1.0"
description = ""
authors = ["Jason Mobarak <jason@swiftnav.com>"]

[tool.poetry.dependencies]
python = "^3.7"
sbp = "^3.1.1"
gnss = { git = "git@github.com:swift-nav/pygnss.git", branch = "v0.4.0" }

[tool.poetry.dev-dependencies]
jedi = "^0.17.0"

[build-system]
requires = ["poetry>=0.12"]
build-backend = "poetry.masonry.api"