16、GPS L1信号捕获和跟踪实现

\qquad 下面是HD-GR GNSS导航软件的GPS L1信号捕获和跟踪实现代码，入口函数gps_track_channels(…)：

// gps_accum_task.c -- GPS L1 signal carrier and code tracking.

/* 
 * Copyright (C) 2005 Andrew Greenberg
 * Distributed under the GNU GENERAL PUBLIC LICENSE (GPL) Version 2 (June 1991).
 * See the "COPYING" file distributed with this software for more information.
 */

/* Namuru GPS OpenSource receiver project
 * Original : tracking.c
 * Modes    : Some code has been modified for adaption to the Namuru HW by Peter Mumford
 * 
 *    In general, the original code has been commented out and
 *    replaced (with peters initials (pjm) on the new code lines).
 *    The Namuru HW is different from the GP4020 / 2021 in the following points:
 *    1) early, prompt and late correlators, each separated by 0.5 chips
 * 
 * version  : V1.0
 * date     : 21st/Dec/2006
 */

/* 
 * HD-GR GNSS receiver project
 * Modes    : Inherited the code of tracking.c in the Namuru GPS receiver project 
 *            V1.0 and made necessary adjustments to adapt to the new HW, RTOS.
 * version  : V1.0
 * date     : xx/xx/2015
 */

#include 
#include 
#include 
#include 
#include 
#include "includes.h"
#include "system.h"
#include "altera_avalon_pio_regs.h"
#include "alt_types.h"
#include "sys/alt_irq.h"
#include "main_allocate.h"
#include "gps_accum_task.h"
#include "gps_message.h"

/*******************************************************************************
 * #defines
 ******************************************************************************/

#define GPS_PULLIN_TIME			1000	// 1 second
#define GPS_PHASETEST_TIME		500		// last 1/2 second of pull in, start phase test

// m_GPS_CH[ch].ef_out = m_GPS_CH[ch].ef_out/(2*PI_SHIFT14*T), T=0.02 s
// m_GPS_CH[ch].ef_out = m_GPS_CH[ch].ef_out/((20-2)*GPS_CARR_FREQ_RES)
#define GPS_PULLIN_EFOUT_COF	(int)(0.5+0.72*PI_SHIFT14*GPS_CARR_FREQ_RES)

/*******************************************************************************
 * Global variables
 ******************************************************************************/

gps_chan_t m_GPS_CH[GPS_MAX_CHANNELS] __attribute__ ((section(".isrdata.rwdata")));

//
// ACCUM
//

/*******************************************************************************
 * Static (module level) variables
 ******************************************************************************/

short m_GpsCarrSrchStep __attribute__ ((section(".isrdata.rwdata")));					// carry search step length
static unsigned short GpsCarrSrchWidth __attribute__ ((section(".isrdata.rwdata")));	// carry search width

/*******************************************************************************
 * 以下环路滤波参数未初始化。引用该源文件代码可在此将它们初始化为适合目标基带模块参数的值。例如：
 * static long Gps_Pull_Code_C0 __attribute__ ((section(".isrdata.rwdata"))) = 477;
 *
 * date: 17st/Sep/2021
******************************************************************************/
#ifdef GPS_USING_NAMURU_DLL
static long  Gps_Pull_Code_TtwoTone __attribute__ ((section(".isrdata.rwdata")));
static long  Gps_Pull_Code_DtTone __attribute__ ((section(".isrdata.rwdata")));
static long  Gps_Lock_Code_TtwoTone __attribute__ ((section(".isrdata.rwdata")));
static long  Gps_Lock_Code_DtTone __attribute__ ((section(".isrdata.rwdata")));
#else
static long  Gps_Pull_Code_C0 __attribute__ ((section(".isrdata.rwdata")));
static long  Gps_Pull_Code_C1 __attribute__ ((section(".isrdata.rwdata")));
static long  Gps_Lock_Code_C0 __attribute__ ((section(".isrdata.rwdata")));
static long  Gps_Lock_Code_C1 __attribute__ ((section(".isrdata.rwdata")));
#endif // GPS_USING_NAMURU_DLL
static long  Gps_Pull_Carr_C0 __attribute__ ((section(".isrdata.rwdata")));
static long  Gps_Pull_Carr_C1 __attribute__ ((section(".isrdata.rwdata")));
static long  Gps_Pull_Carr_C2 __attribute__ ((section(".isrdata.rwdata")));
static long  Gps_Lock_Carr_C0 __attribute__ ((section(".isrdata.rwdata")));
static long  Gps_Lock_Carr_C1 __attribute__ ((section(".isrdata.rwdata")));
static long  Gps_Lock_Carr_C2 __attribute__ ((section(".isrdata.rwdata")));


/*******************************************************************************
 * Prototypes (Local visible functions)
 ******************************************************************************/

