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.2.8 +version: 0.2.9 synopsis: GNSS Converters. description: Haskell bindings for GNSS converters. homepage: http://github.com/swift-nav/gnss-converters 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 @@ -19,7 +19,7 @@ , q32Width , toWn , mjdEpoch - , updateGpsTime + , updateGpsTimeNano , convert , newStore , validateIodcIode @@ -191,31 +191,31 @@ -- | If incoming TOW is less than stored TOW, rollover WN. -- -updateGpsTime :: Word32 -> GpsTimeNano -> GpsTimeNano -updateGpsTime tow gpsTime = +updateGpsTimeNano :: Word32 -> GpsTimeNano -> GpsTimeNano +updateGpsTimeNano tow gpsTime = gpsTime & gpsTimeNano_tow .~ tow & if tow >= gpsTime ^. gpsTimeNano_tow then id else gpsTimeNano_wn %~ (+ 1) -- | Produce GPS Time from Observation header, handling WN rollover. -- -toGpsTime :: MonadStore e m => GpsObservationHeader -> m GpsTimeNano -toGpsTime hdr = do +toGpsTimeNano :: MonadStore e m => GpsObservationHeader -> m GpsTimeNano +toGpsTimeNano hdr = do let tow = hdr ^. gpsObservationHeader_tow station = hdr ^. gpsObservationHeader_station gpsTime <- newGpsTime tow gpsTimeMap <- view storeGpsTimeMap - liftIO $ modifyMap gpsTimeMap gpsTime station $ updateGpsTime tow + liftIO $ modifyMap gpsTimeMap gpsTime station $ updateGpsTimeNano tow -- | 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. -rtcmTimeToGpsTime :: MonadStore e m => Word16 -> Word16 -> m GpsTime -rtcmTimeToGpsTime wn tow = do +toGpsTimeSec :: MonadStore e m => Word16 -> Word16 -> m GpsTimeSec +toGpsTimeSec wn tow = do stateWn <- view storeWn >>= liftIO . readIORef - return $ GpsTime - { _gpsTime_tow = 16 * fromIntegral tow - , _gpsTime_wn = stateWn `shiftR` 10 `shiftL` 10 + wn + 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 @@ -358,7 +358,7 @@ -> GpsObservationHeader -- ^ RTCM observation header -> m ObservationHeader fromGpsObservationHeader totalMsgs n hdr = do - t <- toGpsTime hdr + t <- toGpsTimeNano hdr return ObservationHeader { _observationHeader_t = t -- First nibble is the size of the sequence (n), second nibble is the @@ -448,7 +448,7 @@ -- | 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) + toe <- toGpsTimeSec (m ^. msg1019_ephemeris ^. gpsEphemeris_wn) (m ^. msg1019_ephemeris ^. gpsEphemeris_toe) return EphemerisCommonContent { _ephemerisCommonContent_sid = GnssSignal16 { _gnssSignal16_sat = m ^. msg1019_header ^. gpsEphemerisHeader_sat @@ -599,7 +599,7 @@ 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) + toc <- toGpsTimeSec (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 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 @@ -235,8 +235,8 @@ -- 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, 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 @@ -276,13 +276,13 @@ flip BasicPrelude.mapM_ msgs2_converted $ \m' -> let sid = m' ^. msgEphemerisGps_common ^. ephemerisCommonContent_sid ^. gnssSignal16_sat in - let toc = m' ^. msgEphemerisGps_toc ^. gpsTime_tow 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) . _gpsTime_tow . _msgEphemerisGps_toc) ms 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' (_gpsTime_tow . _msgEphemerisGps_toc) - compareFieldsWithAccessor "wn" m m' (_gpsTime_wn . _msgEphemerisGps_toc) + 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. @@ -330,20 +330,20 @@ toWn 57593 @?= 1907 ] -testUpdateGpsTime :: TestTree -testUpdateGpsTime = +testUpdateGpsTimeNano :: TestTree +testUpdateGpsTimeNano = testGroup "Update GPS Time" [ testCase "old TOW < new TOW" $ do let old = GpsTimeNano 1 0 10 - new = updateGpsTime 2 old + new = updateGpsTimeNano 2 old new ^. gpsTimeNano_wn @?= 10 , testCase "old TOW = new TOW" $ do let old = GpsTimeNano 1 0 10 - new = updateGpsTime 1 old + new = updateGpsTimeNano 1 old new ^. gpsTimeNano_wn @?= 10 , testCase "old TOW > new TOW" $ do let old = GpsTimeNano 1 0 10 - new = updateGpsTime 0 old + new = updateGpsTimeNano 0 old new ^. gpsTimeNano_wn @?= 11 ] @@ -393,7 +393,7 @@ [ testMsg1004 , testMsg1019 , testToWn - , testUpdateGpsTime + , testUpdateGpsTimeNano , testValidateIodcIode , testUriToUra ]