def navigation(trackResults, settings): numGoodSats = 0 for i in range(len(trackResults)): if (trackResults[i].status == 'T'): numGoodSats = numGoodSats + 1 if (numGoodSats < 4): raise Exception('Too few satellites to calculate nav solution') ##TODO : check lengths of all trackResults prompt correlations? if (len(trackResults[0].I_P) < 36000): raise Exception('Length of tracking too short to calculate nav solution') (subFrameStart, activeChnList) = findPreambles(trackResults,settings) #Pass 1500 nav bits (5 subframes), starting at a subframe preamble, to ephemeris.py to get ephemeris eph = [[] for i in range(32)] # for channelNr in activeChnList: ##TODO : CHANGE FOR LOOP BACK TO OVER activeChnList## for channelNr in [0]: #Get 1500 nav bits starting at a subframe navBitsIndices = np.r_[subFrameStart[channelNr]:subFrameStart[channelNr]+(1500*20)] navBits = corrs2bits.unsigned(trackResults[channelNr].I_P[navBitsIndices]) #Get the last parity bit of the previous subFrame #subFrame's first 24 bits are XOR'd with it before transmission D30starIndices = np.r_[subFrameStart[channelNr]-20:subFrameStart[channelNr]] D30star = corrs2bits.unsigned(trackResults[channelNr].I_P[D30starIndices]) #Extract ephemeris from the 5 subFrames (eph[trackResults[channelNr].PRN], TOW) = ephemeris(navBits,D30star) ##TODO : remove below statement (navSolutions, eph) = (0,0) return (navSolutions, eph)
def navigation(trackResults, settings): numGoodSats = 0 for i in range(len(trackResults)): if (trackResults[i].status == 'T'): numGoodSats = numGoodSats + 1 if (numGoodSats < 4): raise Exception('Too few satellites to calculate nav solution') #TODO : check lengths of all trackResults prompt correlations? if (len(trackResults[0].I_P) < 36000): raise Exception('Length of tracking too short to calculate nav solution') (subFrameStart, activeChnList) = findPreambles(trackResults,settings) #Pass 1500 nav bits (5 subframes), starting at a subframe preamble, #to ephemeris.py to get ephemeris eph = [[] for i in range(32)] for channelNr in reversed(activeChnList): #Get 1500 nav bits starting at a subframe navBitsIndices = np.r_[subFrameStart[channelNr]:subFrameStart[channelNr]+(1500*20)] navBits = corrs2bits.unsigned(trackResults[channelNr].I_P[navBitsIndices]) #Get the last parity bit of the previous subFrame #subFrame's first 24 bits are XOR'd with it before transmission D30starIndices = np.r_[subFrameStart[channelNr]-20:subFrameStart[channelNr]] D30star = corrs2bits.unsigned(trackResults[channelNr].I_P[D30starIndices]) #Extract ephemeris from the 5 subFrames (eph[trackResults[channelNr].PRN], TOW) = ephemeris(navBits,D30star) #TODO : Implement better way to determine if satellite is usable (health, accuracy) #Exclude satellite if for some reason ephemeris parameters weren't assigned #(subframe ID's 1-3 weren't assigned?) if (eph[trackResults[channelNr].PRN].IODC==0 and eph[trackResults[channelNr].PRN].IODC_sf2==0 and eph[trackResults[channelNr].PRN].IODC_sf3==0): activeChnList.pop(channelNr) #If we don't have enough satellites after rejecting those whose ephemerides failed #to extract properly, then we can't calculate the nav solution if len(activeChnList) < 4: raise Exception('Not enough satellites after extracting ephemerides to calculate nav solution') #TODO : include support for an initial position here and associated elevation mask #Include all satellites for first iteration of the nav solution satElev = np.ones(len(activeChnList))*90 readyChnList = activeChnList[:] transmitTime = TOW msToProcessNavigation = np.int(np.floor((settings.msToProcess-max(subFrameStart))/settings.navSolPeriod)) navSolutions = navSolutions_class(msToProcessNavigation) for currMeasNr in range(1): # for currMeasNr in range(msToProcessNavigation): if currMeasNr == 0: #append indexes to arrays that need them appended for i in range(len(activeChnList)): navSolutions.channel.rawP[] activeChnList = gThanMask(satElev) for i in activeChnList: navSolutions.channel.PRN[i].append(trackResults[i].PRN) navSolutions.channel.el[currMeasNr] = [[] for i in range(len(activeChnList))] navSolutions.channel.az[currMeasNr] = [[] for i in range(len(activeChnList))] #Find pseudoranges for channelNumber in activeChnList: navSolutions.channel.rawP[channelNumber][currMeasNr] = calculatePseudorange(trackResults, \ [i + settings.navSolPeriod * (currMeasNr) for i in subFrameStart], \ channelNumber, \ settings) #Find satellite positions and clock corrections (satPositions, satClkCorr) = satpos(transmitTime, \ [trackResults[i].PRN for i in activeChnList], \ eph, \ settings) #Find receiver position #We can only calculate solution if >= 4 satellites are found if len(activeChnList) > 3: #Calculate receiver position (xyzdt, \ navSolutions.channel.el, \ navSolutions.channel.az, \ navSolutions.DOP[currMeasNr]) = leastSquarePos(satPositions, navSolutions.channel.rawP[np.r_[activeChnList]][currMeasNr] + satClkCorr*settings.c, settings) #TODO : remove below statement (navSolutions, eph) = (0,0) return (navSolutions, eph)