{-# LANGUAGE FlexibleContexts #-} -- | -- Module: Test.Data.RTCM3.SBP -- Copyright: (c) 2016 Swift Navigation Inc. -- License: LGPL-3 -- Maintainer: Skylark Team -- -- Test RTCMv3 to SBP Conversions. module Test.Data.RTCM3.SBP ( tests ) where import BasicPrelude import Control.Lens import Control.Monad.Trans.Resource import Data.Conduit import Data.Conduit.Binary import qualified Data.Conduit.List as CL import Data.Conduit.Serialization.Binary import Data.HashMap.Strict import Data.IORef import Data.RTCM3.SBP import Data.RTCM3.SBP.Types import Data.Word 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 <*> newIORef mempty runResourceT $ runConvertT s $ runConduit $ sourceFile filename =$= conduitDecode =$= CL.concatMapM convert $$ 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 ^. gnssSignal_code) == l1CSidCode isL2 :: PackedObsContent -> Bool isL2 obs = code == l2CMSidCode || code == l2PSidCode where code = obs ^. packedObsContent_sid ^. gnssSignal_code sat :: PackedObsContent -> Word16 sat obs = obs ^. packedObsContent_sid ^. gnssSignal_sat cn0 :: PackedObsContent -> Double cn0 obs = 0.25 * fromIntegral (obs ^. packedObsContent_cn0) pseudorange :: PackedObsContent -> Double pseudorange obs = fromIntegral (obs ^. packedObsContent_P) / sbpCMinM carrierPhase :: PackedObsContent -> Double carrierPhase obs = whole + fraction where phase = obs ^. packedObsContent_L whole = fromIntegral (phase ^. carrierPhase_i) fraction = fromIntegral (phase ^. carrierPhase_f) / q32Width 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 = assertEqual "Observation header is not equal" header $ obs ^. msgObs_header assertObsHeader _ _ = assertFailure "Invalid message type!" assertObs :: HashMap Word16 (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 = 1 + sat obs def = (0, 0, 0) (p, c, snr) = lookupDefault def prn truth code = obs ^. packedObsContent_sid ^. gnssSignal_code msg' = textToString $ "PRN=" ++ show prn ++ " CODE=" ++ show code -- 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 l1CSidCode | isL2 packed -> assertObs testObs_L2 packed ptol ctol_L2 l2PSidCode | 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!" -- | L1 observations: PRN => Pseudorange, Carrier Phase, SNR -- -- From fixtures/rinex/ucsf_bard_four_seconds.obs RINEX testObs_L1 :: HashMap Word16 (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 Word16 (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 "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 = ObsGPSTime 86354000 1906 , _observationHeader_n_obs = 32 } assertMsgObsLength (msgs !! 1) 15 assertMsgObs (msgs !! 1) -- Message 2 assertObsHeader (msgs !! 2) ObservationHeader { _observationHeader_t = ObsGPSTime 86354000 1906 , _observationHeader_n_obs = 33 } assertMsgObs (msgs !! 2) assertMsgObsLength (msgs !! 2) 1 -- Message 3 assertObsHeader (msgs !! 3) ObservationHeader { _observationHeader_t = ObsGPSTime 86355000 1906 , _observationHeader_n_obs = 32 } assertMsgObsLength (msgs !! 3) 15 -- Message 4 assertObsHeader (msgs !! 4) ObservationHeader { _observationHeader_t = ObsGPSTime 86355000 1906 , _observationHeader_n_obs = 33 } assertMsgObsLength (msgs !! 4) 1 -- Message 6 assertObsHeader (msgs !! 6) ObservationHeader { _observationHeader_t = ObsGPSTime 86356000 1906 , _observationHeader_n_obs = 32 } assertMsgObsLength (msgs !! 6) 15 -- Message 7 assertObsHeader (msgs !! 7) ObservationHeader { _observationHeader_t = ObsGPSTime 86356000 1906 , _observationHeader_n_obs = 33 } assertMsgObsLength (msgs !! 7) 1 ] 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 ] testUpdateGPSTime :: TestTree testUpdateGPSTime = testGroup "Update GPS Time" [ testCase "old TOW < new TOW" $ do let old = ObsGPSTime 1 10 new = updateGPSTime 2 old new ^. obsGPSTime_wn @?= 10 , testCase "old TOW = new TOW" $ do let old = ObsGPSTime 1 10 new = updateGPSTime 1 old new ^. obsGPSTime_wn @?= 10 , testCase "old TOW > new TOW" $ do let old = ObsGPSTime 1 10 new = updateGPSTime 0 old new ^. obsGPSTime_wn @?= 11 ] testUpdateLock :: TestTree testUpdateLock = testGroup "Update Lock" [ testCase "old time < new time" $ do let old = Lock 1 10 new = updateLock 2 old new ^. lockCounter @?= 10 , testCase "old time = new time" $ do let old = Lock 1 10 new = updateLock 1 old new ^. lockCounter @?= 10 , testCase "old time > new time" $ do let old = Lock 1 10 new = updateLock 0 old new ^. lockCounter @?= 11 ] tests :: TestTree tests = testGroup "RTCM3 to SBP conversion tests" [ testMsg1004 , testToWn , testUpdateGPSTime , testUpdateLock ]