diff --git a/gnss-converters.cabal b/gnss-converters.cabal --- a/gnss-converters.cabal +++ b/gnss-converters.cabal @@ -1,6 +1,6 @@ cabal-version: >=1.22 name: gnss-converters -version: 0.3.39 +version: 0.3.40 license: BSD3 copyright: Copyright (C) 2016, 2017 Swift Navigation, Inc. maintainer: Swift Navigation diff --git a/src/Data/RTCM3/SBP/Biases.hs b/src/Data/RTCM3/SBP/Biases.hs --- a/src/Data/RTCM3/SBP/Biases.hs +++ b/src/Data/RTCM3/SBP/Biases.hs @@ -28,7 +28,7 @@ -- | Derive sender from station. -- toSender :: Word16 -> Word16 -toSender station = station .|. 61440 +toSender station = station .|. 61568 -- | Expand out array of biases. -- diff --git a/src/Data/RTCM3/SBP/Ephemerides.hs b/src/Data/RTCM3/SBP/Ephemerides.hs --- a/src/Data/RTCM3/SBP/Ephemerides.hs +++ b/src/Data/RTCM3/SBP/Ephemerides.hs @@ -196,7 +196,7 @@ , _msgEphemerisGps_iode = m ^. msg1019_ephemeris . gpsEphemeris_iode , _msgEphemerisGps_toc = toc } - yield [SBPMsgEphemerisGps m' $ toSBP m' 61440] + yield [SBPMsgEphemerisGps m' $ toSBP m' 61568] -- | Convert an RTCM 1020 GLONASS ephemeris message into an SBP MsgEphemerisGlo. -- @@ -226,4 +226,4 @@ , _msgEphemerisGlo_fcn = (m ^. msg1020_header . glonassEphemerisHeader_channel) + 1 , _msgEphemerisGlo_iod = ((m ^. msg1020_ephemeris . glonassEphemeris_tb) * 15 * 60) .&. 127 } - yield [SBPMsgEphemerisGlo m' $ toSBP m' 61440] + yield [SBPMsgEphemerisGlo m' $ toSBP m' 61568] diff --git a/src/Data/RTCM3/SBP/Logging.hs b/src/Data/RTCM3/SBP/Logging.hs --- a/src/Data/RTCM3/SBP/Logging.hs +++ b/src/Data/RTCM3/SBP/Logging.hs @@ -27,7 +27,7 @@ -- | Derive sender from station. -- toSender :: Word16 -> Word16 -toSender station = station .|. 61440 +toSender station = station .|. 61568 -- | Convert an RTCM 1029 text message into an SBP MsgLog message. -- diff --git a/src/Data/RTCM3/SBP/MSM.hs b/src/Data/RTCM3/SBP/MSM.hs --- a/src/Data/RTCM3/SBP/MSM.hs +++ b/src/Data/RTCM3/SBP/MSM.hs @@ -46,9 +46,15 @@ sig <- mask (hdr ^. msmHeader_signalMask) pure (sat, sig) +-- | Max GPS satellite number. +-- +gpsMaxSat :: Word8 +gpsMaxSat = 32 + toGpsSat :: Word8 -> Maybe Word8 toGpsSat sat - | sat >= 64 = Nothing + | sat < 1 = Nothing + | sat > gpsMaxSat = Nothing | otherwise = Just sat toGpsCode :: Word8 -> Maybe Word8 @@ -153,19 +159,24 @@ | otherwise = 15 toPackedObsContent1074 :: Msm46SatelliteData -> Msm4SignalData -> (Int, Word8) -> (Int, Word8) -> Maybe PackedObsContent -toPackedObsContent1074 satData sigData (satIndex, sat) (sigIndex, sig) = do - sid <- toGpsSignal sat sig - freq <- toGpsFrequency sig - Just PackedObsContent - { _packedObsContent_P = toP (roughPseudorange + finePseudorange) - , _packedObsContent_L = toL ((roughPseudorange + finePhaserange) * (freq / 299792458.0)) - , _packedObsContent_D = toD - , _packedObsContent_cn0 = ((sigData ^. msm4SignalData_cnrs) !! sigIndex) * 4 - , _packedObsContent_lock = (sigData ^. msm4SignalData_lockTimes) !! sigIndex - , _packedObsContent_sid = sid - , _packedObsContent_flags = pseudorangeValid .|. phaseValid .|. halfCycleResolved - } +toPackedObsContent1074 satData sigData (satIndex, sat) (sigIndex, sig) + | no = Nothing + | otherwise = do + sid <- toGpsSignal sat sig + freq <- toGpsFrequency sig + Just PackedObsContent + { _packedObsContent_P = toP (roughPseudorange + finePseudorange) + , _packedObsContent_L = toL ((roughPseudorange + finePhaserange) * (freq / 299792458.0)) + , _packedObsContent_D = toD + , _packedObsContent_cn0 = ((sigData ^. msm4SignalData_cnrs) !! sigIndex) * 4 + , _packedObsContent_lock = (sigData ^. msm4SignalData_lockTimes) !! sigIndex + , _packedObsContent_sid = sid + , _packedObsContent_flags = pseudorangeValid .|. phaseValid .|. halfCycleResolved + } where + no = ((satData ^. msm46SatelliteData_ranges) !! satIndex) == 255 || + ((sigData ^. msm4SignalData_pseudoranges) !! sigIndex) == 16384 || + ((sigData ^. msm4SignalData_phaseranges) !! sigIndex) == 2097152 pseudorangeValid = 1 phaseValid = 2 halfCycleResolved = bool 4 0 $ (sigData ^. msm4SignalData_halfCycles) !! sigIndex @@ -175,19 +186,24 @@ finePhaserange = fromIntegral ((sigData ^. msm4SignalData_phaseranges) !! sigIndex) * gpsPseudorange * (2 ** (-29)) toPackedObsContent1077 :: Msm57SatelliteData -> Msm7SignalData -> (Int, Word8) -> (Int, Word8) -> Maybe PackedObsContent -toPackedObsContent1077 satData sigData (satIndex, sat) (sigIndex, sig) = do - sid <- toGpsSignal sat sig - freq <- toGpsFrequency sig - Just PackedObsContent - { _packedObsContent_P = toP (roughPseudorange + finePseudorange) - , _packedObsContent_L = toL ((roughPseudorange + finePhaserange) * (freq / 299792458.0)) - , _packedObsContent_D = toD - , _packedObsContent_cn0 = round (fromIntegral ((sigData ^. msm7SignalData_cnrs) !! sigIndex) * ((2 :: Double) ** (-4)) * 4) - , _packedObsContent_lock = toLock $ lock ((sigData ^. msm7SignalData_lockTimes) !! sigIndex) - , _packedObsContent_sid = sid - , _packedObsContent_flags = pseudorangeValid .|. phaseValid .|. halfCycleResolved - } +toPackedObsContent1077 satData sigData (satIndex, sat) (sigIndex, sig) + | no = Nothing + | otherwise = do + sid <- toGpsSignal sat sig + freq <- toGpsFrequency sig + Just PackedObsContent + { _packedObsContent_P = toP (roughPseudorange + finePseudorange) + , _packedObsContent_L = toL ((roughPseudorange + finePhaserange) * (freq / 299792458.0)) + , _packedObsContent_D = toD + , _packedObsContent_cn0 = round (fromIntegral ((sigData ^. msm7SignalData_cnrs) !! sigIndex) * ((2 :: Double) ** (-4)) * 4) + , _packedObsContent_lock = toLock $ lock ((sigData ^. msm7SignalData_lockTimes) !! sigIndex) + , _packedObsContent_sid = sid + , _packedObsContent_flags = pseudorangeValid .|. phaseValid .|. halfCycleResolved + } where + no = ((satData ^. msm57SatelliteData_ranges) !! satIndex) == 255 || + ((sigData ^. msm7SignalData_pseudoranges) !! sigIndex) == 524288 || + ((sigData ^. msm7SignalData_phaseranges) !! sigIndex) == 8388608 pseudorangeValid = 1 phaseValid = 2 halfCycleResolved = bool 4 0 $ (sigData ^. msm7SignalData_halfCycles) !! sigIndex 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 @@ -182,7 +182,7 @@ , _packedObsContent_flags = obsFlags } where - no = sat > gpsMaxSat || l1o ^. gpsL1Observation_pseudorange == 524288 || l1o ^. gpsL1Observation_carrierMinusCode == -524288 + no = sat < 1 || sat > gpsMaxSat || l1o ^. gpsL1Observation_pseudorange == 524288 || l1o ^. gpsL1Observation_carrierMinusCode == -524288 p1 = pseudorange gpsPseudorange (l1o ^. gpsL1Observation_pseudorange) (l1eo ^. gpsL1ExtObservation_ambiguity) l1 = carrierPhase gpsL1CarrierPhase p1 (l1o ^. gpsL1Observation_carrierMinusCode) @@ -201,7 +201,7 @@ , _packedObsContent_flags = obsFlags } where - no = sat > gpsMaxSat || l2o ^. gpsL2Observation_pseudorangeDifference == -8192 || l2o ^. gpsL2Observation_carrierMinusCode == -524288 + no = sat < 1 || sat > gpsMaxSat || l2o ^. gpsL2Observation_pseudorangeDifference == -8192 || l2o ^. gpsL2Observation_carrierMinusCode == -524288 p1 = pseudorange gpsPseudorange (l1o ^. gpsL1Observation_pseudorange) (l1eo ^. gpsL1ExtObservation_ambiguity) p2 = p1 + pseudorangeDifference (l2o ^. gpsL2Observation_pseudorangeDifference) l2 = carrierPhase gpsL2CarrierPhase p1 (l2o ^. gpsL2Observation_carrierMinusCode) @@ -222,7 +222,7 @@ , _packedObsContent_flags = obsFlags } where - no = sat > glonassMaxSat || l1o ^. glonassL1Observation_carrierMinusCode == -524288 + no = sat < 1 || sat > glonassMaxSat || l1o ^. glonassL1Observation_frequency > 20 || l1o ^. glonassL1Observation_carrierMinusCode == -524288 p1 = pseudorange glonassPseudorange (l1o ^. glonassL1Observation_pseudorange) (l1eo ^. glonassL1ExtObservation_ambiguity) l1 = carrierPhase (glonassL1CarrierPhase (l1o ^. glonassL1Observation_frequency)) p1 (l1o ^. glonassL1Observation_carrierMinusCode) @@ -242,7 +242,7 @@ , _packedObsContent_flags = obsFlags } where - no = sat > glonassMaxSat || l2o ^. glonassL2Observation_pseudorangeDifference == -8192 || l2o ^. glonassL2Observation_carrierMinusCode == -524288 + no = sat < 1 || sat > glonassMaxSat || l1o ^. glonassL1Observation_frequency > 20 || l2o ^. glonassL2Observation_pseudorangeDifference == -8192 || l2o ^. glonassL2Observation_carrierMinusCode == -524288 p1 = pseudorange glonassPseudorange (l1o ^. glonassL1Observation_pseudorange) (l1eo ^. glonassL1ExtObservation_ambiguity) p2 = p1 + pseudorangeDifference (l2o ^. glonassL2Observation_pseudorangeDifference) l2 = carrierPhase (glonassL2CarrierPhase (l1o ^. glonassL1Observation_frequency)) p1 (l2o ^. glonassL2Observation_carrierMinusCode) @@ -277,7 +277,7 @@ -- | Derive sender from station. -- toSender :: Word16 -> Word16 -toSender station = station .|. 61440 +toSender station = station .|. 61568 -- | FromObservations produces GPS time, SBP packed observations, and sender from RTCMv3 observation messages. -- diff --git a/src/Data/RTCM3/SBP/Positions.hs b/src/Data/RTCM3/SBP/Positions.hs --- a/src/Data/RTCM3/SBP/Positions.hs +++ b/src/Data/RTCM3/SBP/Positions.hs @@ -25,7 +25,7 @@ -- | Derive sender from station. -- toSender :: Word16 -> Word16 -toSender station = station .|. 61440 +toSender station = station .|. 61568 -- | FromPosition produces ecef x, y, x and sender values from RTCMv3 position messages. -- diff --git a/src/Data/RTCM3/SBP/SSR.hs b/src/Data/RTCM3/SBP/SSR.hs --- a/src/Data/RTCM3/SBP/SSR.hs +++ b/src/Data/RTCM3/SBP/SSR.hs @@ -73,7 +73,7 @@ , _msgSsrOrbitClock_c1 = c ^. gpsOrbitClockCorrection_deltaClockC1 , _msgSsrOrbitClock_c2 = c ^. gpsOrbitClockCorrection_deltaClockC2 } - SBPMsgSsrOrbitClock m' $ toSBP m' 61440 + SBPMsgSsrOrbitClock m' $ toSBP m' 61568 glonassOrbitClockConverter :: MonadStore e m => Msg1066 -> Conduit i m [SBPMsg] glonassOrbitClockConverter m = do @@ -98,7 +98,7 @@ , _msgSsrOrbitClock_c1 = c ^. glonassOrbitClockCorrection_deltaClockC1 , _msgSsrOrbitClock_c2 = c ^. glonassOrbitClockCorrection_deltaClockC2 } - SBPMsgSsrOrbitClock m' $ toSBP m' 61440 + SBPMsgSsrOrbitClock m' $ toSBP m' 61568 gpsCodeBiasConverter :: MonadStore e m => Msg1059 -> Conduit i m [SBPMsg] gpsCodeBiasConverter m = do @@ -117,7 +117,7 @@ , _codeBiasesContent_value = b ^. gpsCodeBias_codeBias } } - SBPMsgSsrCodeBiases m' $ toSBP m' 61440 + SBPMsgSsrCodeBiases m' $ toSBP m' 61568 glonassCodeBiasConverter :: MonadStore e m => Msg1065 -> Conduit i m [SBPMsg] glonassCodeBiasConverter m = do @@ -136,7 +136,7 @@ , _codeBiasesContent_value = b ^. glonassCodeBias_codeBias } } - SBPMsgSsrCodeBiases m' $ toSBP m' 61440 + SBPMsgSsrCodeBiases m' $ toSBP m' 61568 gpsPhaseBiasConverter :: MonadStore e m => Msg1265 -> Conduit i m [SBPMsg] gpsPhaseBiasConverter m = do @@ -162,7 +162,7 @@ , _phaseBiasesContent_bias = b ^. gpsPhaseBias_phaseBias } } - SBPMsgSsrPhaseBiases m' $ toSBP m' 61440 + SBPMsgSsrPhaseBiases m' $ toSBP m' 61568 glonassPhaseBiasConverter :: MonadStore e m => Msg1266 -> Conduit i m [SBPMsg] glonassPhaseBiasConverter m = do @@ -188,4 +188,4 @@ , _phaseBiasesContent_bias = b ^. glonassPhaseBias_phaseBias } } - SBPMsgSsrPhaseBiases m' $ toSBP m' 61440 + SBPMsgSsrPhaseBiases m' $ toSBP m' 61568