diff --git a/gnss-converters.cabal b/gnss-converters.cabal --- a/gnss-converters.cabal +++ b/gnss-converters.cabal @@ -1,12 +1,12 @@ name: gnss-converters -version: 0.2.1 +version: 0.2.2 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) 2016 Swift Navigation, Inc. +author: Mark Fine , Joshua Gross +maintainer: Swift Navigation +copyright: Copyright (C) 2016, 2017 Swift Navigation, Inc. category: Network build-type: Simple cabal-version: >= 1.10 @@ -73,6 +73,7 @@ build-depends: HUnit-approx , base , basic-prelude + , binary , binary-conduit , bytestring , conduit @@ -84,6 +85,7 @@ , sbp , tasty , tasty-hunit + , text , unordered-containers ghc-options: -threaded -rtsopts -with-rtsopts=-N -Wall default-language: Haskell2010 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 @@ -4,7 +4,7 @@ -- Module: Data.RTCM3.SBP -- Copyright: Copyright (C) 2016 Swift Navigation, Inc. -- License: LGPL-3 --- Maintainer: Mark Fine +-- Maintainer: Swift Navigation -- Stability: experimental -- Portability: portable -- @@ -21,6 +21,8 @@ , updateGpsTime , convert , newStore + , validateIodcIode + , gpsUriToUra ) where import BasicPrelude @@ -147,6 +149,10 @@ headerSize = 11 packedObsSize = 17 +-- | 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 @@ -163,6 +169,8 @@ maybe' :: Maybe a -> b -> (a -> b) -> b maybe' m b a = maybe b a m +applyScaleFactor :: (Floating a, Integral b) => a -> a -> b -> a +applyScaleFactor n p x = (n ** p) * fromIntegral x -------------------------------------------------------------------------------- -- GNSS RTCM observation reconstruction utilities @@ -198,6 +206,27 @@ gpsTimeMap <- view storeGpsTimeMap liftIO $ modifyMap gpsTimeMap gpsTime station $ updateGpsTime tow +-- | Produce GPS time from RTCM time (week-number % 1024, tow in seconds, scaled 2^4). +-- Stateful but non-side-effecting. +rtcmTimeToGpsTime :: MonadStore e m => Word16 -> Word16 -> m GpsTime +rtcmTimeToGpsTime wn tow = do + stateWn <- view storeWn >>= liftIO . readIORef + let wn' = reconcileWn stateWn wn + return $ GpsTime + { _gpsTime_tow = 16 * fromIntegral tow + , _gpsTime_wn = wn' + } + +-- | Reconcile a "stateful", current (system?) week number with a +-- mod-1024 week number from RTCM. The idea here is that an RTCM message +-- will say the week number is 913 when the current week number is actually +-- 1937. +reconcileWn :: Word16 -> Word16 -> Word16 +reconcileWn curr truncated = + if (truncated + 1024) <= curr + then reconcileWn curr (truncated + 1024) + else truncated + -- | MJD GPS Epoch - First day in GPS week 0. See DF051 of the RTCM3 spec -- mjdEpoch :: Word16 @@ -413,6 +442,33 @@ -- pseudorange valid } +-- | 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 <- rtcmTimeToGpsTime (m ^. msg1019_ephemeris ^. gpsEphemeris_wn) (m ^. msg1019_ephemeris ^. gpsEphemeris_toe) + return EphemerisCommonContent + { _ephemerisCommonContent_sid = GnssSignal + { _gnssSignal_sat = fromIntegral $ m ^. msg1019_header ^. ephemerisHeader_sat - 1 + -- ^ SBP GnssSignal sat ID is off-by-one; GnssSignal16 is not. + , _gnssSignal_code = 0 -- there is an L2P status flag in msg 1019, but I don't think that applies + , _gnssSignal_reserved = 0 + } + , _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 + } + -- | Construct SBP GPS observation message (possibly chunked). -- chunkToMsgObs :: MonadStore e m @@ -433,7 +489,34 @@ 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 = + if not fitInt then 4 * 60 * 60 else + if iodc >= 240 && iodc <= 247 then 8 * 60 * 60 else + if (iodc >= 248 && iodc <= 255) || iodc == 496 then 14 * 60 * 60 else + if (iodc >= 497 && iodc <= 503) || (iodc >= 1021 && iodc <= 1023) then 26 * 60 * 60 else + if iodc >= 504 && iodc <= 510 then 50 * 60 * 60 else + if iodc == 511 || (iodc >= 752 && iodc <= 756) then 74 * 60 * 60 else + if iodc == 757 then 98 * 60 * 60 else + 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 = + if uri < 0 then -1 else + if uri == 1 then 2.8 else + if uri == 3 then 5.7 else + if uri == 5 then 11.3 else + if uri == 15 then 6144 else + if uri <= 6 then 2 ** (1 + (uri / 2)) else + if uri > 6 && uri < 15 then 2 ** (uri - 2) else + -1 + -------------------------------------------------------------------------------- -- RTCM to SBP conversion utilities: RTCM Msgs. 1002 (L1 RTK), 1004 (L1+L2 RTK), -- 1005 (antenna position), 1006 (antenna position). @@ -520,6 +603,37 @@ , _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 <- rtcmTimeToGpsTime (m ^. msg1019_ephemeris ^. gpsEphemeris_wn) (m ^. msg1019_ephemeris ^. gpsEphemeris_toc) + return MsgEphemerisGps + { _msgEphemerisGps_common = commonContent + , _msgEphemerisGps_tgd = applyScaleFactor 2 (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_tgd + , _msgEphemerisGps_c_rs = applyScaleFactor 2 (-5) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_rs + , _msgEphemerisGps_c_rc = applyScaleFactor 2 (-5) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_rc + , _msgEphemerisGps_c_uc = applyScaleFactor 2 (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_uc + , _msgEphemerisGps_c_us = applyScaleFactor 2 (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_us + , _msgEphemerisGps_c_ic = applyScaleFactor 2 (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_ic + , _msgEphemerisGps_c_is = applyScaleFactor 2 (-29) $ m ^. msg1019_ephemeris ^. gpsEphemeris_c_is + , _msgEphemerisGps_dn = gpsPi * (applyScaleFactor 2 (-43) $ m ^. msg1019_ephemeris ^. gpsEphemeris_dn) + , _msgEphemerisGps_m0 = gpsPi * (applyScaleFactor 2 (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_m0) + , _msgEphemerisGps_ecc = applyScaleFactor 2 (-33) $ m ^. msg1019_ephemeris ^. gpsEphemeris_ecc + , _msgEphemerisGps_sqrta = applyScaleFactor 2 (-19) $ m ^. msg1019_ephemeris ^. gpsEphemeris_sqrta + , _msgEphemerisGps_omega0 = gpsPi * (applyScaleFactor 2 (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_omega0) + , _msgEphemerisGps_omegadot = gpsPi * (applyScaleFactor 2 (-43) $ m ^. msg1019_ephemeris ^. gpsEphemeris_omegadot) + , _msgEphemerisGps_w = gpsPi * (applyScaleFactor 2 (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_w) + , _msgEphemerisGps_inc = gpsPi * (applyScaleFactor 2 (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_i0) + , _msgEphemerisGps_inc_dot = gpsPi * (applyScaleFactor 2 (-43) $ m ^. msg1019_ephemeris ^. gpsEphemeris_idot) + , _msgEphemerisGps_af0 = applyScaleFactor 2 (-31) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af0 + , _msgEphemerisGps_af1 = applyScaleFactor 2 (-43) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af1 + , _msgEphemerisGps_af2 = applyScaleFactor 2 (-55) $ m ^. msg1019_ephemeris ^. gpsEphemeris_af2 + , _msgEphemerisGps_iodc = m ^. msg1019_ephemeris ^. gpsEphemeris_iodc + , _msgEphemerisGps_iode = m ^. msg1019_ephemeris ^. gpsEphemeris_iode + , _msgEphemerisGps_toc = toc + } + -- | Convert an RTCM message into possibly multiple SBP messages. -- convert :: MonadStore e m => RTCM3Msg -> m [SBPMsg] @@ -544,6 +658,10 @@ wn <- view storeWn liftIO $ writeIORef wn $ toWn $ m ^. msg1013_header ^. messageHeader_mjd return mempty + (RTCM3Msg1019 m _rtcm3) -> do + let sender = 0 + m' <- fromMsg1019 m + return [SBPMsgEphemerisGps m' $ toSBP m' $ toSender sender] _rtcm3Msg -> return mempty newStore :: IO Store 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 @@ -11,7 +11,7 @@ -- Module: Data.RTCM3.SBP.Types -- Copyright: Copyright (C) 2015 Swift Navigation, Inc. -- License: LGPL-3 --- Maintainer: Mark Fine +-- Maintainer: Swift Navigation -- Stability: experimental -- Portability: portable -- 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 @@ -4,7 +4,7 @@ -- Module: SwiftNav.SBP.RTCM3 -- Copyright: Copyright (C) 2015 Swift Navigation, Inc. -- License: LGPL-3 --- Maintainer: Mark Fine +-- Maintainer: Swift Navigation -- Stability: experimental -- Portability: portable -- diff --git a/test/Test.hs b/test/Test.hs --- a/test/Test.hs +++ b/test/Test.hs @@ -2,7 +2,7 @@ -- Module: Test -- Copyright: (c) 2016 Swift Navigation Inc. -- License: AllRightsReserved --- Maintainer: Skylark Team +-- Maintainer: Swift Navigation -- -- Test module for GNSS converters 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,10 +1,11 @@ {-# LANGUAGE FlexibleContexts #-} +{-# LANGUAGE LambdaCase #-} -- | -- Module: Test.Data.RTCM3.SBP -- Copyright: (c) 2016 Swift Navigation Inc. -- License: LGPL-3 --- Maintainer: Skylark Team +-- Maintainer: Swift Navigation -- -- Test RTCMv3 to SBP Conversions. @@ -15,28 +16,38 @@ 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 +import Data.Conduit.Binary hiding (head) import qualified Data.Conduit.List as CL import Data.Conduit.Serialization.Binary -import Data.HashMap.Strict +import Data.HashMap.Strict hiding (filter, mapMaybe) import Data.IORef import Data.RTCM3.SBP import Data.RTCM3.SBP.Types +import qualified Data.Text as T import SwiftNav.SBP import Test.HUnit.Approx import Test.Tasty import Test.Tasty.HUnit -decodeRTCMFile :: FilePath -> IO [SBPMsg] -decodeRTCMFile filename = do - s <- Store <$> newIORef 1906 <*> newIORef mempty +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 @@ -113,6 +124,19 @@ 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 @@ -145,7 +169,7 @@ testMsg1004 = testGroup "Msg1004 conversion to SBP" [ testCase "Four seconds of RTCM3" $ do - msgs <- decodeRTCMFile "fixtures/rtcm3/ucsf_bard_four_seconds.rtcm3" + 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: @@ -158,7 +182,8 @@ _observationHeader_t = GpsTimeNano 86354000 0 1906 , _observationHeader_n_obs = 32 } - assertMsgObsLength (msgs !! 1) 15 + assertMsgMaxLength (msgs !! 1) + assertMsgObsLength (msgs !! 1) 14 assertMsgObs (msgs !! 1) -- Message 2 assertObsHeader (msgs !! 2) ObservationHeader { @@ -166,33 +191,126 @@ , _observationHeader_n_obs = 33 } assertMsgObs (msgs !! 2) - assertMsgObsLength (msgs !! 2) 1 + assertMsgMaxLength (msgs !! 2) + assertMsgObsLength (msgs !! 2) 2 -- Message 3 assertObsHeader (msgs !! 3) ObservationHeader { _observationHeader_t = GpsTimeNano 86355000 0 1906 , _observationHeader_n_obs = 32 } - assertMsgObsLength (msgs !! 3) 15 + assertMsgMaxLength (msgs !! 3) + assertMsgObsLength (msgs !! 3) 14 -- Message 4 assertObsHeader (msgs !! 4) ObservationHeader { _observationHeader_t = GpsTimeNano 86355000 0 1906 , _observationHeader_n_obs = 33 } - assertMsgObsLength (msgs !! 4) 1 + assertMsgMaxLength (msgs !! 4) + assertMsgObsLength (msgs !! 4) 2 -- Message 6 assertObsHeader (msgs !! 6) ObservationHeader { _observationHeader_t = GpsTimeNano 86356000 0 1906 , _observationHeader_n_obs = 32 } - assertMsgObsLength (msgs !! 6) 15 + assertMsgMaxLength (msgs !! 6) + assertMsgObsLength (msgs !! 6) 14 -- Message 7 assertObsHeader (msgs !! 7) ObservationHeader { _observationHeader_t = GpsTimeNano 86356000 0 1906 , _observationHeader_n_obs = 33 } - assertMsgObsLength (msgs !! 7) 1 + 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 ^. gpsTime_tow == 331200 + assertBool "index 0, sat 4, toc" $ (msgs !! 0) ^. msgEphemerisGps_toc ^. gpsTime_wn == 1937 + assertBool "index 0, sat 4, sid" $ (msgs !! 0) ^. msgEphemerisGps_common ^. ephemerisCommonContent_sid ^. gnssSignal_sat == 4 + 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' + + flip BasicPrelude.mapM_ msgs2_converted $ \m' -> + let sid = m' ^. msgEphemerisGps_common ^. ephemerisCommonContent_sid ^. gnssSignal_sat in + let toc = m' ^. msgEphemerisGps_toc ^. gpsTime_tow in + let ms = filter ((== sid) . _gnssSignal_sat . _ephemerisCommonContent_sid . _msgEphemerisGps_common) msgs2_sbp in + let ms' = filter ((== toc) . _gpsTime_tow . _msgEphemerisGps_toc) ms in + if length ms' == 0 then return () else + let m = head ms' in do + compareFieldsWithAccessor "toc" m m' (_gpsTime_tow . _msgEphemerisGps_toc) + compareFieldsWithAccessor "wn" m m' (_gpsTime_wn . _msgEphemerisGps_toc) + compareFieldsWithAccessor "sid" m m' (_gnssSignal_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) + ] + testToWn :: TestTree testToWn = testGroup "MJD to GPS week number" @@ -227,10 +345,53 @@ new ^. gpsTimeNano_wn @?= 11 ] +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 + ] + +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) + ] + + where + testN n expected = + let s = T.unpack $ show n in + testCase s $ assertEqual s expected $ gpsUriToUra n + tests :: TestTree tests = testGroup "RTCM3 to SBP conversion tests" [ testMsg1004 + , testMsg1019 , testToWn , testUpdateGpsTime + , testValidateIodcIode + , testUriToUra ]