static void gps_backto_acquire( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void gps_enter_pull_in( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void gps_check_signal( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
#ifdef GPS_USING_NAMURU_DLL
static void gps_dll( unsigned short ch, long dcode_freq, long TtwoTone, long DtTone) __attribute__ ((section(".isrcode.txt")));
#else
static void gps_dll1( unsigned short ch, long dcode_freq, long c0, long c1) __attribute__ ((section(".isrcode.txt")));
#endif // GPS_USING_NAMURU_DLL
static void gps_pll1( unsigned short ch, long dcarr_phase, long dcarr_freq, long c0, long c1, long c2) __attribute__ ((section(".isrcode.txt")));
static void gps_pull_in_checklock(unsigned short ch, long dcarr_phase) __attribute__ ((section(".isrcode.txt")));
static void gps_acquire( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void gps_confirm( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void gps_freq_pull( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void gps_pull_in( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void gps_lock( unsigned short ch) __attribute__ ((section(".isrcode.txt")));

#ifdef GPS_FALSE_PHASE_LOCK_DETECTOR
static short gps_cd1sec_FPLD(unsigned short ch) __attribute__ ((section(".isrcode.txt")));
#endif // GPS_FALSE_PHASE_LOCK_DETECTOR


/*******************************************************************************
 * Write 32 bits to the code DCO rate and carrier DCO rate registers
 *
 * Modified by Peter Mumford for namuru HW (2006)
 * Modified by Cheng Huaide for HD-GR GNSS (2015)
 ******************************************************************************/

inline void gps_set_code_dco_rate( unsigned short ch, unsigned long freq)
{
    write_to_correlator( (GPS_CH00_BASE + ch * CH_BASE_STEP + CODE_NCO), freq );
}

inline void gps_set_carrier_dco_rate( unsigned short ch, unsigned long freq)
{
	write_to_correlator( (GPS_CH00_BASE + ch * CH_BASE_STEP + CARRIER_NCO), freq );
}


/*
 * False Phase Lock Detector (FPLD)
*/

#ifdef GPS_FALSE_PHASE_LOCK_DETECTOR

// FPLD defines
#define FPLD_G_THRESHOLD		(PI_SHIFT14 / 12)	// 15 deg.

static short gps_cd1sec_FPLD(unsigned short ch)
{
	signed long i_now = m_GPS_CH[ch].i_p_20 >> 4;
	signed long q_now = m_GPS_CH[ch].q_p_20 >> 4;
	signed long i_old = m_GPS_CH[ch].i_p_20_1 >> 4;
	signed long q_old = m_GPS_CH[ch].q_p_20_1 >> 4;
	signed long C = i_old*q_now - q_old*i_now;
	signed long D = i_old*i_now + q_old*q_now;

	if (m_GPS_CH[ch].cnt_fpld == 0) {
		m_GPS_CH[ch].c_fpld = 0;
		m_GPS_CH[ch].d_fpld = 0;
	}

	m_GPS_CH[ch].cnt_fpld ++;
	m_GPS_CH[ch].c_fpld += (C - m_GPS_CH[ch].c_fpld)/m_GPS_CH[ch].cnt_fpld;
	m_GPS_CH[ch].d_fpld += (D - m_GPS_CH[ch].d_fpld)/m_GPS_CH[ch].cnt_fpld;
	if (m_GPS_CH[ch].cnt_fpld == 50) {
		m_GPS_CH[ch].cnt_fpld = 0;
		C = fix_atan2(m_GPS_CH[ch].c_fpld, m_GPS_CH[ch].d_fpld);
		if (labs(C) > FPLD_G_THRESHOLD) {
			return C;
		}
	}
	return 0;
}

#endif // GPS_FALSE_PHASE_LOCK_DETECTOR

/******************************************************************************
 * Need to set up m_GPS_CH[] structure and initialize the loop dynamic parameters.
 ******************************************************************************/
void gps_initialize_tracking( void)
{
    unsigned short ch;

	// Why are these a good choices?
	GpsCarrSrchWidth = 40;    // search 20 frequency steps on either side
	m_GpsCarrSrchStep = (short)(500./GPS_CARR_FREQ_RES); // 500Hz (CHD)

    for (ch = 0; ch < GPS_MAX_CHANNELS; ch++) {
		m_GPS_CH[ch].state = CHANNEL_OFF;
    }
}

static void gps_backto_acquire( unsigned short ch)
{
	m_GPS_CH[ch].state = CHANNEL_ACQUISITION;
	gps_clear_messages(ch);		// flag the message_thread that the
								// past subframes are no longer valid

	m_GPS_CH[ch].codes = 0;
	m_GPS_CH[ch].code_freq = GPS_CODE_REF;
	gps_set_code_dco_rate(ch, m_GPS_CH[ch].code_freq);

	// Clear sync flags
	m_GPS_CH[ch].bit_sync = 0;
}

static void gps_enter_pull_in( unsigned short ch) 
{
	m_GPS_CH[ch].state = CHANNEL_PULL_IN;
	m_GPS_CH[ch].ch_time = 0;
	m_GPS_CH[ch].th_rms = 0;
	m_GPS_CH[ch].bit_sync = 0;

	m_GPS_CH[ch].dcode_freq_1 = 0;
	m_GPS_CH[ch].dcarr_phase_1 = 0;
	m_GPS_CH[ch].dcarr_phase_2 = 0;

	m_GPS_CH[ch].ms_sign = 0x12345;
	m_GPS_CH[ch].ms_count = 0;
}

static void gps_check_signal( unsigned short ch)
{
	// Amplitude Tang decider, exit if the amplitude is always small.
	if (m_GPS_CH[ch].p_mag<TANG_THRESHOLD) {
		m_GPS_CH[ch].tang-=3;
	}
	else if (m_GPS_CH[ch].tang<90) {
		m_GPS_CH[ch].tang+=1;
	}

	if (m_GPS_CH[ch].tang<-30) {
		gps_backto_acquire(ch);
	}
}

#ifdef GPS_USING_NAMURU_DLL
static void gps_dll( unsigned short ch, long dcode_freq, long TtwoTone, long DtTone)
{
	long ddf = (dcode_freq - m_GPS_CH[ch].dcode_freq_1) * TtwoTone;
	m_GPS_CH[ch].code_freq += (dcode_freq * DtTone + ddf) >> 14;
	gps_set_code_dco_rate( ch, m_GPS_CH[ch].code_freq);
	m_GPS_CH[ch].dcode_freq_1 = dcode_freq;
}
#else
static void gps_dll1( unsigned short ch, long dcode_freq, long c0, long c1)
{
	long ddf = dcode_freq * c0 + m_GPS_CH[ch].dcode_freq_1 * c1;
	m_GPS_CH[ch].code_freq += ddf >> 14;
	gps_set_code_dco_rate( ch, m_GPS_CH[ch].code_freq);
	m_GPS_CH[ch].dcode_freq_1 = dcode_freq;
}
#endif // GPS_USING_NAMURU_DLL

static void gps_pll1( unsigned short ch, long dcarr_phase, long dcarr_freq, long c0, long c1, long c2)
{
	long ddcar = dcarr_phase*c0 + m_GPS_CH[ch].dcarr_phase_1*c1 + dcarr_freq*c2;
	m_GPS_CH[ch].carrier_freq += ddcar >> 14;
	gps_set_carrier_dco_rate( ch, m_GPS_CH[ch].carrier_freq);
	m_GPS_CH[ch].dcarr_phase_1 = dcarr_phase;
}


/******************************************************************************
 FUNCTION gps_pull_in_checklock(unsigned short ch, long dcarr_phase)
 RETURNS  None.

 PARAMETERS
 ch  char			// Which correlator channel to use
 long dcarr_phase	// A measure of carrier phase error

 PURPOSE  to check if can transit to tracking mode.
 ******************************************************************************/
static void gps_pull_in_checklock(unsigned short ch, long dcarr_phase)
{
	// Near the end of pull in, start the phase test
	if (m_GPS_CH[ch].ch_time > (GPS_PULLIN_TIME - GPS_PHASETEST_TIME)) {
		m_GPS_CH[ch].th_rms += (dcarr_phase * dcarr_phase) >> 14;
	}

	// Done with pull in. Wait until the end of a data bit.
	// A bit transition will happen at the next dump.
	if ((m_GPS_CH[ch].ms_count == 19) && (m_GPS_CH[ch].ch_time >= GPS_PULLIN_TIME)) {
		// Bit edge was detected.
		if (m_GPS_CH[ch].bit_sync >= 5) {
			// Calculate the mean square value of phase error. Because of the function 
			// fix_sqrt enlarges the root result by 2^7, so the unit of the mean square 
			// value is still 1 radian = 16384, and the subsequent mean square error 
			// 12000 is about 40 degrees.
			
			// Sufficient signal, transition to tracking mode
			// (12000*12000 >> 14) = 8789.0625
			if (m_GPS_CH[ch].th_rms < (8789*GPS_PHASETEST_TIME)) {
				m_GPS_CH[ch].i_p_20 = 0;
				m_GPS_CH[ch].q_p_20 = 0;
				m_GPS_CH[ch].i_e_20 = 0;
				m_GPS_CH[ch].q_e_20 = 0;
				m_GPS_CH[ch].i_l_20 = 0;
				m_GPS_CH[ch].q_l_20 = 0;
#ifdef GPS_CODE_LOOP_20MS_RATE
#endif // GPS_CODE_LOOP_20MS_RATE

#ifdef GPS_FALSE_PHASE_LOCK_DETECTOR
				m_GPS_CH[ch].i_p_20_1 = 0;
				m_GPS_CH[ch].q_p_20_1 = 0;
				m_GPS_CH[ch].c_fpld = 0;
				m_GPS_CH[ch].d_fpld = 0;
				m_GPS_CH[ch].cnt_fpld = 0;
				m_GPS_CH[ch].tot_fpld = 0;
#endif // GPS_FALSE_PHASE_LOCK_DETECTOR

				// Officially switch modes
				m_GPS_CH[ch].state = CHANNEL_LOCK;
				return;
			}
		}

		// We lost the pullin. Eventually, do a nice transition back to
		// gps_confirm and/or gps_acquire. For now, to be paranoid, just kill
		// the channel.
		gps_backto_acquire(ch);
	}
}

/******************************************************************************
 FUNCTION gps_acquire( unsigned short ch)
 RETURNS  None.

 PARAMETERS
 ch  char // Which correlator channel to use

 PURPOSE  to perform initial gps_acquire by searching code and frequency space
 looking for a high correlation

 Original function : acquire
 WRITTEN BY
 Clifford Kelley

 Modified by Peter Mumford for namuru HW (2006)
 Modified by Cheng Huaide for HD-GR GNSS (2015)
 ******************************************************************************/
static void gps_acquire( unsigned short ch)
{
	/* Search carrier frequency bins */
	if (abs(m_GPS_CH[ch].n_freq) <= GpsCarrSrchWidth) {
		long power;

		power = m_GPS_CH[ch].e_mag + m_GPS_CH[ch].p_mag + m_GPS_CH[ch].l_mag;
		if (power > ACQ_THRESHOLD) {
			m_GPS_CH[ch].state = CHANNEL_CONFIRM;
			m_GPS_CH[ch].n_confirm = 0;
			m_GPS_CH[ch].m_thresh = 0;
			return;
		}

		// No satellite yet; try delaying the code DCO 1/2 chip
		// accumulators[ch].write.code_slew_counter = 1;
		write_to_correlator(GPS_CH00_BASE + ch * CH_BASE_STEP + CODE_SLEW, 1);

		// Keep count of how many code phases we've searched
		m_GPS_CH[ch].codes++;

		// if (m_GPS_CH[ch].codes > 2044) // PRN code length in 1/2 chips
        // All code offsets have been searched; try another frequency bin
		if (m_GPS_CH[ch].codes > 2045) {
			// reset code phase count
            m_GPS_CH[ch].codes = 0;

			// Move to another frequency bin
			// Note the use of carrier_corr, this is meant to be a correction
			// for estimated TCXO frequency error, currently set to zero.
			// See the comment in cold_allocate_channel()
			// Generate a search sequence: 0, 1, -1, 2, -2, ...
			// This can be re-written to avoid the multiply.
			if (m_GPS_CH[ch].n_freq & 1) { // Odd search bins map to the "right"
				m_GPS_CH[ch].carrier_freq = GPS_CARRIER_REF + m_GPS_CH[ch].carrier_corr +
					m_GpsCarrSrchStep * (1 + (m_GPS_CH[ch].n_freq >> 1));
			}
			else { // Even search bins are to the "left" of GPS_CARRIER_REF
				m_GPS_CH[ch].carrier_freq = GPS_CARRIER_REF + m_GPS_CH[ch].carrier_corr -
					m_GpsCarrSrchStep * (m_GPS_CH[ch].n_freq >> 1);
			}

			// Set carrier DCO
			gps_set_carrier_dco_rate(ch, m_GPS_CH[ch].carrier_freq);

			m_GPS_CH[ch].n_freq++; // next time try the next search bin
		}
	}
	else {
		// End of frequency search: release the channel. A mainline thread will
		// eventually allocate  another satellite PRN to this channel
		m_GPS_CH[ch].state = CHANNEL_OFF;
	}
}

/*******************************************************************************
 FUNCTION gps_confirm(unsigned short ch)
 RETURNS  None.

 PARAMETERS
 ch  char  channel number

 PURPOSE  to gps_lock the presence of a high correlation peak using an n of m
 algorithm

 Original function : confirm
 WRITTEN BY
 Clifford Kelley

 Modified by Peter Mumford for namuru HW (2006)
 Modified by Cheng Huaide for HD-GR GNSS (2015)
*******************************************************************************/
static void gps_confirm( unsigned short ch)
{
	long power;

	// count number of gps_confirm attempts
	m_GPS_CH[ch].n_confirm++;

	power = m_GPS_CH[ch].e_mag + m_GPS_CH[ch].p_mag + m_GPS_CH[ch].l_mag;
	if (power > ACQ_THRESHOLD) {
		// count number of good hits
		m_GPS_CH[ch].m_thresh++;
	}

    // try "n" gps_confirm attempts
	if (m_GPS_CH[ch].n_confirm > 10) {
		// confirmed if good hits >= "m"
		if (m_GPS_CH[ch].m_thresh >= 8) {
			m_GPS_CH[ch].state = CHANNEL_FREQ_PULL;

			m_GPS_CH[ch].ch_time = 0;
			m_GPS_CH[ch].th_rms = 0;
			m_GPS_CH[ch].bit_sync = 0;

			m_GPS_CH[ch].dcode_freq_1 = 0;
			m_GPS_CH[ch].dcarr_phase_1 = 0;
			m_GPS_CH[ch].dcarr_phase_2 = 0;

			// Some garbage data
			m_GPS_CH[ch].ms_sign = 0x12345;
			m_GPS_CH[ch].ms_count = 0;

			m_GPS_CH[ch].tang = 0;

			m_GPS_CH[ch].ef_out = 0;
			m_GPS_CH[ch].ef_max = -0x7fffffff;
			m_GPS_CH[ch].ef_min = 0x7fffffff;
		}
        else {
			// Keep searching - assumes search parameters are still ok
			m_GPS_CH[ch].state = CHANNEL_ACQUISITION;

			// Clear sync flags
			m_GPS_CH[ch].bit_sync = 0;
		}
	}
}

/*******************************************************************************
 FUNCTION gps_freq_pull(unsigned short ch)
 RETURNS  None.

 PARAMETERS
 ch  char  channel number

 PURPOSE  to adjust the frequency of the acquired signal using a FLL.
*******************************************************************************/
static void gps_freq_pull( unsigned short ch)
{
	gps_check_signal(ch);
	if (m_GPS_CH[ch].state != CHANNEL_FREQ_PULL) {
		return;
	}

	// Calculate frequency error
	signed long C = m_GPS_CH[ch].i_p_1*m_GPS_CH[ch].q_p - m_GPS_CH[ch].q_p_1*m_GPS_CH[ch].i_p;
	signed long D = m_GPS_CH[ch].i_p_1*m_GPS_CH[ch].i_p + m_GPS_CH[ch].q_p_1*m_GPS_CH[ch].q_p;
	signed long df = fix_atan2(C, D);

	// Accumulate frequency error
	m_GPS_CH[ch].ef_out += df;
	if (m_GPS_CH[ch].ef_max < df) m_GPS_CH[ch].ef_max = df;
	if (m_GPS_CH[ch].ef_min > df) m_GPS_CH[ch].ef_min = df;

	m_GPS_CH[ch].ms_count ++;
	if (m_GPS_CH[ch].ms_count == 20) {
		// Set carrier NCO
		df = (m_GPS_CH[ch].ef_out - (m_GPS_CH[ch].ef_max + m_GPS_CH[ch].ef_min))/GPS_PULLIN_EFOUT_COF;
		m_GPS_CH[ch].carrier_freq += df;
		gps_set_carrier_dco_rate( ch, m_GPS_CH[ch].carrier_freq);

		gps_enter_pull_in(ch);
	}
}


/*******************************************************************************
 FUNCTION gps_pull_in( unsigned short ch)
 RETURNS  None.

 PARAMETERS
 ch  char  channel number

 PURPOSE
 pull in the frequency by trying to track the signal with a
 combination FLL and PLL
 it will attempt to track for xxx ms, the last xxx ms of data will be
 gathered to determine if we have both code and carrier gps_lock
 if so we will transition to track

 Original function : pull_in
 WRITTEN BY
 Clifford Kelley

 Modified by Peter Mumford for namuru HW (2006)
 Modified by Cheng Huaide for HD-GR GNSS (2015)
*******************************************************************************/
static void gps_pull_in( unsigned short ch)
{
	gps_check_signal(ch);
	if (m_GPS_CH[ch].state != CHANNEL_PULL_IN) {
		return;
	}

	unsigned long dms_sign;
	signed long C, D, M;
	signed long q_sum,i_sum;
	signed long dcode_freq, dcarr_phase, dcarr_freq;

	// 1. gps_pull_in Code tracking loop
	if (m_GPS_CH[ch].e_mag != 0 || m_GPS_CH[ch].l_mag != 0) {
		dcode_freq = ((m_GPS_CH[ch].e_mag - m_GPS_CH[ch].l_mag)<<14) /
				(m_GPS_CH[ch].e_mag + m_GPS_CH[ch].l_mag);
        if (m_GPS_CH[ch].ch_time <= 2) {
        	m_GPS_CH[ch].dcode_freq_1 = dcode_freq;
        }
        else {
#ifdef GPS_USING_NAMURU_DLL
			gps_dll(ch, dcode_freq, Gps_Pull_Code_TtwoTone, Gps_Pull_Code_DtTone);
#else
			gps_dll1(ch, dcode_freq, Gps_Pull_Code_C0, Gps_Pull_Code_C1);
#endif // GPS_USING_NAMURU_DLL
        }
	}

	// 2. Calculate phase error, frequency error, discriminated phase error
	// dcarr_phase is a measure of phase error
	i_sum = m_GPS_CH[ch].i_p + m_GPS_CH[ch].i_e + m_GPS_CH[ch].i_l;
	q_sum = m_GPS_CH[ch].q_p + m_GPS_CH[ch].q_e + m_GPS_CH[ch].q_l;
	if (i_sum || q_sum) {
		dcarr_phase = fix_atan(q_sum, i_sum);
	}
	else {
		dcarr_phase = m_GPS_CH[ch].dcarr_phase_1;
	}

	// 3. gps_pull_in Carrier tracking loop
	if (m_GPS_CH[ch].ch_time <= 5) {
		m_GPS_CH[ch].dcarr_phase_2 = m_GPS_CH[ch].dcarr_phase_1;
		m_GPS_CH[ch].dcarr_phase_1 = dcarr_phase;
	}
	else {
		C = (m_GPS_CH[ch].i_p_1*m_GPS_CH[ch].q_p - m_GPS_CH[ch].q_p_1*m_GPS_CH[ch].i_p);
		D = (m_GPS_CH[ch].i_p_1*m_GPS_CH[ch].i_p + m_GPS_CH[ch].q_p_1*m_GPS_CH[ch].q_p);
		M = lmag(C,D);
		dcarr_freq = (D >= 0) ? (C<<14)/M:-(C<<14)/M;
		gps_pll1(ch, dcarr_phase, dcarr_freq, Gps_Pull_Carr_C0, Gps_Pull_Carr_C1, Gps_Pull_Carr_C2);
	}

	// 4. Shift sign buffer to left
	dms_sign = m_GPS_CH[ch].ms_sign;
	m_GPS_CH[ch].ms_sign = ((m_GPS_CH[ch].ms_sign << 1) & 0x000fffff);
	// Set the LSB bit if negative
	if (i_sum < 0) {
		m_GPS_CH[ch].ms_sign |= 1;
	}

    // 5. Check bit edge transition
	// If the last 20 ms have the same sign and this dump is different:
	dms_sign ^= m_GPS_CH[ch].ms_sign;
	if (m_GPS_CH[ch].bit_sync == 0) {
		if (dms_sign == 0x01) {
			// Let the world know we're synced to the satellite message bits
			m_GPS_CH[ch].bit_sync = 1;
			// sync the ms count to the bit stream
			m_GPS_CH[ch].ms_count = 19;
			// set the flag that tells tracking() to set the 1ms epoch counter
			// after the accumulator registers are read: this will sync the
			// epoch counter with the bit stream (and the ms_count, too).
			m_GPS_CH[ch].load_1ms_epoch_count = 1;
		}
	}
	else {
		if ((dms_sign & 0x01) == 0x01) {
			if (m_GPS_CH[ch].ms_count == 19) {
				m_GPS_CH[ch].bit_sync ++;
			}
			else {
				m_GPS_CH[ch].bit_sync = 0;
			}
		}
	}

	// 6. Increase 1 ms epoch counter modulo 20
	m_GPS_CH[ch].ms_count++;
	if (m_GPS_CH[ch].ms_count > 19) {
		m_GPS_CH[ch].ms_count = 0;
	}

	// 7. Update variables for next loop
	// Update time counter
	m_GPS_CH[ch].ch_time++;

	// 8. Check if can transit to tracking mode
	gps_pull_in_checklock(ch, dcarr_phase);
}

/*******************************************************************************
 FUNCTION gps_lock( unsigned short ch)
 RETURNS  None.

 PARAMETERS  char ch  , channel number

 PURPOSE track carrier and code, and partially decode the navigation message
 (to determine TOW, subframe etc.)

 Original function : lock
 WRITTEN BY
 Clifford Kelley
 added Carrier Aiding as suggested by Jenna Cheng, UCR

 Modified by Peter Mumford for namuru HW (2006)
 Modified by Cheng Huaide for HD-GR GNSS (2015)
*******************************************************************************/
static void gps_lock( unsigned short ch)
{
	gps_check_signal(ch);
	if (m_GPS_CH[ch].state != CHANNEL_LOCK) {
		return;
	}

	signed long C, D, M;
	signed long q_sum, i_sum;
	signed long dcode_freq, dcarr_phase, dcarr_freq;

	// Check and correct ms_count according to epoch_codes
	if (m_GPS_CH[ch].epoch_codes < m_GPS_CH[ch].ms_count && m_GPS_CH[ch].ms_count != 19) {
		m_GPS_CH[ch].ms_count = 19;
	}
	else {
		m_GPS_CH[ch].ms_count = m_GPS_CH[ch].epoch_codes;
	}

	// Carrier loop
	i_sum = m_GPS_CH[ch].i_p + m_GPS_CH[ch].i_e + m_GPS_CH[ch].i_l;
	q_sum = m_GPS_CH[ch].q_p + m_GPS_CH[ch].q_e + m_GPS_CH[ch].q_l;
	if (i_sum != 0 || q_sum != 0) {
		C = (m_GPS_CH[ch].i_p_1*m_GPS_CH[ch].q_p - m_GPS_CH[ch].q_p_1*m_GPS_CH[ch].i_p);
		D = (m_GPS_CH[ch].i_p_1*m_GPS_CH[ch].i_p + m_GPS_CH[ch].q_p_1*m_GPS_CH[ch].q_p);
		M = lmag(C,D);
		dcarr_freq = (D >= 0) ? (C<<14)/M:-(C<<14)/M;
		dcarr_phase = sgn(i_sum) * (q_sum << 14) / lmag( i_sum, q_sum);
		gps_pll1(ch, dcarr_phase, dcarr_freq, Gps_Lock_Carr_C0, Gps_Lock_Carr_C1, Gps_Lock_Carr_C2);
	}

#ifndef GPS_CODE_LOOP_20MS_RATE
	// Code tracking loop @ 1ms rate
	if (m_GPS_CH[ch].e_mag != 0 || m_GPS_CH[ch].l_mag != 0) {
		dcode_freq = ((m_GPS_CH[ch].e_mag - m_GPS_CH[ch].l_mag)<<14) /
				(m_GPS_CH[ch].e_mag + m_GPS_CH[ch].l_mag);
#ifdef GPS_USING_NAMURU_DLL
		gps_dll(ch, dcode_freq, Gps_Pull_Code_TtwoTone, Gps_Pull_Code_DtTone);
#else
		gps_dll1(ch, dcode_freq, Gps_Pull_Code_C0, Gps_Pull_Code_C1);
#endif // GPS_USING_NAMURU_DLL
	}
#endif // GPS_CODE_LOOP_20MS_RATE

	// 20ms accumulator
	m_GPS_CH[ch].i_p_20 += m_GPS_CH[ch].i_p;
	m_GPS_CH[ch].q_p_20 += m_GPS_CH[ch].q_p;
	m_GPS_CH[ch].i_e_20 += m_GPS_CH[ch].i_e;
	m_GPS_CH[ch].q_e_20 += m_GPS_CH[ch].q_e;
	m_GPS_CH[ch].i_l_20 += m_GPS_CH[ch].i_l;
	m_GPS_CH[ch].q_l_20 += m_GPS_CH[ch].q_l;

	// Code tracking loop @ 20ms rate
	if (m_GPS_CH[ch].ms_count == 19) {
		/* Deleted by CHD -- 2020.4.29
		if (!m_GPS_CH[ch].bit_sync) {
			gps_backto_acquire(ch);
			return;
		}
		Deleted by CHD -- 2020.4.29 */

#ifdef GPS_CODE_LOOP_20MS_RATE
		// Code tracking loop @ 20ms rate
		signed long e_mag_20 = lmag( m_GPS_CH[ch].i_e_20, m_GPS_CH[ch].q_e_20);
		signed long l_mag_20 = lmag( m_GPS_CH[ch].i_l_20, m_GPS_CH[ch].q_l_20);
		if (e_mag_20 != 0 || l_mag_20 != 0) {
			dcode_freq = ((e_mag_20 - l_mag_20) << 14)/(e_mag_20 + l_mag_20);
#ifdef GPS_USING_NAMURU_DLL
			gps_dll(ch, dcode_freq, Gps_Lock_Code_TtwoTone, Gps_Lock_Code_DtTone);
#else
			gps_dll1(ch, dcode_freq, Gps_Lock_Code_C0, Gps_Lock_Code_C1);
#endif // GPS_USING_NAMURU_DLL
		}
#endif // GPS_CODE_LOOP_20MS_RATE

		// if (!m_ephetable[m_GPS_CH[ch].prn-1].valid || !m_messages[ch].set_epoch_flag) {
		// Data bit
		// m_GPS_CH[ch].bit = ((m_GPS_CH[ch].i_e_20 + m_GPS_CH[ch].i_l_20 + m_GPS_CH[ch].i_p_20) > 0);
		if ((m_GPS_CH[ch].i_e_20 + m_GPS_CH[ch].i_l_20 + m_GPS_CH[ch].i_p_20) > 0) {
			g_channel_bits |= (1 << ch);
		}

		// Flag that this bit is ready to process (written to the message_flag
		// in the tracking() function after we've gone through all the channels
		g_channels_with_bits |= (1 << ch);
		// }

		// Increment the time, in bits, since the week began. Used in
		// the measurement thread. Also set to the true time of
		// week when we get the TOW from a valid subframe in the
		// messages thread.
		m_GPS_CH[ch].time_in_bits++;
		if (m_GPS_CH[ch].time_in_bits >= BITS_IN_WEEK)
			m_GPS_CH[ch].time_in_bits -= BITS_IN_WEEK;

#ifdef GPS_FALSE_PHASE_LOCK_DETECTOR
		m_GPS_CH[ch].tot_fpld ++;
		if (m_GPS_CH[ch].tot_fpld > 50) {
			dcarr_phase = gps_cd1sec_FPLD(ch);
			if (dcarr_phase != 0) {
				m_GPS_CH[ch].carrier_freq += (dcarr_phase>0) ? (25/GPS_CARR_FREQ_RES):-(25/GPS_CARR_FREQ_RES);
				gps_set_carrier_dco_rate( ch, m_GPS_CH[ch].carrier_freq);
			}
		}
		m_GPS_CH[ch].i_p_20_1 = m_GPS_CH[ch].i_p_20;
		m_GPS_CH[ch].q_p_20_1 = m_GPS_CH[ch].q_p_20;
#endif // GPS_FALSE_PHASE_LOCK_DETECTOR

		// Clear coherent accumulations
		m_GPS_CH[ch].i_p_20 = 0;
		m_GPS_CH[ch].q_p_20 = 0;
		m_GPS_CH[ch].i_e_20 = 0;
		m_GPS_CH[ch].q_e_20 = 0;
		m_GPS_CH[ch].i_l_20 = 0;
		m_GPS_CH[ch].q_l_20  = 0;
#ifdef GPS_CODE_LOOP_20MS_RATE
#endif // GPS_CODE_LOOP_20MS_RATE
	}
}


/*******************************************************************************
 FUNCTION gps_accum_newdata(unsigned long new_data)
 RETURNS  None.

 PARAMETERS  unsigned long new_data		channel new data flags

 PURPOSE Grab new accumulation data for each GPS channel.
*******************************************************************************/
void gps_accum_newdata(unsigned long new_data)
{
	unsigned short ch, ch_index;

	// top of correlator block register map
	ch_index = GPS_CH00_BASE;

	// Sequentially check each channel for new data. 
	for (ch = 0; ch < GPS_MAX_CHANNELS; ch++) {
		// if (new_data & (1 << ch))
		if (new_data & (1 << ch)) {
			m_GPS_CH[ch].i_p_1 = m_GPS_CH[ch].i_p;
			m_GPS_CH[ch].q_p_1 = m_GPS_CH[ch].q_p;

			// The built-in function IORD is used to maintain thread-safe operations. (pjm)
#ifdef ENABLE_32BIT_ACCUMULATOR

			m_GPS_CH[ch].i_e = (signed long)(read_from_correlator( ch_index + I_EARLY ));
			m_GPS_CH[ch].q_e = (signed long)(read_from_correlator( ch_index + Q_EARLY ));
			m_GPS_CH[ch].i_p = (signed long)(read_from_correlator( ch_index + I_PROMPT ));
			m_GPS_CH[ch].q_p = (signed long)(read_from_correlator( ch_index + Q_PROMPT ));
			m_GPS_CH[ch].i_l = (signed long)(read_from_correlator( ch_index + I_LATE ));
			m_GPS_CH[ch].q_l = (signed long)(read_from_correlator( ch_index + Q_LATE ));

#else // ENABLE_32BIT_ACCUMULATOR

			m_GPS_CH[ch].i_e = (signed short)(read_from_correlator( ch_index + I_EARLY ));
			m_GPS_CH[ch].q_e = (signed short)(read_from_correlator( ch_index + Q_EARLY ));
			m_GPS_CH[ch].i_p = (signed short)(read_from_correlator( ch_index + I_PROMPT ));
			m_GPS_CH[ch].q_p = (signed short)(read_from_correlator( ch_index + Q_PROMPT ));
			m_GPS_CH[ch].i_l = (signed short)(read_from_correlator( ch_index + I_LATE ));
			m_GPS_CH[ch].q_l = (signed short)(read_from_correlator( ch_index + Q_LATE ));

#endif // ENABLE_32BIT_ACCUMULATOR

			// If the last dump was the first dump in a new satellite
			// message data bit, then lock() sets the load_1ms_epoch_flag
			// so that we can set the 1m epoch counter here. Why here?
			// GP4020 Baseband Processor Design Manual, pg 60: "Ideally,
			// epoch counter accesses should occur following the reading of
			// the accumulation register at each DUMP." Great, thanks for
			// the tip, now how 'bout you tell us WHY?!
			if (m_GPS_CH[ch].load_1ms_epoch_count) {
				// Load 1 ms epoch counter
				write_to_correlator( ch_index + EPOCH_LOAD, 1 );
				m_GPS_CH[ch].load_1ms_epoch_count = 0;
				m_GPS_CH[ch].epoch_codes = 1;
			}
			else {
				m_GPS_CH[ch].epoch_codes = read_from_correlator( ch_index + EPOCH_CHECK ) & 0x1F;
			}

			// We expect the 1ms epoch counter to always stay sync'd until
			// we lose lock. To sync the 20ms epoch counter (the upper bits)
			// we wait until we get a signal from the message thread that
			// we just got the TLM+HOW words; this means we're 60 bits into
			// the message. Since the damn epoch counter counts to *50* (?!)
			// we mod it with 60 which gives us 10 (0xA00 when shifted 8).
			if (m_GPS_CH[ch].sync_20ms_epoch_count) {
				// 0xA (60%50) shifted up 5 bits gives 0x140
				write_to_correlator( ch_index + EPOCH_LOAD, m_GPS_CH[ch].epoch_codes | 0x140 );
				m_GPS_CH[ch].sync_20ms_epoch_count = 0;
			}

			// lmag approximates sqrt(i^2 + q^2) for long's
			m_GPS_CH[ch].e_mag = lmag(m_GPS_CH[ch].i_e, m_GPS_CH[ch].q_e);
			m_GPS_CH[ch].p_mag = lmag(m_GPS_CH[ch].i_p, m_GPS_CH[ch].q_p);
			m_GPS_CH[ch].l_mag = lmag(m_GPS_CH[ch].i_l, m_GPS_CH[ch].q_l);
		}
		// increment channel index to top of next channel map
		ch_index += CH_BASE_STEP;
	}
}

/*******************************************************************************
 FUNCTION gps_track_channels(unsigned long new_data)
 RETURNS  None.

 PARAMETERS  unsigned long new_data		channel new data flags

 PURPOSE GPS channel signal acquisition and tracking main routine。
*******************************************************************************/
void gps_track_channels(unsigned long new_data)
{
	unsigned short ch;

	// Finally, in a second (slower) loop, track each channel that dumped. Note
	// that channels which are CHANNEL_OFF will be allocated satellites to
	// track in a mainline thread.
	for (ch = 0; ch < GPS_MAX_CHANNELS; ch++) {
		// if( (new_data & (1 << ch)) && (m_GPS_CH[ch].state != CHANNEL_OFF))
		// We already checked for dumped channels above, can  we somehow
		// avoid checking this again??
		if ((new_data & (1 << ch)) && (m_GPS_CH[ch].state != CHANNEL_OFF)) { // namuru (PJM)
			switch (m_GPS_CH[ch].state) {
			case CHANNEL_ACQUISITION:
				gps_acquire( ch);
				break;
			case CHANNEL_CONFIRM:
				gps_confirm( ch);
				break;
			case CHANNEL_FREQ_PULL:
				gps_freq_pull( ch);
				break;
			case CHANNEL_PULL_IN:
				gps_pull_in( ch);
				break;
			case CHANNEL_LOCK:
				if (m_GPS_CH[ch].backto_pull_in) {
					m_GPS_CH[ch].backto_pull_in = 0;
					gps_enter_pull_in(ch);
					gps_pull_in( ch);
				}
				else {
					gps_lock( ch);
				}
				break;
			default:
				// TODO: assert an error here
				break;
			}
		}

		// If the channel is off, set a flag saying so
		if (m_GPS_CH[ch].state == CHANNEL_OFF) {
			g_channels_to_allocate |= (1 << ch);
		}
	}
}

• 我的新浪博客账号
• 我的存档免费软件
• 我的存档学习资料