gnss-converters 0.3.8 → 0.3.9
raw patch · 9 files changed
+157/−609 lines, 9 filesdep +aesondep +aeson-prettydep +tasty-goldendep −HUnit-approx
Dependencies added: aeson, aeson-pretty, tasty-golden
Dependencies removed: HUnit-approx
Files
- gnss-converters.cabal +10/−3
- main/RTCM32SBP.hs +4/−4
- main/SBP2RTCM3.hs +13/−1
- src/Data/RTCM3/SBP.hs +11/−196
- src/Data/RTCM3/SBP/Observations.hs +22/−25
- src/Data/RTCM3/SBP/Positions.hs +56/−0
- src/Data/RTCM3/SBP/Types.hs +1/−3
- src/SwiftNav/SBP/RTCM3.hs +4/−3
- test/Test/Data/RTCM3/SBP.hs +36/−374
gnss-converters.cabal view
@@ -1,5 +1,5 @@ name: gnss-converters-version: 0.3.8+version: 0.3.9 cabal-version: >=1.22 build-type: Simple license: BSD3@@ -20,6 +20,7 @@ exposed-modules: Data.RTCM3.SBP Data.RTCM3.SBP.Observations+ Data.RTCM3.SBP.Positions Data.RTCM3.SBP.Time Data.RTCM3.SBP.Types SwiftNav.SBP.RTCM3@@ -47,7 +48,11 @@ build-depends: base >=4.8.2.0, basic-prelude >=0.5.2,- gnss-converters+ binary-conduit >=1.2.4.1,+ conduit >=1.2.10,+ conduit-extra >=1.1.16,+ gnss-converters,+ resourcet >=1.1.9 default-language: Haskell2010 hs-source-dirs: main ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall@@ -70,7 +75,8 @@ type: exitcode-stdio-1.0 main-is: Test.hs build-depends:- HUnit-approx >=1.1,+ aeson >=0.11.3.0,+ aeson-pretty >=0.7.2, base >=4.8.2.0, basic-prelude >=0.5.2, binary >=0.7.5.0,@@ -84,6 +90,7 @@ rtcm >=0.2.2, sbp >=2.2.11, tasty >=0.11.2.1,+ tasty-golden >=2.3.1.1, tasty-hunit >=0.9.2, text >=1.2.2.2, unordered-containers >=0.2.8.0
main/RTCM32SBP.hs view
@@ -23,8 +23,8 @@ main = do s <- newStore runResourceT $ runConvertT s $- sourceHandle stdin =$=- conduitDecode =$=- converter =$=- conduitEncode $$+ sourceHandle stdin =$=+ conduitDecode =$=+ awaitForever converter =$=+ conduitEncode $$ sinkHandle stdout
main/SBP2RTCM3.hs view
@@ -11,6 +11,18 @@ -- SBP to RTCM3 tool. import BasicPrelude+import Control.Monad.Trans.Resource+import Data.Conduit+import Data.Conduit.Binary+import Data.Conduit.Serialization.Binary+import SwiftNav.SBP.RTCM3+import System.IO main :: IO ()-main = undefined+main =+ runResourceT $+ sourceHandle stdin =$=+ conduitDecode =$=+ awaitForever converter =$=+ conduitEncode $$+ sinkHandle stdout
src/Data/RTCM3/SBP.hs view
@@ -12,219 +12,34 @@ -- RTCMv3 to SBP Conversions. module Data.RTCM3.SBP- ( toWn- , mjdEpoch- , convert- , converter+ ( converter , newStore- , validateIodcIode- , gpsUriToUra ) where import BasicPrelude-import Control.Lens-import Data.Bits import Data.Conduit import Data.IORef import Data.RTCM3+import qualified Data.RTCM3.SBP.Ephemerides as Ephemerides import qualified Data.RTCM3.SBP.Observations as Observations-import Data.RTCM3.SBP.Time+import qualified Data.RTCM3.SBP.Positions as Positions import Data.RTCM3.SBP.Types-import Data.Word import SwiftNav.SBP -{-# ANN module ("HLint: ignore Redundant if"::String) #-}------------------------------------------------------------------------------------- SBP, GNSS, RTCM constant definitions---- | The official GPS value of Pi. This is the value used by the CS to curve--- fit ephemeris parameters and should be used in all ephemeris calculations.-gpsPi :: Double-gpsPi = 3.1415926535898------------------------------------------------------------------------------------- General utilities--applyScaleFactor :: (Floating a, Integral b) => a -> b -> a-applyScaleFactor p x = (2 ** p) * fromIntegral x------------------------------------------------------------------------------------- GNSS RTCM observation reconstruction utilities---fromEcefVal :: Int64 -> Double-fromEcefVal x = fromIntegral x / 10000---- | Produce GPS time from RTCM time (week-number % 1024, tow in seconds, scaled 2^4).--- Stateful but non-side-effecting. The week number is mod-1024 - add in the base--- wn from the stored wn masked to 10 bits.-toGpsTimeSec :: MonadStore e m => Word16 -> Word16 -> m GpsTimeSec-toGpsTimeSec wn tow = do- stateWn <- view storeWn >>= liftIO . readIORef- return GpsTimeSec- { _gpsTimeSec_tow = 16 * fromIntegral tow- , _gpsTimeSec_wn = stateWn `shiftR` 10 `shiftL` 10 + wn- }---- | MJD GPS Epoch - First day in GPS week 0. See DF051 of the RTCM3 spec----mjdEpoch :: Word16-mjdEpoch = 44244---- | Convert from MJD to GPS week number------ See DF051 of the RTCM3 spec-toWn :: Word16 -> Word16-toWn mjd = (mjd - mjdEpoch) `div` 7---- | Validate IODE/IODC flags. The IODC and IODE flags (least significant bits) should be--- equal. IODC is Word16 while IODE is Word8, so we shift IODC to drop the most significant bits.--- We return a 1 if valid, 0 if invalid.-validateIodcIode :: Word16 -> Word8 -> Word8-validateIodcIode iodc iode =- if iodc' == iode then 1 else 0- where- iodc' = fromIntegral $ iodc `shiftL` 8 `shiftR` 8---- | Construct an EphemerisCommonContent from an RTCM 1019 message.-toGpsEphemerisCommonContent :: MonadStore e m => Msg1019 -> m EphemerisCommonContent-toGpsEphemerisCommonContent m = do- toe <- toGpsTimeSec (m ^. msg1019_ephemeris ^. gpsEphemeris_wn) (m ^. msg1019_ephemeris ^. gpsEphemeris_toe)- return EphemerisCommonContent- { _ephemerisCommonContent_sid = GnssSignal16- { _gnssSignal16_sat = m ^. msg1019_header ^. gpsEphemerisHeader_sat- , _gnssSignal16_code = 0 -- there is an L2P status flag in msg 1019, but I don't think that applies- }- , _ephemerisCommonContent_toe = toe- , _ephemerisCommonContent_ura = gpsUriToUra (fromIntegral $ m ^. msg1019_ephemeris ^. gpsEphemeris_svHealth)- , _ephemerisCommonContent_fit_interval = decodeFitInterval (m ^. msg1019_ephemeris ^. gpsEphemeris_fitInterval) (m ^. msg1019_ephemeris ^. gpsEphemeris_iodc)- , _ephemerisCommonContent_valid = validateIodcIode (m ^. msg1019_ephemeris ^. gpsEphemeris_iodc) (m ^. msg1019_ephemeris ^. gpsEphemeris_iode)- , _ephemerisCommonContent_health_bits = m ^. msg1019_ephemeris ^. gpsEphemeris_svHealth- }---- | 16 bit SBP Sender Id is 12 bit RTCM Station Id with high nibble or'd in----toSender :: Word16 -> Word16-toSender = (.|. 0xf000)---- | Decode SBP 'fitInterval' from RTCM/GPS 'fitIntervalFlag' and IODC.--- Implementation adapted from libswiftnav/ephemeris.c.-decodeFitInterval :: Bool -> Word16 -> Word32-decodeFitInterval fitInt iodc- | not fitInt = 4 * 60 * 60- | iodc >= 240 && iodc <= 247 = 8 * 60 * 60- | (iodc >= 248 && iodc <= 255) || iodc == 496 = 14 * 60 * 60- | (iodc >= 497 && iodc <= 503) || (iodc >= 1021 && iodc <= 1023) = 26 * 60 * 60- | iodc >= 504 && iodc <= 510 = 50 * 60 * 60- | iodc == 511 || (iodc >= 752 && iodc <= 756) = 74 * 60 * 60- | iodc == 757 = 98 * 60 * 60- | otherwise = 6 * 60 * 60---- | Convert between RTCM/GPS URA ("User Range Accuracy") index to a number in meters.--- See section 2.5.3, "User Range Accuracy", in the GPS signal specification.--- Indices 1, 3, and 5 are hard-coded according to spec, and 15 is hard-coded according--- to SBP/Piksi convention.-gpsUriToUra :: Double -> Double-gpsUriToUra uri- | uri < 0 = -1- | uri == 1 = 2.8- | uri == 3 = 5.7- | uri == 5 = 11.3- | uri == 15 = 6144- | uri <= 6 = 2 ** (1 + (uri / 2))- | uri > 6 && uri < 15 = 2 ** (uri - 2)- | otherwise = -1------------------------------------------------------------------------------------- RTCM to SBP conversion utilities: RTCM Msgs. 1002 (L1 RTK), 1004 (L1+L2 RTK),--- 1005 (antenna position), 1006 (antenna position).---- | Convert an RTCM 1005 antenna reference position message into an SBP--- MsgBasePosEcef.-fromMsg1005 :: MonadStore e m => Msg1005 -> m MsgBasePosEcef-fromMsg1005 m =- return MsgBasePosEcef- { _msgBasePosEcef_x = fromEcefVal $ m ^. msg1005_reference ^. antennaReference_ecef_x- , _msgBasePosEcef_y = fromEcefVal $ m ^. msg1005_reference ^. antennaReference_ecef_y- , _msgBasePosEcef_z = fromEcefVal $ m ^. msg1005_reference ^. antennaReference_ecef_z- }---- | Convert an RTCM 1006 antenna reference position message into an SBP--- MsgBasePosEcef.-fromMsg1006 :: MonadStore e m => Msg1006 -> m MsgBasePosEcef-fromMsg1006 m =- return MsgBasePosEcef- { _msgBasePosEcef_x = fromEcefVal $ m ^. msg1006_reference ^. antennaReference_ecef_x- , _msgBasePosEcef_y = fromEcefVal $ m ^. msg1006_reference ^. antennaReference_ecef_y- , _msgBasePosEcef_z = fromEcefVal $ m ^. msg1006_reference ^. antennaReference_ecef_z- }---- | Convert an RTCM 1019 GPS ephemeris message into an SBP MsgEphemerisGps.-fromMsg1019 :: MonadStore e m => Msg1019 -> m MsgEphemerisGps-fromMsg1019 m = do- commonContent <- toGpsEphemerisCommonContent m- toc <- toGpsTimeSec (m ^. msg1019_ephemeris ^. gpsEphemeris_wn) (m ^. msg1019_ephemeris ^. gpsEphemeris_toc)- return MsgEphemerisGps- { _msgEphemerisGps_common = commonContent- , _msgEphemerisGps_tgd = applyScaleFactor (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_tgd- , _msgEphemerisGps_c_rs = applyScaleFactor (-5) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_rs- , _msgEphemerisGps_c_rc = applyScaleFactor (-5) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_rc- , _msgEphemerisGps_c_uc = applyScaleFactor (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_uc- , _msgEphemerisGps_c_us = applyScaleFactor (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_us- , _msgEphemerisGps_c_ic = applyScaleFactor (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_ic- , _msgEphemerisGps_c_is = applyScaleFactor (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_is- , _msgEphemerisGps_dn = gpsPi * applyScaleFactor (-43) (m ^. msg1019_ephemeris ^. gpsEphemeris_dn)- , _msgEphemerisGps_m0 = gpsPi * applyScaleFactor (-31) (m ^. msg1019_ephemeris ^. gpsEphemeris_m0)- , _msgEphemerisGps_ecc = applyScaleFactor (-33) $ m ^. msg1019_ephemeris ^. gpsEphemeris_ecc- , _msgEphemerisGps_sqrta = applyScaleFactor (-19) $ m ^. msg1019_ephemeris ^. gpsEphemeris_sqrta- , _msgEphemerisGps_omega0 = gpsPi * applyScaleFactor (-31) (m ^. msg1019_ephemeris ^. gpsEphemeris_omega0)- , _msgEphemerisGps_omegadot = gpsPi * applyScaleFactor (-43) (m ^. msg1019_ephemeris ^. gpsEphemeris_omegadot)- , _msgEphemerisGps_w = gpsPi * applyScaleFactor (-31) (m ^. msg1019_ephemeris ^. gpsEphemeris_w)- , _msgEphemerisGps_inc = gpsPi * applyScaleFactor (-31) (m ^. msg1019_ephemeris ^. gpsEphemeris_i0)- , _msgEphemerisGps_inc_dot = gpsPi * applyScaleFactor (-43) (m ^. msg1019_ephemeris ^. gpsEphemeris_idot)- , _msgEphemerisGps_af0 = applyScaleFactor (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af0- , _msgEphemerisGps_af1 = applyScaleFactor (-43) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af1- , _msgEphemerisGps_af2 = applyScaleFactor (-55) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af2- , _msgEphemerisGps_iodc = m ^. msg1019_ephemeris ^. gpsEphemeris_iodc- , _msgEphemerisGps_iode = m ^. msg1019_ephemeris ^. gpsEphemeris_iode- , _msgEphemerisGps_toc = toc- }--msg1005Converter :: MonadStore e m => Msg1005 -> Conduit i m [SBPMsg]-msg1005Converter m = do- m' <- fromMsg1005 m- yield [SBPMsgBasePosEcef m' $ toSBP m' $ toSender $ m ^. msg1005_reference ^. antennaReference_station]--msg1006Converter :: MonadStore e m => Msg1006 -> Conduit i m [SBPMsg]-msg1006Converter m = do- m' <- fromMsg1006 m- yield [SBPMsgBasePosEcef m' $ toSBP m' $ toSender $ m ^. msg1006_reference ^. antennaReference_station]--msg1019Converter :: MonadStore e m => Msg1019 -> Conduit i m [SBPMsg]-msg1019Converter m = do- m' <- fromMsg1019 m- yield [SBPMsgEphemerisGps m' $ toSBP m' $ toSender 0]- -- | Convert an RTCM message into possibly multiple SBP messages. ---convert :: MonadStore e m => RTCM3Msg -> Conduit i m [SBPMsg]-convert = \case+converter :: MonadStore e m => RTCM3Msg -> Conduit i m [SBPMsg]+converter = \case (RTCM3Msg1002 m _rtcm3) -> Observations.converter m (RTCM3Msg1004 m _rtcm3) -> Observations.converter m (RTCM3Msg1010 m _rtcm3) -> Observations.converter m (RTCM3Msg1012 m _rtcm3) -> Observations.converter m- (RTCM3Msg1005 m _rtcm3) -> msg1005Converter m- (RTCM3Msg1006 m _rtcm3) -> msg1006Converter m- (RTCM3Msg1019 m _rtcm3) -> msg1019Converter m+ (RTCM3Msg1005 m _rtcm3) -> Positions.converter m+ (RTCM3Msg1006 m _rtcm3) -> Positions.converter m+ (RTCM3Msg1019 m _rtcm3) -> Ephemerides.converter m _rtcm3Msg -> mempty --- | Convert an RTCM message into possibly multiple SBP messages.+-- | Setup new storage for converter. ---converter :: MonadStore e m => Conduit RTCM3Msg m [SBPMsg]-converter = awaitForever convert--newStore :: IO Store-newStore = do- t <- currentGpsTime- Store <$> newIORef (t ^. gpsTimeNano_wn) <*> newIORef t <*> newIORef mempty <*> newIORef mempty+newStore :: MonadIO m => m Store+newStore = liftIO $ Store <$> newIORef mempty <*> newIORef mempty
src/Data/RTCM3/SBP/Observations.hs view
@@ -39,12 +39,13 @@ -- | Update and convert stored and incoming GPS times. ---toGpsTimeNano :: MonadStore e m => Word16 -> (GpsTimeNano -> GpsTimeNano) -> m GpsTimeNano+toGpsTimeNano :: MonadStore e m => Word16 -> (GpsTimeNano -> GpsTimeNano) -> m (GpsTimeNano, GpsTimeNano) toGpsTimeNano station rollover = do timeMap <- view storeGpsTimeMap modifyIORefM timeMap $ \timeMap' -> do- t <- rollover <$> maybe currentGpsTime return (timeMap' ^. at station)- return (timeMap' & at station ?~ t, t)+ t <- maybe currentGpsTime return (timeMap' ^. at station)+ let t' = rollover t+ return (timeMap' & at station ?~ t', (t, t')) -- | Default observation doppler. --@@ -302,7 +303,7 @@ -- | FromObservations produces GPS time, SBP packed observations, and sender from RTCMv3 observation messages. -- class FromObservations a where- gpsTimeNano :: MonadStore e m => a -> m GpsTimeNano+ gpsTimeNano :: MonadStore e m => a -> m (GpsTimeNano, GpsTimeNano) packedObsContents :: a -> [PackedObsContent] sender :: a -> Word16 synchronous :: a -> Bool@@ -350,24 +351,20 @@ -- converter :: (MonadStore e m, FromObservations a) => a -> Conduit i m [SBPMsg] converter m = do- time <- view storeGpsTime- t <- liftIO $ readIORef time- t' <- gpsTimeNano m- when (t' >= t) $ do- observations <- view storeObservations- obs <- liftIO $ readIORef observations- when (t' > t) $ do- liftIO $ writeIORef time t'- unless (null obs) $ do- liftIO $ writeIORef observations mempty- ms <- toMsgObs t (toList obs) $ sender m- yield ms- let obs' = fromList $ packedObsContents m- if synchronous m then liftIO $ modifyIORef' observations (obs' <>) else do- obs'' <- liftIO $ readIORef observations- unless (null obs'') $- liftIO $ writeIORef observations mempty- let obs''' = obs' <> obs''- unless (null obs''') $ do- ms <- toMsgObs t' (toList obs''') $ sender m- yield ms+ (t, t') <- gpsTimeNano m+ observations <- view storeObservations+ obs <- liftIO $ readIORef observations+ when (t' /= t) $+ unless (null obs) $ do+ liftIO $ writeIORef observations mempty+ ms <- toMsgObs t (toList obs) $ sender m+ yield ms+ let obs' = fromList $ packedObsContents m+ if synchronous m then liftIO $ modifyIORef' observations (obs' <>) else do+ obs'' <- liftIO $ readIORef observations+ unless (null obs'') $+ liftIO $ writeIORef observations mempty+ let obs''' = obs' <> obs''+ unless (null obs''') $ do+ ms <- toMsgObs t' (toList obs''') $ sender m+ yield ms
+ src/Data/RTCM3/SBP/Positions.hs view
@@ -0,0 +1,56 @@+{-# LANGUAGE NoImplicitPrelude #-}++-- |+-- Module: Data.RTCM3.SBP.Positions+-- Copyright: Copyright (C) 2016 Swift Navigation, Inc.+-- License: LGPL-3+-- Maintainer: Swift Navigation <dev@swiftnav.com>+-- Stability: experimental+-- Portability: portable+--+-- RTCMv3 to SBP Position Conversions.++module Data.RTCM3.SBP.Positions+ ( converter+ ) where++import BasicPrelude+import Control.Lens+import Data.Bits+import Data.Conduit+import Data.RTCM3+import Data.Word+import SwiftNav.SBP++-- | Derive sender from station.+--+toSender :: Word16 -> Word16+toSender station = station .|. 61440++-- | FromPosition produces ecef x, y, x and sender values from RTCMv3 position messages.+--+class FromPosition a where+ ecefX :: a -> Int64+ ecefY :: a -> Int64+ ecefZ :: a -> Int64+ sender :: a -> Word16++instance FromPosition Msg1005 where+ ecefX = view (msg1005_reference . antennaReference_ecef_x)+ ecefY = view (msg1005_reference . antennaReference_ecef_y)+ ecefZ = view (msg1005_reference . antennaReference_ecef_z)+ sender = toSender . view (msg1005_reference . antennaReference_station)++instance FromPosition Msg1006 where+ ecefX = view (msg1006_reference . antennaReference_ecef_x)+ ecefY = view (msg1006_reference . antennaReference_ecef_y)+ ecefZ = view (msg1006_reference . antennaReference_ecef_z)+ sender = toSender . view (msg1006_reference . antennaReference_station)++converter :: (Monad m, FromPosition a) => a -> Conduit i m [SBPMsg]+converter m = do+ let x = fromIntegral (ecefX m) / 10000+ y = fromIntegral (ecefY m) / 10000+ z = fromIntegral (ecefZ m) / 10000+ m' = MsgBasePosEcef x y z+ yield [SBPMsgBasePosEcef m' $ toSBP m' $ toSender $ sender m]
src/Data/RTCM3/SBP/Types.hs view
@@ -71,9 +71,7 @@ liftBase = liftBaseDefault data Store = Store- { _storeWn :: IORef Word16- , _storeGpsTime :: IORef GpsTimeNano- , _storeGpsTimeMap :: IORef GpsTimeNanoMap+ { _storeGpsTimeMap :: IORef GpsTimeNanoMap , _storeObservations :: IORef (Vector PackedObsContent) } deriving ( Eq )
@@ -12,13 +12,14 @@ -- SBP to RTCMv3 Conversions. module SwiftNav.SBP.RTCM3- ( convert+ ( converter ) where import BasicPrelude+import Data.Conduit import Data.RTCM3 import SwiftNav.SBP -convert :: Monad m => SBPMsg -> m [RTCM3Msg]-convert = \case+converter :: Monad m => SBPMsg -> Conduit i m [RTCM3Msg]+converter = \case _sbpMsg -> return mempty
test/Test/Data/RTCM3/SBP.hs view
@@ -1,390 +1,52 @@-{-# LANGUAGE CPP #-}-{-# LANGUAGE FlexibleContexts #-}-{-# LANGUAGE LambdaCase #-} {-# LANGUAGE NoImplicitPrelude #-} {-# LANGUAGE OverloadedStrings #-} --- |--- Module: Test.Data.RTCM3.SBP--- Copyright: (c) 2016 Swift Navigation Inc.--- License: LGPL-3--- Maintainer: Swift Navigation <dev@swift-nav.com>------ Test RTCMv3 to SBP Conversions.- module Test.Data.RTCM3.SBP ( tests ) where -import BasicPrelude-import Control.Lens-import Control.Monad.Trans.Resource-import Data.Binary-import qualified Data.ByteString.Lazy as LBS-import Data.Conduit-import Data.Conduit.Binary hiding (head)-import qualified Data.Conduit.List as CL-import Data.Conduit.Serialization.Binary-import Data.HashMap.Strict hiding (filter, mapMaybe)-import Data.IORef-import Data.RTCM3.SBP-import Data.RTCM3.SBP.Types-import SwiftNav.SBP-import Test.HUnit.Approx-import Test.Tasty-import Test.Tasty.HUnit--decodeRTCMFile :: Word16 -> FilePath -> IO [SBPMsg]-decodeRTCMFile wn filename = do- s <- Store <$> newIORef wn <*> newIORef mempty- runResourceT $ runConvertT s $ runConduit $- sourceFile filename =$=- conduitDecode =$=- CL.concatMapM convert $$- CL.consume--decodeSBPFile :: FilePath -> IO [SBPMsg]-decodeSBPFile filename = do- runResourceT $ runConduit $- sourceFile filename =$=- conduitDecode $$- CL.consume--basePosition :: MsgBasePosEcef -> (Double, Double, Double)-basePosition msg' = ( msg' ^. msgBasePosEcef_x- , msg' ^. msgBasePosEcef_y- , msg' ^. msgBasePosEcef_z- )--isL1 :: PackedObsContent -> Bool-isL1 obs = (obs ^. packedObsContent_sid ^. gnssSignal16_code) == 0--isL2 :: PackedObsContent -> Bool-isL2 obs = code == 1 || code == 6- where- code = obs ^. packedObsContent_sid ^. gnssSignal16_code--sat :: PackedObsContent -> Word8-sat obs = obs ^. packedObsContent_sid ^. gnssSignal16_sat--cn0 :: PackedObsContent -> Double-cn0 obs = 0.25 * fromIntegral (obs ^. packedObsContent_cn0)--pseudorange :: PackedObsContent -> Double-pseudorange obs = fromIntegral (obs ^. packedObsContent_P) / 50--carrierPhase :: PackedObsContent -> Double-carrierPhase obs = whole + fraction- where- phase = obs ^. packedObsContent_L- whole = fromIntegral (phase ^. carrierPhase_i)- fraction = fromIntegral (phase ^. carrierPhase_f) / 256--assertExpectedBase :: SBPMsg -> (Double, Double, Double) -> Assertion-assertExpectedBase (SBPMsgBasePosEcef posEcef _) pos =- assertEqual "Base station position error!" pos $ basePosition posEcef-assertExpectedBase _ _ = assertFailure "Invalid message type!"--assertObsHeader :: SBPMsg -> ObservationHeader -> Assertion-assertObsHeader (SBPMsgObs obs _) header = do- assertEqual "Observation header is not equal" (header ^. observationHeader_n_obs) $ obs ^. msgObs_header ^. observationHeader_n_obs- assertEqual "Observation header is not equal" (header ^. observationHeader_t ^. gpsTimeNano_tow) $ obs ^. msgObs_header ^. observationHeader_t ^. gpsTimeNano_tow-assertObsHeader _ _ = assertFailure "Invalid message type!"--assertObs :: HashMap Word8 (Double, Double, Double)- -> PackedObsContent- -> Double -- ^ Pseudorange tolerance (meters)- -> Double -- ^ Carrier phase tolerance (cycles)- -> Word8 -- ^ Satellite band code- -> Assertion-assertObs truth obs ptol ctol sig = do- let prn = sat obs- def = (0, 0, 0)- (p, c, snr) = lookupDefault def prn truth- code = obs ^. packedObsContent_sid ^. gnssSignal16_code-#if MIN_VERSION_basic_prelude(0,6,1)- msg' = "PRN=" ++ show prn ++ " CODE=" ++ show code-#else- msg' = textToString $ "PRN=" ++ show prn ++ " CODE=" ++ show code-#endif- -- Pseudorange representation error- assertApproxEqual ("Incorrect pseudorange" ++ msg') ptol p $ pseudorange obs- -- Carrier phase representation error- assertApproxEqual ("Incorrect carrier phase" ++ msg') ctol c $ carrierPhase obs- -- SNRs/cn0 should be exact- assertEqual ("Incorrect SNR" ++ msg') snr $ cn0 obs- assertEqual ("Incorrect code" ++ msg') sig code--assertMsgObs :: SBPMsg -> Assertion-assertMsgObs (SBPMsgObs obs' _) = do- let ptol = 0.020 -- 2cm representation errorn- ctol_L1 = 0.005 -- carrier phase / L1 wavelength = 0.001m/0.190m cycles- ctol_L2 = 0.004 -- carrier phase / L2 wavelength = 0.001m/0.250m cycles- forM_ (obs' ^. msgObs_obs) $ \packed ->- case () of- _ | isL1 packed -> assertObs testObs_L1 packed ptol ctol_L1 0- | isL2 packed -> assertObs testObs_L2 packed ptol ctol_L2 6- | otherwise -> assertFailure "Not L1 or L2!"-assertMsgObs _ = assertFailure "Invalid message type!"--assertMsgObsLength :: SBPMsg -> Int -> Assertion-assertMsgObsLength (SBPMsgObs obs' _) len = length (obs' ^. msgObs_obs) @?= len-assertMsgObsLength _ _ = assertFailure "Invalid message type!"--assertMsgMaxLength :: SBPMsg -> Assertion-assertMsgMaxLength m = assertBool "The message is too damn long" $ LBS.length (encode m) <= 255 + 2---- | Given two objects and an accessor, assert that the common field is equal.-compareFieldsWithAccessor :: (Eq b, Show b) => String -> a -> a -> (a -> b) -> Assertion-compareFieldsWithAccessor m o1 o2 a = assertEqual ("Accessed field must be equal: " <> m) (a o1) (a o2)---- | Convert arbitrary SBP messages to a list of just GpsEphemeris messages.-messagesToGpsEphemeris :: [SBPMsg] -> [MsgEphemerisGps]-messagesToGpsEphemeris = mapMaybe $ \case- (SBPMsgEphemerisGps m _) -> Just m- _ -> Nothing---- | L1 observations: PRN => Pseudorange, Carrier Phase, SNR------ From fixtures/rinex/ucsf_bard_four_seconds.obs RINEX-testObs_L1 :: HashMap Word8 (Double, Double, Double)-testObs_L1 = fromList [ (5, (20982568.242, 110264218.428, 50.500))- , (31, (24390286.418, 128171817.525, 43.000))- , (25, (20560669.144, 108047101.226, 51.750))- , (12, (21678581.616, 113921748.286, 49.500))- , (29, (20679317.864, 108670633.460, 50.000))- , (21, (23719676.742, 124647833.324, 41.000))- , (2, (22612474.630, 118829414.562, 44.000))- , (20, (21815089.336, 114639133.794, 48.000))- ]---- | L2 observations: PRN => Pseudorange, Carrier Phase, SNR------ From fixtures/rinex/ucsf_bard_four_seconds.obs RINEX-testObs_L2 :: HashMap Word8 (Double, Double, Double)-testObs_L2 = fromList [ (5, (20982564.742, 85920194.806, 40.000))- , (31, (24390281.338, 99874145.710, 22.750))- , (25, (20560668.244, 84192554.659, 42.750))- , (12, (21678578.556, 88770214.224, 38.500))- , (29, (20679314.544, 84678442.345, 42.750))- , (21, (23719674.482, 97128180.775, 23.250))- , (2, (22612468.690, 92594355.238, 30.750))- , (20, (21815086.356, 89329221.086, 34.500))- ]--testMsg1004 :: TestTree-testMsg1004 =- testGroup "Msg1004 conversion to SBP"- [ testCase "Four seconds of RTCM3" $ do- msgs <- decodeRTCMFile 1906 "fixtures/rtcm3/ucsf_bard_four_seconds.rtcm3"- -- There are eight messages- length msgs @?= 8- -- Check message 0 and 5 against values in RINEX file:- -- fixtures/rinex/ucsf_bard_four_seconds.obs- let expectedBasePos = (-2709557.0660, -4260015.9160, 3884773.0630)- assertExpectedBase (msgs !! 0) expectedBasePos- assertExpectedBase (msgs !! 5) expectedBasePos- -- Message 1- assertObsHeader (msgs !! 1) ObservationHeader {- _observationHeader_t = GpsTimeNano 86354000 0 1906- , _observationHeader_n_obs = 32- }- assertMsgMaxLength (msgs !! 1)- assertMsgObsLength (msgs !! 1) 14- assertMsgObs (msgs !! 1)- -- Message 2- assertObsHeader (msgs !! 2) ObservationHeader {- _observationHeader_t = GpsTimeNano 86354000 0 1906- , _observationHeader_n_obs = 33- }- assertMsgObs (msgs !! 2)- assertMsgMaxLength (msgs !! 2)- assertMsgObsLength (msgs !! 2) 2- -- Message 3- assertObsHeader (msgs !! 3) ObservationHeader {- _observationHeader_t = GpsTimeNano 86355000 0 1906- , _observationHeader_n_obs = 32- }- assertMsgMaxLength (msgs !! 3)- assertMsgObsLength (msgs !! 3) 14- -- Message 4- assertObsHeader (msgs !! 4) ObservationHeader {- _observationHeader_t = GpsTimeNano 86355000 0 1906- , _observationHeader_n_obs = 33- }- assertMsgMaxLength (msgs !! 4)- assertMsgObsLength (msgs !! 4) 2- -- Message 6- assertObsHeader (msgs !! 6) ObservationHeader {- _observationHeader_t = GpsTimeNano 86356000 0 1906- , _observationHeader_n_obs = 32- }- assertMsgMaxLength (msgs !! 6)- assertMsgObsLength (msgs !! 6) 14- -- Message 7- assertObsHeader (msgs !! 7) ObservationHeader {- _observationHeader_t = GpsTimeNano 86356000 0 1906- , _observationHeader_n_obs = 33- }- assertMsgMaxLength (msgs !! 7)- assertMsgObsLength (msgs !! 7) 2- ]--testMsg1019 :: TestTree-testMsg1019 =- testGroup "Msg1019 conversion to SBP"- [ testCase "Full ephemeris in RTCM" $ do- msgs' <- decodeRTCMFile 1937 "fixtures/rtcm3/20170222_gps_ephemeris_331200.rtcm3"- let msgs = messagesToGpsEphemeris msgs'- BasicPrelude.mapM_ assertMsgMaxLength msgs'-- -- No values from this set of ephemerides were compared to Piksi.- -- Index 0, sat 4- assertBool "index 0, sat 4, toc" $ (msgs !! 0) ^. msgEphemerisGps_toc ^. gpsTimeSec_tow == 331200- assertBool "index 0, sat 4, toc" $ (msgs !! 0) ^. msgEphemerisGps_toc ^. gpsTimeSec_wn == 1937- assertBool "index 0, sat 4, sid" $ (msgs !! 0) ^. msgEphemerisGps_common ^. ephemerisCommonContent_sid ^. gnssSignal16_sat == 5- assertBool "index 0, sat 4, ura" $ (msgs !! 0) ^. msgEphemerisGps_common ^. ephemerisCommonContent_ura == 2- assertBool "index 0, sat 4, valid" $ (msgs !! 0) ^. msgEphemerisGps_common ^. ephemerisCommonContent_valid == 1- assertBool "index 0, sat 4, fitint" $ (msgs !! 0) ^. msgEphemerisGps_common ^. ephemerisCommonContent_fit_interval == 14400- assertBool "index 0, sat 4, health" $ (msgs !! 0) ^. msgEphemerisGps_common ^. ephemerisCommonContent_health_bits == 0- assertBool "index 0, sat 4, dn" $ (msgs !! 0) ^. msgEphemerisGps_dn == 4.8134147837606336e-9- assertBool "index 0, sat 4, w" $ (msgs !! 0) ^. msgEphemerisGps_w == 0.5556409483786487- assertBool "index 0, sat 4, tgd" $ (msgs !! 0) ^. msgEphemerisGps_tgd == -1.0710209608078003e-8- assertBool "index 0, sat 4, c_rs" $ (msgs !! 0) ^. msgEphemerisGps_c_rs == 11.0625- assertBool "index 0, sat 4, c_rc" $ (msgs !! 0) ^. msgEphemerisGps_c_rc == 195.6875- assertBool "index 0, sat 4, c_us" $ (msgs !! 0) ^. msgEphemerisGps_c_us == 9.134411811828613e-6- assertBool "index 0, sat 4, c_uc" $ (msgs !! 0) ^. msgEphemerisGps_c_uc == 7.040798664093018e-7- assertBool "index 0, sat 4, c_is" $ (msgs !! 0) ^. msgEphemerisGps_c_is == 2.0489096641540527e-8- assertBool "index 0, sat 4, c_ic" $ (msgs !! 0) ^. msgEphemerisGps_c_ic == 4.6566128730773926e-8- assertBool "index 0, sat 4, m0" $ (msgs !! 0) ^. msgEphemerisGps_m0 == -6.1410736472242544e-2- assertBool "index 0, sat 4, ecc" $ (msgs !! 0) ^. msgEphemerisGps_ecc == 4.897040314972401e-3- assertBool "index 0, sat 4, sqrta" $ (msgs !! 0) ^. msgEphemerisGps_sqrta == 5153.690675735474- assertBool "index 0, sat 4, omega0" $ (msgs !! 0) ^. msgEphemerisGps_omega0 == 0.26338009467812684- assertBool "index 0, sat 4, omegadot" $ (msgs !! 0) ^. msgEphemerisGps_omegadot == -8.164982961456692e-9- assertBool "index 0, sat 4, inc" $ (msgs !! 0) ^. msgEphemerisGps_inc == 0.9470684719233013- assertBool "index 0, sat 4, incdot" $ (msgs !! 0) ^. msgEphemerisGps_inc_dot == 5.146642949765581e-10- assertBool "index 0, sat 4, af0" $ (msgs !! 0) ^. msgEphemerisGps_af0 == -5.923490971326828e-5- assertBool "index 0, sat 4, af1" $ (msgs !! 0) ^. msgEphemerisGps_af1 == 2.0463630789890885e-12- assertBool "index 0, sat 4, af2" $ (msgs !! 0) ^. msgEphemerisGps_af2 == 0- assertBool "index 0, sat 4, iode" $ (msgs !! 0) ^. msgEphemerisGps_iode == 50- assertBool "index 0, sat 4, iodc" $ (msgs !! 0) ^. msgEphemerisGps_iodc == 50-- , testCase "Compare RTCM from ephemeris service and SBP messages from Piksi" $ do- -- Loop through all the decoded RTCM ephemerides, find the corresponding SBP- -- message, and compare fields.- -- Note that sender and CRC are different so we can't expect exact matches.- -- Note that RTCM will have all sats but SBP will only have a subset.- msgs2_converted' <- decodeRTCMFile 1937 "fixtures/rtcm3/20170222_gps_ephemeris_345600.rtcm3"- msgs2_sbp' <- decodeSBPFile "fixtures/sbp/20170222_gps_ephemeris_345600.sbp"- let msgs2_converted = messagesToGpsEphemeris msgs2_converted'- let msgs2_sbp = messagesToGpsEphemeris msgs2_sbp'+import BasicPrelude hiding (map)+import Control.Monad.Trans.Resource+import Data.Aeson+import Data.Aeson.Encode.Pretty+import Data.ByteString.Lazy hiding (ByteString, map)+import Data.Conduit+import Data.Conduit.Binary+import Data.Conduit.List+import Data.Conduit.Serialization.Binary+import Data.RTCM3.SBP+import Data.RTCM3.SBP.Types+import Test.Tasty+import Test.Tasty.Golden - flip BasicPrelude.mapM_ msgs2_converted $ \m' ->- let sid = m' ^. msgEphemerisGps_common ^. ephemerisCommonContent_sid ^. gnssSignal16_sat in- let toc = m' ^. msgEphemerisGps_toc ^. gpsTimeSec_tow in- let ms = filter ((== sid) . _gnssSignal16_sat . _ephemerisCommonContent_sid . _msgEphemerisGps_common) msgs2_sbp in- let ms' = filter ((== toc) . _gpsTimeSec_tow . _msgEphemerisGps_toc) ms in- if length ms' == 0 then return () else- let m = head ms' in do- compareFieldsWithAccessor "toc" m m' (_gpsTimeSec_tow . _msgEphemerisGps_toc)- compareFieldsWithAccessor "wn" m m' (_gpsTimeSec_wn . _msgEphemerisGps_toc)- compareFieldsWithAccessor "sid" m m' (_gnssSignal16_sat . _ephemerisCommonContent_sid . _msgEphemerisGps_common)- -- Does not always match the values coming out of a Piksi. I have reason to believe- -- that the Piksi might be the problem.- --compareFieldsWithAccessor "ura" m m' (_ephemerisCommonContent_ura . _msgEphemerisGps_common)- compareFieldsWithAccessor "valid" m m' (_ephemerisCommonContent_valid . _msgEphemerisGps_common)- compareFieldsWithAccessor "fitint" m m' (_ephemerisCommonContent_fit_interval . _msgEphemerisGps_common)- compareFieldsWithAccessor "health" m m' (_ephemerisCommonContent_health_bits . _msgEphemerisGps_common)- compareFieldsWithAccessor "dn" m m' (_msgEphemerisGps_dn)- compareFieldsWithAccessor "w" m m' (_msgEphemerisGps_w)- compareFieldsWithAccessor "tgd" m m' (_msgEphemerisGps_tgd)- compareFieldsWithAccessor "c_rs" m m' (_msgEphemerisGps_c_rs)- compareFieldsWithAccessor "c_rc" m m' (_msgEphemerisGps_c_rc)- compareFieldsWithAccessor "c_is" m m' (_msgEphemerisGps_c_is)- compareFieldsWithAccessor "c_ic" m m' (_msgEphemerisGps_c_ic)- compareFieldsWithAccessor "c_us" m m' (_msgEphemerisGps_c_us)- compareFieldsWithAccessor "c_uc" m m' (_msgEphemerisGps_c_uc)- compareFieldsWithAccessor "m0" m m' (_msgEphemerisGps_m0)- compareFieldsWithAccessor "ecc" m m' (_msgEphemerisGps_ecc)- compareFieldsWithAccessor "sqrta" m m' (_msgEphemerisGps_sqrta)- compareFieldsWithAccessor "omega0" m m' (_msgEphemerisGps_omega0)- compareFieldsWithAccessor "omega." m m' (_msgEphemerisGps_omegadot)- compareFieldsWithAccessor "inc" m m' (_msgEphemerisGps_inc)- compareFieldsWithAccessor "incdot" m m' (_msgEphemerisGps_inc_dot)- compareFieldsWithAccessor "af0" m m' (_msgEphemerisGps_af0)- compareFieldsWithAccessor "af1" m m' (_msgEphemerisGps_af1)- compareFieldsWithAccessor "af2" m m' (_msgEphemerisGps_af2)- compareFieldsWithAccessor "iode" m m' (_msgEphemerisGps_iode)- compareFieldsWithAccessor "iodc" m m' (_msgEphemerisGps_iodc)- ]+encodeLine :: ToJSON a => a -> ByteString+encodeLine v = toStrict $ encodePretty' defConfig { confCompare = compare } v <> "\n" -testToWn :: TestTree-testToWn =- testGroup "MJD to GPS week number"- [ testCase "Day 0, Week 0" $- toWn mjdEpoch @?= 0- , testCase "Day 4, Week 1906" $- toWn 57590 @?= 1906- , testCase "Day 0, Week 1906" $- toWn 57586 @?= 1906- , testCase "Day 7, Week 1905" $- toWn 57585 @?= 1905- , testCase "Day 7, Week 1906" $- toWn 57592 @?= 1906- , testCase "Day 0, Week 1907" $- toWn 57593 @?= 1907- ]+runConverter :: FilePath -> FilePath -> IO ()+runConverter f t = do+ s <- newStore+ runResourceT $ runConvertT s $+ sourceFile f =$=+ conduitDecode =$=+ awaitForever converter =$=+ map encodeLine $$+ sinkFile t -testValidateIodcIode :: TestTree-testValidateIodcIode =- testGroup "Validate IODC/IODE"- [ testCase "valid, equal, max" $ assertEqual "" 1 $ validateIodcIode 0x00FF 0xFF- , testCase "valid, equal, min" $ assertEqual "" 1 $ validateIodcIode 0x0000 0x00- , testCase "valid, IODC has higher bits set, max" $ assertEqual "" 1 $ validateIodcIode 0xFFFF 0xFF- , testCase "valid, IODC has higher bits set, min" $ assertEqual "" 1 $ validateIodcIode 0xFF00 0x00- , testCase "invalid, unequal" $ assertEqual "" 0 $ validateIodcIode 0x0001 0x02- , testCase "invalid, IODC has higher bits set" $ assertEqual "" 0 $ validateIodcIode 0xFF14 0x15- ]+goldenFileTest :: TestName -> FilePath -> FilePath -> TestTree+goldenFileTest name s d = do+ let n = ("test/golden" </>)+ f = "from_" <> d+ t = "to_" <> d+ goldenVsFile name (n f) (n t) $ runConverter (n s) (n t) -testUriToUra :: TestTree-testUriToUra =- testGroup "Convert user range index to user range accuracy"- [ testN 0 2.0- , testN 1 2.8- , testN 2 4.0- , testN 3 5.7- , testN 4 8.0- , testN 5 11.3- , testN 6 16.0- , testN 7 32.0- , testN 8 64.0- , testN 9 128.0- , testN 10 256.0- , testN 11 512.0- , testN 12 1024.0- , testN 13 2048.0- , testN 14 4096.0- , testN 15 6144.0- , testN 16 (-1.0)- , testN 17 (-1.0)- , testN (-1) (-1.0)+testObservations :: TestTree+testObservations =+ testGroup "Observations tests"+ [ goldenFileTest "glo-day-rollover" "glo_day_rollover.rtcm" "glo_day_rollover.json" ] - where- testN n expected =-#if MIN_VERSION_basic_prelude(0,6,1)- let s = show n in-#else- let s = textToString $ show n in-#endif- testCase s $ assertEqual s expected $ gpsUriToUra n- tests :: TestTree tests =- testGroup "RTCM3 to SBP conversion tests"- [ testMsg1004- , testMsg1019- , testToWn- , testValidateIodcIode- , testUriToUra+ testGroup "RTCM3 to SBP tests"+ [ testObservations ]