diff --git a/haskell/main/SBP2NMEA.hs b/haskell/main/SBP2NMEA.hs new file mode 100644 index 0000000000..8970df6ac6 --- /dev/null +++ b/haskell/main/SBP2NMEA.hs @@ -0,0 +1,137 @@ +{-# LANGUAGE NoImplicitPrelude #-} +{-# LANGUAGE OverloadedStrings #-} + +-- | +-- Module: SBP2JNMEA +-- Copyright: Copyright (C) 2020 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- SBP to NMEA tool - reads SBP binary from stdin and sends NMEA +-- to stdout. + +import BasicPrelude hiding (map) +import Control.Lens +import Control.Monad.Trans.Resource +import Data.Bits +import qualified Data.ByteString as BS +import qualified Data.ByteString.Char8 as BS8 +import Data.Conduit +import Data.Conduit.Binary +import Data.Conduit.List +import Data.Conduit.Serialization.Binary +import Data.Time +import Data.Time.Clock.POSIX +import Data.Word +import SwiftNav.SBP +import System.IO +import Text.Printf + +-- | NMEA time. +-- +timestamp :: MsgPosLlh -> ByteString +timestamp posLlh = BS8.pack $ printf "%s.%03u" times mods + where + epochs = 315964800 + offsets = 18 + (tows, mods) = divMod (posLlh ^. msgPosLlh_tow) 1000 + seconds = epochs - offsets + tows + time = posixSecondsToUTCTime $ fromIntegral seconds + times = formatTime defaultTimeLocale "%H%M%S" time + +-- | NMEA latitude. +-- +lat :: MsgPosLlh -> ByteString +lat posLlh = BS8.pack $ printf "%02u%010.7f" i (60 * f) + where + (i, f) = properFraction (abs $ posLlh ^. msgPosLlh_lat) :: (Word16, Double) + +-- | NMEA latitude direction. +-- +latDir :: MsgPosLlh -> ByteString +latDir posLlh = bool "S" "N" $ posLlh ^. msgPosLlh_lat > 0 + +-- | NMEA longitude. +-- +lon :: MsgPosLlh -> ByteString +lon posLlh = BS8.pack $ printf "%03u%010.7f" i (60 * f) + where + (i, f) = properFraction (abs $ posLlh ^. msgPosLlh_lon) :: (Word16, Double) + +-- | NMEA longitude direction. +-- +lonDir :: MsgPosLlh -> ByteString +lonDir posLlh = bool "W" "E" $ posLlh ^. msgPosLlh_lon > 0 + +-- | NMEA quality. +-- +quality :: MsgPosLlh -> ByteString +quality posLlh = case posLlh ^. msgPosLlh_flags .&. 0x7 of + 0 -> "0" + 1 -> "1" + 2 -> "2" + 3 -> "5" + 4 -> "4" + _ -> "0" + +-- | NMEA number of satellites. +-- +satellites :: MsgPosLlh -> ByteString +satellites posLlh = BS8.pack $ printf "%02u" $ posLlh ^. msgPosLlh_n_sats + +-- | NMEA altitude. +-- +height :: MsgPosLlh -> ByteString +height posLlh = BS8.pack $ printf "%.2f" $ posLlh ^. msgPosLlh_height + +-- | NMEA sentence for GGA for GPS. +-- +sentence :: MsgPosLlh -> ByteString +sentence posLlh = + BS8.intercalate "," + [ "GPGGA" + , timestamp posLlh + , lat posLlh + , latDir posLlh + , lon posLlh + , lonDir posLlh + , quality posLlh + , satellites posLlh + , "0.0" + , height posLlh + , "M" + , "0.0" + , "M" + , mempty + , mempty + ] + +-- | NMEA checksum. +-- +checksum :: ByteString -> ByteString +checksum s = BS8.pack $ printf "%02x" $ BS.foldl' xor 0 s + +-- | NMEA GGA for GPS. +-- +gpgga :: MsgPosLlh -> ByteString +gpgga posLlh = "$" <> s <> "*" <> c <> "\r\n" + where + s = sentence posLlh + c = checksum s + +-- | Encode a SBPMsg to a line of JSON. +encodeLine :: SBPMsg -> ByteString +encodeLine (SBPMsgPosLlh posLlh _msg) = gpgga posLlh +encodeLine _ = mempty + +main :: IO () +main = do + hSetBuffering stdin NoBuffering + hSetBuffering stdout NoBuffering + runResourceT $ + sourceHandle stdin + =$= conduitDecode + =$= map encodeLine + $$ sinkHandle stdout diff --git a/haskell/sbp.cabal b/haskell/sbp.cabal index 5bfcdcd731..eca6502d7e 100644 --- a/haskell/sbp.cabal +++ b/haskell/sbp.cabal @@ -82,6 +82,22 @@ executable sbp2json , sbp default-language: Haskell2010 +executable sbp2nmea + hs-source-dirs: main + main-is: SBP2NMEA.hs + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + build-depends: base + , basic-prelude + , binary-conduit + , bytestring + , conduit + , conduit-extra + , lens + , resourcet + , sbp + , time + default-language: Haskell2010 + executable sbp2prettyjson hs-source-dirs: main main-is: SBP2PRETTYJSON.hs diff --git a/scripts/travis_python_haskell.bash b/scripts/travis_python_haskell.bash index 8854b179f6..6bbd71c336 100755 --- a/scripts/travis_python_haskell.bash +++ b/scripts/travis_python_haskell.bash @@ -25,13 +25,15 @@ cp "$haskell_bins/sbp2json" . cp "$haskell_bins/sbp2prettyjson" . cp "$haskell_bins/json2sbp" . cp "$haskell_bins/json2json" . +cp "$haskell_bins/sbp2nmea" . tar -C "$haskell_bins" -czf sbp_linux_tools.tar.gz \ sbp2json \ sbp2prettyjson \ sbp2yaml \ json2sbp \ - json2json + json2json \ + sbp2nmea VERSION=$(git describe --always --tags --dirty) BUILD_TRIPLET=$(cc -dumpmachine)