diff --git a/Setup.hs b/Setup.hs new file mode 100644 --- /dev/null +++ b/Setup.hs @@ -0,0 +1,2 @@ +import Distribution.Simple +main = defaultMain diff --git a/bench/Bench.hs b/bench/Bench.hs new file mode 100644 --- /dev/null +++ b/bench/Bench.hs @@ -0,0 +1,7 @@ +import BasicPrelude +import Criterion.Main + +main :: IO () +main = defaultMain + [ + ] diff --git a/gnss-converters.cabal b/gnss-converters.cabal new file mode 100644 --- /dev/null +++ b/gnss-converters.cabal @@ -0,0 +1,85 @@ +name: gnss-converters +version: 0.1.0 +synopsis: GNSS Converters. +description: Haskell bindings for GNSS converters. +homepage: http://github.com/swift-nav/gnss-converters +license: BSD3 +author: Swift Navigation Inc. +maintainer: Mark Fine +copyright: Copyright (C) 2015 Swift Navigation, Inc. +category: Network +build-type: Simple +cabal-version: >= 1.10 + +source-repository head + type: git + location: https://github.com/swift-nav/gnss-converters + +library + hs-source-dirs: src + exposed-modules: Data.RTCM3.SBP + , SwiftNav.SBP.RTCM3 + ghc-options: -Wall + build-depends: base >= 4.7 && < 5 + , basic-prelude + , conduit-extra + , lens + , rtcm + , sbp + default-language: Haskell2010 + default-extensions: NoImplicitPrelude + OverloadedStrings + +executable sbp2rtcm3 + hs-source-dirs: main + main-is: SBP2RTCM3.hs + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + build-depends: base + , basic-prelude + , gnss-converters + default-language: Haskell2010 + default-extensions: NoImplicitPrelude + OverloadedStrings + +executable rtcm32sbp + hs-source-dirs: main + main-is: RTCM32SBP.hs + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + build-depends: base + , basic-prelude + , binary-conduit + , conduit + , conduit-extra + , gnss-converters + , resourcet + default-language: Haskell2010 + default-extensions: NoImplicitPrelude + OverloadedStrings + +test-suite test + type: exitcode-stdio-1.0 + hs-source-dirs: test + main-is: Test.hs + build-depends: base + , basic-prelude + , tasty + , tasty-hunit + , gnss-converters + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + default-language: Haskell2010 + default-extensions: NoImplicitPrelude + OverloadedStrings + +benchmark bench + type: exitcode-stdio-1.0 + hs-source-dirs: bench + main-is: Bench.hs + build-depends: base + , basic-prelude + , criterion + , gnss-converters + ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall + default-language: Haskell2010 + default-extensions: NoImplicitPrelude + OverloadedStrings + diff --git a/main/RTCM32SBP.hs b/main/RTCM32SBP.hs new file mode 100644 --- /dev/null +++ b/main/RTCM32SBP.hs @@ -0,0 +1,26 @@ +-- | +-- Module: RTCM32SBP +-- Copyright: (c) 2015 Mark Fine +-- License: BSD3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- RTCM3 to SBP tool. + +import BasicPrelude +import Control.Monad.Trans.Resource +import Data.Conduit +import Data.Conduit.Binary +import qualified Data.Conduit.List as CL +import Data.Conduit.Serialization.Binary +import Data.RTCM3.SBP +import System.IO + +main :: IO () +main = runResourceT $ + sourceHandle stdin =$= + conduitDecode =$= + CL.mapMaybe convert =$= + conduitEncode $$ + sinkHandle stdout diff --git a/main/SBP2RTCM3.hs b/main/SBP2RTCM3.hs new file mode 100644 --- /dev/null +++ b/main/SBP2RTCM3.hs @@ -0,0 +1,14 @@ +-- | +-- Module: SBP2RTCM3 +-- Copyright: (c) 2015 Mark Fine +-- License: BSD3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- SBP to RTCM3 tool. + +import BasicPrelude + +main :: IO () +main = undefined diff --git a/src/Data/RTCM3/SBP.hs b/src/Data/RTCM3/SBP.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/SBP.hs @@ -0,0 +1,136 @@ +{-# LANGUAGE LambdaCase #-} + +-- | +-- Module: Data.RTCM3.SBP +-- Copyright: Copyright (C) 2015 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- RTCMv3 to SBP Conversions. + +module Data.RTCM3.SBP + ( convert + ) where + +import BasicPrelude +import Control.Lens +import Data.Bits +import Data.Word +import Data.RTCM3 +import SwiftNav.SBP + +fromEcefVal :: Int64 -> Double +fromEcefVal x = fromIntegral x / 10000 + +toGPSTime :: GpsObservationHeader -> ObsGPSTime +toGPSTime hdr = ObsGPSTime + { _obsGPSTime_tow = hdr ^. gpsObservationHeader_tow + , _obsGPSTime_wn = 1874 + } + +fromGpsObservationHeader :: GpsObservationHeader -> ObservationHeader +fromGpsObservationHeader hdr = ObservationHeader + { _observationHeader_t = toGPSTime hdr + , _observationHeader_n_obs = fromIntegral $ hdr ^. gpsObservationHeader_n + } + +toP :: GpsL1Observation -> GpsL1ExtObservation -> Word32 +toP l1 l1e = round $ p * 100 where + p = 0.02 * fromIntegral (l1 ^. gpsL1Observation_pseudorange) + + 299792.458 * fromIntegral (l1e ^. gpsL1ExtObservation_ambiguity) + +toL :: GpsL1Observation -> GpsL1ExtObservation -> CarrierPhase +toL l1 l1e = CarrierPhase + { _carrierPhase_i = fromIntegral $ iint `shiftR` 8 + , _carrierPhase_f = fromIntegral $ iint .&. 0xFF + } where + p = 0.02 * fromIntegral (l1 ^. gpsL1Observation_pseudorange) + + 299792.458 * fromIntegral (l1e ^. gpsL1ExtObservation_ambiguity) + lm :: Double + lm = p + 0.0005 * fromIntegral (l1 ^. gpsL1Observation_carrierMinusCode) + l = (lm - 22e6) / 0.190293673 + iint :: Int64 + iint = - round (l * 255.0) + +toCn0 :: GpsL1ExtObservation -> Word8 +toCn0 = (^. gpsL1ExtObservation_cnr) + +toLock :: GpsL1Observation -> Word16 +toLock _l1 = 0 + +toSid :: Word8 -> SBPGnssSignal +toSid sat = SBPGnssSignal + { _sBPGnssSignal_sat = fromIntegral $ sat - 1 + , _sBPGnssSignal_band = 0 + , _sBPGnssSignal_constellation = 0 + } + +fromObservation1002 :: Observation1002 -> PackedObsContent +fromObservation1002 obs = PackedObsContent + { _packedObsContent_P = toP l1 l1e + , _packedObsContent_L = toL l1 l1e + , _packedObsContent_cn0 = toCn0 l1e + , _packedObsContent_lock = toLock l1 + , _packedObsContent_sid = toSid sat + } where + sat = obs ^. observation1002_sat + l1 = obs ^. observation1002_l1 + l1e = obs ^. observation1002_l1e + +fromObservation1004 :: Observation1004 -> PackedObsContent +fromObservation1004 obs = PackedObsContent + { _packedObsContent_P = toP l1 l1e + , _packedObsContent_L = toL l1 l1e + , _packedObsContent_cn0 = toCn0 l1e + , _packedObsContent_lock = toLock l1 + , _packedObsContent_sid = toSid sat + } where + sat = obs ^. observation1004_sat + l1 = obs ^. observation1004_l1 + l1e = obs ^. observation1004_l1e + +fromMsg1005 :: Msg1005 -> MsgBasePosEcef +fromMsg1005 msg = MsgBasePosEcef + { _msgBasePosEcef_x = fromEcefVal $ msg ^. msg1005_reference ^. antennaReference_ecef_x + , _msgBasePosEcef_y = fromEcefVal $ msg ^. msg1005_reference ^. antennaReference_ecef_y + , _msgBasePosEcef_z = fromEcefVal $ msg ^. msg1005_reference ^. antennaReference_ecef_z + } + +fromMsg1006 :: Msg1006 -> MsgBasePosEcef +fromMsg1006 msg = MsgBasePosEcef + { _msgBasePosEcef_x = fromEcefVal $ msg ^. msg1006_reference ^. antennaReference_ecef_x + , _msgBasePosEcef_y = fromEcefVal $ msg ^. msg1006_reference ^. antennaReference_ecef_y + , _msgBasePosEcef_z = fromEcefVal $ msg ^. msg1006_reference ^. antennaReference_ecef_z + } + +fromMsg1002 :: Msg1002 -> MsgObs +fromMsg1002 msg = MsgObs + { _msgObs_header = fromGpsObservationHeader $ msg ^. msg1002_header + , _msgObs_obs = map fromObservation1002 $ msg ^. msg1002_observations + } + +fromMsg1004 :: Msg1004 -> MsgObs +fromMsg1004 msg = MsgObs + { _msgObs_header = fromGpsObservationHeader $ msg ^. msg1004_header + , _msgObs_obs = map fromObservation1004 $ msg ^. msg1004_observations + } + +convert :: RTCM3Msg -> Maybe SBPMsg +convert = \case + (RTCM3Msg1002 msg _rtcm3) -> Just $ + SBPMsgObs msg' $ toSBP msg' defaultSender where + msg' = fromMsg1002 msg + (RTCM3Msg1004 msg _rtcm3) -> Just $ + SBPMsgObs msg' $ toSBP msg' defaultSender where + msg' = fromMsg1004 msg + (RTCM3Msg1005 msg _rtcm3) -> Just $ + SBPMsgBasePosEcef msg' $ toSBP msg' defaultSender where + msg' = fromMsg1005 msg + (RTCM3Msg1006 msg _rtcm3) -> Just $ + SBPMsgBasePosEcef msg' $ toSBP msg' defaultSender where + msg' = fromMsg1006 msg + _rtcm3Msg -> Nothing + + diff --git a/src/SwiftNav/SBP/RTCM3.hs b/src/SwiftNav/SBP/RTCM3.hs new file mode 100644 --- /dev/null +++ b/src/SwiftNav/SBP/RTCM3.hs @@ -0,0 +1,20 @@ +-- | +-- Module: SwiftNav.SBP.RTCM3 +-- Copyright: Copyright (C) 2015 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Mark Fine +-- Stability: experimental +-- Portability: portable +-- +-- SBP to RTCMv3 Conversions. + +module SwiftNav.SBP.RTCM3 + ( convert + ) where + +import BasicPrelude +import Data.RTCM3 +import SwiftNav.SBP + +convert :: SBPMsg -> Maybe RTCM3Msg +convert = undefined diff --git a/test/Test.hs b/test/Test.hs new file mode 100644 --- /dev/null +++ b/test/Test.hs @@ -0,0 +1,10 @@ +import BasicPrelude +import Test.Tasty + +tests :: TestTree +tests = testGroup "Tests" + [ + ] + +main :: IO () +main = defaultMain tests