gnss-converters-0.1.10: test/Test/Data/RTCM3/SBP.hs
{-# LANGUAGE FlexibleContexts #-}
{-# LANGUAGE LambdaCase #-}
{-# LANGUAGE NoImplicitPrelude #-}
{-# LANGUAGE OverloadedStrings #-}
{-# LANGUAGE RecordWildCards #-}
-- |
-- Module: Test.Data.RTCM3.SBP
-- Copyright: (c) 2016 Swift Navigation Inc.
-- License: LGPL-3
-- Maintainer: Skylark Team <skylark@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.Conduit
import Data.Conduit.Binary
import Data.Conduit.Serialization.Binary
import Data.HashMap.Strict
import Data.RTCM3.SBP
import qualified Data.Conduit.List as CL
import Data.Word
import SwiftNav.SBP
import Test.HUnit.Approx
import Test.Tasty
import Test.Tasty.HUnit
decodeRTCMFile :: (MonadIO m, MonadBaseControl IO m, MonadThrow m)
=> FilePath
-> m [SBPMsg]
decodeRTCMFile filename =
runResourceT . runConduit $
sourceFile filename =$=
conduitDecode =$=
CL.mapMaybeM convert =$=
CL.concat $$
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 1905
, _observationHeader_n_obs = 32
}
assertMsgObsLength (msgs !! 1) 15
assertMsgObs (msgs !! 1)
-- Message 2
assertObsHeader (msgs !! 2) ObservationHeader {
_observationHeader_t = ObsGPSTime 86354000 1905
, _observationHeader_n_obs = 33
}
assertMsgObs (msgs !! 2)
assertMsgObsLength (msgs !! 2) 1
-- Message 3
assertObsHeader (msgs !! 3) ObservationHeader {
_observationHeader_t = ObsGPSTime 86355000 1905
, _observationHeader_n_obs = 32
}
assertMsgObsLength (msgs !! 3) 15
-- Message 4
assertObsHeader (msgs !! 4) ObservationHeader {
_observationHeader_t = ObsGPSTime 86355000 1905
, _observationHeader_n_obs = 33
}
assertMsgObsLength (msgs !! 4) 1
-- Message 6
assertObsHeader (msgs !! 6) ObservationHeader {
_observationHeader_t = ObsGPSTime 86356000 1905
, _observationHeader_n_obs = 32
}
assertMsgObsLength (msgs !! 6) 15
-- Message 7
assertObsHeader (msgs !! 7) ObservationHeader {
_observationHeader_t = ObsGPSTime 86356000 1905
, _observationHeader_n_obs = 33
}
assertMsgObsLength (msgs !! 7) 1
]
tests :: TestTree
tests =
testGroup "RTCM3 to SBP conversion tests"
[ testMsg1004
]