SBP 2 NMEA implementation

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 <dev@swiftnav.com>
+-- 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

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

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)