diff --git a/gnss-converters.cabal b/gnss-converters.cabal --- a/gnss-converters.cabal +++ b/gnss-converters.cabal @@ -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 diff --git a/main/RTCM32SBP.hs b/main/RTCM32SBP.hs --- a/main/RTCM32SBP.hs +++ b/main/RTCM32SBP.hs @@ -23,8 +23,8 @@ main = do s <- newStore runResourceT $ runConvertT s $ - sourceHandle stdin =$= - conduitDecode =$= - converter =$= - conduitEncode $$ + sourceHandle stdin =$= + conduitDecode =$= + awaitForever converter =$= + conduitEncode $$ sinkHandle stdout diff --git a/main/SBP2RTCM3.hs b/main/SBP2RTCM3.hs --- a/main/SBP2RTCM3.hs +++ b/main/SBP2RTCM3.hs @@ -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 diff --git a/src/Data/RTCM3/SBP.hs b/src/Data/RTCM3/SBP.hs --- a/src/Data/RTCM3/SBP.hs +++ b/src/Data/RTCM3/SBP.hs @@ -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 diff --git a/src/Data/RTCM3/SBP/Observations.hs b/src/Data/RTCM3/SBP/Observations.hs --- a/src/Data/RTCM3/SBP/Observations.hs +++ b/src/Data/RTCM3/SBP/Observations.hs @@ -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 diff --git a/src/Data/RTCM3/SBP/Positions.hs b/src/Data/RTCM3/SBP/Positions.hs new file mode 100644 --- /dev/null +++ b/src/Data/RTCM3/SBP/Positions.hs @@ -0,0 +1,56 @@ +{-# LANGUAGE NoImplicitPrelude #-} + +-- | +-- Module: Data.RTCM3.SBP.Positions +-- Copyright: Copyright (C) 2016 Swift Navigation, Inc. +-- License: LGPL-3 +-- Maintainer: Swift Navigation +-- 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] diff --git a/src/Data/RTCM3/SBP/Types.hs b/src/Data/RTCM3/SBP/Types.hs --- a/src/Data/RTCM3/SBP/Types.hs +++ b/src/Data/RTCM3/SBP/Types.hs @@ -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 ) diff --git a/src/SwiftNav/SBP/RTCM3.hs b/src/SwiftNav/SBP/RTCM3.hs --- a/src/SwiftNav/SBP/RTCM3.hs +++ b/src/SwiftNav/SBP/RTCM3.hs @@ -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 diff --git a/test/Test/Data/RTCM3/SBP.hs b/test/Test/Data/RTCM3/SBP.hs --- a/test/Test/Data/RTCM3/SBP.hs +++ b/test/Test/Data/RTCM3/SBP.hs @@ -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 --- --- 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 ]