%GPS Week number: weekNumber = floor(-double(gnssRaw.FullBiasNanos)*1e-9/GpsConstants.WEEKSEC); %check for fields that are commonly all zero and may be missing from gnssRaw if ~isfield(gnssRaw,'BiasNanos') gnssRaw.BiasNanos = 0; end if ~isfield(gnssRaw,'TimeOffsetNanos') gnssRaw.TimeOffsetNanos = 0; end %compute time of measurement relative to start of week %subtract big longs (i.e. time from 1980) before casting time of week as double WEEKNANOS = int64(GpsConstants.WEEKSEC*1e9); weekNumberNanos = int64(weekNumber)*int64(GpsConstants.WEEKSEC*1e9); %compute tRxNanos using gnssRaw.FullBiasNanos(1), so that % tRxNanos includes rx clock drift since the first epoch: tRxNanos = gnssRaw.TimeNanos -gnssRaw.FullBiasNanos(1) - weekNumberNanos; %Assert if Tow state ~=1, because then gnssRaw.FullBiasNanos(1) might be wrong State = gnssRaw.State(1); assert(bitand(State,2^0) & bitand(State,2^3),... 'gnssRaw.State(1) must have bits 0 and 3 true before calling ProcessGnssMeas') %tRxNanos now since beginning of the week, unless we had a week rollover %assert(all(tRxNanos <= WEEKNANOS),'tRxNanos should be <= WEEKNANOS') %TBD check week rollover code, and add assert tRxNanos <= WEEKNANOS after assert(all(tRxNanos >= 0),'tRxNanos should be >= 0') %subtract the fractional offsets TimeOffsetNanos and BiasNanos: tRxSeconds = (double(tRxNanos)-gnssRaw.TimeOffsetNanos-gnssRaw.BiasNanos)*1e-9; tTxSeconds = double(gnssRaw.ReceivedSvTimeNanos)*1e-9; %check for week rollover in tRxSeconds [prSeconds,tRxSeconds] = CheckGpsWeekRollover(tRxSeconds,tTxSeconds); %we are ready to compute pseudorange in meters: PrM = prSeconds*GpsConstants.LIGHTSPEED;
尚无评论!