18、BDS B1I信号捕获和跟踪实现

2024-04-27 04:48
文章标签 实现 18 信号 捕获 跟踪 bds b1i

本文主要是介绍18、BDS B1I信号捕获和跟踪实现，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

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

// b1i_accum_task.c -- BDS B1I 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 and *            functions.* version  : V1.0* date     : xx/xx/2015*/#include <io.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>
#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 "b1i_accum_task.h"
#include "b1i_message.h"/******************************************************************************** #defines******************************************************************************/// m_B1I_CH[ch].ef_out = m_B1I_CH[ch].ef_out/(2*PI_SHIFT14*T), T=0.02 s
// m_B1I_CH[ch].ef_out = m_B1I_CH[ch].ef_out/((20-2)*B1I_CARR_FREQ_RES)
#define B1I_PULLIN_EFOUT_COF	(int)(0.5+0.72*PI_SHIFT14*B1I_CARR_FREQ_RES)/******************************************************************************** Global variables******************************************************************************/
b1i_chan_t m_B1I_CH[B1I_MAX_CHANNELS] __attribute__ ((section(".isrdata.rwdata")));unsigned short m_D1SecCode[20] __attribute__ ((section(".isrdata.rwdata"))) ={0,0,0,0,0,1,0,0,1,1,0,1,0,1,0,0,1,1,1,0};//
// ACCUM
///******************************************************************************** Static (module level) variables******************************************************************************/short m_B1iCarrSrchStep __attribute__ ((section(".isrdata.rwdata")));					// carry search step length
static unsigned short B1iCarrSrchWidth __attribute__ ((section(".isrdata.rwdata")));	// carry search widthstatic short B1iPullInTime __attribute__ ((section(".isrdata.rwdata")));
static short B1iPhaseTest __attribute__ ((section(".isrdata.rwdata")));/******************************************************************************** 以下环路滤波参数未初始化。引用该源文件代码可在此将它们初始化为适合目标基带模块参数的值。例如：* static long B1i_Pull_Carr_C0 __attribute__ ((section(".isrdata.rwdata"))) = 910;* * date: 17st/Sep/2021
******************************************************************************/
static long  B1i_Pull_Code_TtwoTone __attribute__ ((section(".isrdata.rwdata")));
static long  B1i_Pull_Code_DtTone __attribute__ ((section(".isrdata.rwdata")));
static long  D1_Lock_Code_TtwoTone __attribute__ ((section(".isrdata.rwdata")));
static long  D1_Lock_Code_DtTone __attribute__ ((section(".isrdata.rwdata")));
static long  D2_Lock_Code_TtwoTone __attribute__ ((section(".isrdata.rwdata")));
static long  D2_Lock_Code_DtTone __attribute__ ((section(".isrdata.rwdata")));
static long  B1i_Pull_Carr_C0 __attribute__ ((section(".isrdata.rwdata")));
static long  B1i_Pull_Carr_C1 __attribute__ ((section(".isrdata.rwdata")));
static long  B1i_Pull_Carr_C2 __attribute__ ((section(".isrdata.rwdata")));
static long  B1i_Lock_Carr_C0 __attribute__ ((section(".isrdata.rwdata")));
static long  B1i_Lock_Carr_C1 __attribute__ ((section(".isrdata.rwdata")));
static long  B1i_Lock_Carr_C2 __attribute__ ((section(".isrdata.rwdata")));/******************************************************************************** Prototypes (Local visible functions)******************************************************************************/static void b1i_backto_acquire( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void b1i_enter_pull_in( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void b1i_check_signal( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void b1i_dll( unsigned short ch, long dcode_freq, long TtwoTone, long DtTone) __attribute__ ((section(".isrcode.txt")));
static void b1i_pll1( unsigned short ch, long dcarr_phase, long dcarr_freq, long c0, long c1, long c2) __attribute__ ((section(".isrcode.txt")));
static void b1i_acquire( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void b1i_confirm( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void b1i_freq_pull( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void b1i_pull_in( unsigned short ch) __attribute__ ((section(".isrcode.txt")));
static void b1i_lock( unsigned short ch) __attribute__ ((section(".isrcode.txt")));#ifdef B1I_FALSE_PHASE_LOCK_DETECTOR
static short b1i_cd1sec_FPLD(unsigned short ch) __attribute__ ((section(".isrcode.txt")));
#endif // B1I_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 b1i_set_code_dco_rate( unsigned short ch, unsigned long freq)
{write_to_correlator( (B1I_CH00_BASE + ch * CH_BASE_STEP + CODE_NCO), freq );
}inline void b1i_set_carrier_dco_rate( unsigned short ch, unsigned long freq)
{write_to_correlator( (B1I_CH00_BASE + ch * CH_BASE_STEP + CARRIER_NCO), freq );
}/** False Phase Lock Detector (FPLD)
*/#ifdef B1I_FALSE_PHASE_LOCK_DETECTOR// FPLD defines
#define FPLD_G_THRESHOLD		(PI_SHIFT14 / 12)	// 15 deg.static short b1i_cd1sec_FPLD(unsigned short ch)
{signed long i_now = m_B1I_CH[ch].i_p_20 >> 4;signed long q_now = m_B1I_CH[ch].q_p_20 >> 4;signed long i_old = m_B1I_CH[ch].i_p_20_1 >> 4;signed long q_old = m_B1I_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_B1I_CH[ch].cnt_fpld == 0) {m_B1I_CH[ch].c_fpld = 0;m_B1I_CH[ch].d_fpld = 0;}m_B1I_CH[ch].cnt_fpld ++;m_B1I_CH[ch].c_fpld += (C - m_B1I_CH[ch].c_fpld)/m_B1I_CH[ch].cnt_fpld;m_B1I_CH[ch].d_fpld += (D - m_B1I_CH[ch].d_fpld)/m_B1I_CH[ch].cnt_fpld;if (m_B1I_CH[ch].cnt_fpld == 50) {m_B1I_CH[ch].cnt_fpld = 0;C = fix_atan2(m_B1I_CH[ch].c_fpld, m_B1I_CH[ch].d_fpld);if (labs(C) > FPLD_G_THRESHOLD) {return C;}}return 0;
}#endif // B1I_FALSE_PHASE_LOCK_DETECTOR/******************************************************************************* Need to set up m_B1I_CH[] structure and initialize the loop dynamic parameters.******************************************************************************/
void b1i_initialize_tracking( void)
{unsigned short ch;// Why are these a good choices?B1iCarrSrchWidth = 40;    // search 20 frequency steps on either sidem_B1iCarrSrchStep = (short)(400./B1I_CARR_FREQ_RES); // 400Hz (CHD)B1iPullInTime = 1000;		// 1 secondB1iPhaseTest  = 500;		// last 1/2 second of pull in, start phase testfor (ch = 0; ch < B1I_MAX_CHANNELS; ch++) {m_B1I_CH[ch].state = CHANNEL_OFF;}
}static void b1i_backto_acquire( unsigned short ch)
{m_B1I_CH[ch].state = CHANNEL_ACQUISITION;// flag the message_thread that the past subframes are no longer validb1i_clear_messages(ch);m_B1I_CH[ch].codes = 0;m_B1I_CH[ch].code_freq = B1I_CODE_REF;b1i_set_code_dco_rate(ch, m_B1I_CH[ch].code_freq);// Clear sync flagsm_B1I_CH[ch].bit_sync = 0;
}void b1i_enter_pull_in( unsigned short ch)
{m_B1I_CH[ch].state = CHANNEL_PULL_IN;m_B1I_CH[ch].ch_time = 0;m_B1I_CH[ch].th_rms = 0;m_B1I_CH[ch].bit_sync = 0;m_B1I_CH[ch].dcode_freq_1 = 0;m_B1I_CH[ch].dcarr_phase_1 = 0;m_B1I_CH[ch].dcarr_phase_2 = 0;// Some garbage datam_B1I_CH[ch].ms_sign = 0x12345;m_B1I_CH[ch].ms_count = 0;m_B1I_CH[ch].ms_count_20 = 0;m_B1I_CH[ch].tang = 0;
}static void b1i_check_signal( unsigned short ch)
{// Amplitude Tang decider, exit if the amplitude is always small.if (m_B1I_CH[ch].p_mag<TANG_THRESHOLD) {m_B1I_CH[ch].tang-=3;}else if (m_B1I_CH[ch].tang<90) {m_B1I_CH[ch].tang+=1;}if (m_B1I_CH[ch].tang<-30) {b1i_backto_acquire(ch);}
}static void b1i_dll( unsigned short ch, long dcode_freq, long TtwoTone, long DtTone)
{long ddf = (dcode_freq - m_B1I_CH[ch].dcode_freq_1) * TtwoTone;m_B1I_CH[ch].code_freq += (dcode_freq * DtTone + ddf) >> 14;b1i_set_code_dco_rate( ch, m_B1I_CH[ch].code_freq);m_B1I_CH[ch].dcode_freq_1 = dcode_freq;
}static void b1i_pll1( unsigned short ch, long dcarr_phase, long dcarr_freq, long c0, long c1, long c2)
{long ddcar = dcarr_phase*c0 + m_B1I_CH[ch].dcarr_phase_1*c1 + dcarr_freq*c2;m_B1I_CH[ch].carrier_freq += ddcar >> 14;b1i_set_carrier_dco_rate( ch, m_B1I_CH[ch].carrier_freq);m_B1I_CH[ch].dcarr_phase_1 = dcarr_phase;
}/******************************************************************************FUNCTION b1i_acquire( unsigned short ch)RETURNS  None.PARAMETERSch  char // Which correlator channel to usePURPOSE  to perform initial b1i_acquire by searching code and frequency spacelooking for a high correlationOriginal function : acquireWRITTEN BYClifford KelleyModified by Peter Mumford for namuru HW (2006)Modified by Cheng Huaide for HD-GR GNSS (2015)******************************************************************************/
static void b1i_acquire( unsigned short ch)
{// Search carrier frequency binsif (abs(m_B1I_CH[ch].n_freq) <= B1iCarrSrchWidth) {long power;power = m_B1I_CH[ch].e_mag + m_B1I_CH[ch].p_mag + m_B1I_CH[ch].l_mag;if (power > ACQ_THRESHOLD) {m_B1I_CH[ch].state = CHANNEL_CONFIRM;m_B1I_CH[ch].n_confirm = 0;m_B1I_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(B1I_CH00_BASE + ch * CH_BASE_STEP + CODE_SLEW, 1);// Keep count of how many code phases we've searchedm_B1I_CH[ch].codes++;// if (m_B1I_CH[ch].codes > 2044) // PRN code length in 1/2 chips// All code offsets have been searched; try another frequency binif (m_B1I_CH[ch].codes > 4091) {// reset code phase countm_B1I_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_B1I_CH[ch].n_freq & 1) { // Odd search bins map to the "right"m_B1I_CH[ch].carrier_freq = B1I_CARRIER_REF + m_B1I_CH[ch].carrier_corr +m_B1iCarrSrchStep * (1 + (m_B1I_CH[ch].n_freq >> 1));}else { // Even search bins are to the "left" of B1I_CARRIER_REFm_B1I_CH[ch].carrier_freq = B1I_CARRIER_REF + m_B1I_CH[ch].carrier_corr -m_B1iCarrSrchStep * (m_B1I_CH[ch].n_freq >> 1);}// Set carrier DCOb1i_set_carrier_dco_rate(ch, m_B1I_CH[ch].carrier_freq);m_B1I_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 channelm_B1I_CH[ch].state = CHANNEL_OFF;}
}/*******************************************************************************FUNCTION b1i_confirm(unsigned short ch)RETURNS  None.PARAMETERSch  char  channel numberPURPOSE  to b1i_lock the presence of a high correlation peak using an n of malgorithmOriginal function : confirmWRITTEN BYClifford KelleyModified by Peter Mumford for namuru HW (2006)Modified by Cheng Huaide for HD-GR GNSS (2015)
*******************************************************************************/
static void b1i_confirm( unsigned short ch)
{long power;// count number of b1i_confirm attemptsm_B1I_CH[ch].n_confirm++;power = m_B1I_CH[ch].e_mag + m_B1I_CH[ch].p_mag + m_B1I_CH[ch].l_mag;if (power > ACQ_THRESHOLD) {// count number of good hitsm_B1I_CH[ch].m_thresh++;}// try "n" b1i_confirm attemptsif (m_B1I_CH[ch].n_confirm > 10) {// confirmed if good hits >= "m"if (m_B1I_CH[ch].m_thresh >= 8) {m_B1I_CH[ch].state = CHANNEL_FREQ_PULL;m_B1I_CH[ch].ch_time = 0;m_B1I_CH[ch].th_rms = 0;m_B1I_CH[ch].bit_sync = 0;m_B1I_CH[ch].dcode_freq_1 = 0;m_B1I_CH[ch].dcarr_phase_1 = 0;m_B1I_CH[ch].dcarr_phase_2 = 0;// Some garbage datam_B1I_CH[ch].ms_sign = 0x12345;m_B1I_CH[ch].ms_count = 0;m_B1I_CH[ch].tang = 0;m_B1I_CH[ch].ef_out = 0;m_B1I_CH[ch].ef_max = -0x7fffffff;m_B1I_CH[ch].ef_min = 0x7fffffff;}else {// Keep searching - assumes search parameters are still okm_B1I_CH[ch].state = CHANNEL_ACQUISITION;// Clear sync flagsm_B1I_CH[ch].bit_sync = 0;}}
}/*******************************************************************************FUNCTION b1i_freq_pull(unsigned short ch)RETURNS  None.PARAMETERSch  char  channel numberPURPOSE  to adjust the frequency of the acquired signal using a FLL.
*******************************************************************************/
static void b1i_freq_pull( unsigned short ch)
{b1i_check_signal(ch);if (m_B1I_CH[ch].state != CHANNEL_FREQ_PULL) {return;}// Calculate frequency errorsigned long C = m_B1I_CH[ch].i_p_1*m_B1I_CH[ch].q_p - m_B1I_CH[ch].q_p_1*m_B1I_CH[ch].i_p;signed long D = m_B1I_CH[ch].i_p_1*m_B1I_CH[ch].i_p + m_B1I_CH[ch].q_p_1*m_B1I_CH[ch].q_p;signed long df = fix_atan2(C, D);// Accumulate frequency errorm_B1I_CH[ch].ef_out += df;if (m_B1I_CH[ch].ef_max < df) m_B1I_CH[ch].ef_max = df;if (m_B1I_CH[ch].ef_min > df) m_B1I_CH[ch].ef_min = df;m_B1I_CH[ch].ms_count ++;if (m_B1I_CH[ch].ms_count == 20) {// Set carrier NCOdf = (m_B1I_CH[ch].ef_out - (m_B1I_CH[ch].ef_max + m_B1I_CH[ch].ef_min))/B1I_PULLIN_EFOUT_COF;m_B1I_CH[ch].carrier_freq += df;b1i_set_carrier_dco_rate( ch, m_B1I_CH[ch].carrier_freq);b1i_enter_pull_in(ch);}
}/*******************************************************************************FUNCTION b1i_pull_in( unsigned short ch)RETURNS  None.PARAMETERSch  char  channel numberPURPOSEpull in the frequency by trying to track the signal with acombination FLL and PLLit will attempt to track for xxx ms, the last xxx ms of data will begathered to determine if we have both code and carrier b1i_lockif so we will transition to trackOriginal function : pull_inWRITTEN BYClifford KelleyModified by Peter Mumford for namuru HW (2006)Modified by Cheng Huaide for HD-GR GNSS (2015)
*******************************************************************************/
static void b1i_pull_in( unsigned short ch)
{b1i_check_signal(ch);if (m_B1I_CH[ch].state != CHANNEL_PULL_IN) {return;}signed long C, D, M;signed long i_sum, q_sum;signed long dcode_freq, dcarr_phase, dcarr_freq;unsigned short check_bit_sync = 0;// This branch is probably almost always taken, so maybe skip the test?// If both zero the correction is not zero (2nd order) except this test// makes it so, which is a kink in the transfer function. Is this right?if ((m_B1I_CH[ch].e_mag != 0) || (m_B1I_CH[ch].l_mag != 0)) {dcode_freq = ((m_B1I_CH[ch].e_mag - m_B1I_CH[ch].l_mag)<<14) /(m_B1I_CH[ch].e_mag + m_B1I_CH[ch].l_mag);if (m_B1I_CH[ch].ch_time <= 2) {m_B1I_CH[ch].dcode_freq_1 = dcode_freq;}else {b1i_dll(ch, dcode_freq, B1i_Pull_Code_TtwoTone, B1i_Pull_Code_DtTone);}}// dcarr_phase is a measure of phase errori_sum = m_B1I_CH[ch].i_p + m_B1I_CH[ch].i_e + m_B1I_CH[ch].i_l;q_sum = m_B1I_CH[ch].q_p + m_B1I_CH[ch].q_e + m_B1I_CH[ch].q_l;if (i_sum || q_sum) {dcarr_phase = fix_atan(q_sum, i_sum);}else {dcarr_phase = m_B1I_CH[ch].dcarr_phase_1;}// Increase 1 ms epoch counter modulo 20 or 2m_B1I_CH[ch].ms_count++;if (m_B1I_CH[ch].ms_count > m_B1I_CH[ch].ms_maxval) {m_B1I_CH[ch].ms_count = 0;}m_B1I_CH[ch].ms_count_20++;if (m_B1I_CH[ch].ms_count_20 > 19) {m_B1I_CH[ch].ms_count_20 = 0;}// Check if the last 20 ms (2 ms) have the same sign and this dump// is different: if so, then we just had a bit edge transitionif (!m_B1I_CH[ch].bit_sync) {unsigned long ms_sign;unsigned short bit_sign = (i_sum < 0) ? 1:0;if (IS_D1_NAVMESSAGE(m_B1I_CH[ch].prn)) {// 剥离二次编码ms_sign = (m_B1I_CH[ch].ms_sign ^ D1_SECONDARY_CODE);// 如果符号发生变化且ms_sign的20位符号不变if ((bit_sign != (ms_sign & 1)) &&(ms_sign == 0 || ms_sign == 0xfffff)) {check_bit_sync = 1;}}else {// 如果符号发生变化if (bit_sign != (m_B1I_CH[ch].ms_sign & 1)) {// 计算差分值: 如果所有奇数位为0，则位同步ms_sign = m_B1I_CH[ch].ms_sign ^ (m_B1I_CH[ch].ms_sign << 1);if ((ms_sign & 0xaaaaa) == 0) {check_bit_sync = 1;}}}}// Shift sign buffer to leftm_B1I_CH[ch].ms_sign = ((m_B1I_CH[ch].ms_sign << 1) & 0xfffff);// Set the LSB bit if negativeif (i_sum < 0) {m_B1I_CH[ch].ms_sign |= 1;}if (check_bit_sync) {// Test if last two sums were within 1/4 radian of pi/2// 	4096 correponds to 1/4 of radian or aobut 14 degree. This is a strict//	requirement, since we could accept up to 20 degree phase error.//	20 degree corresponds to 5719if ((labs( dcarr_phase) < 5719) &&				// 4096(labs( m_B1I_CH[ch].dcarr_phase_1) < 5719)) {	// 4096// Let the world know we're synced to the satellite message bitsm_B1I_CH[ch].bit_sync = 1;// sync the ms count to the bit streamm_B1I_CH[ch].ms_count = 0;m_B1I_CH[ch].ms_count_20 = 0;// 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_B1I_CH[ch].load_1ms_epoch_count = 1;}}// Near the end of pull in, start the phase testif (m_B1I_CH[ch].ch_time > (B1iPullInTime - B1iPhaseTest)) {m_B1I_CH[ch].th_rms += (dcarr_phase * dcarr_phase) >> 14;}// b1i_pull_in Carrier tracking loopif (m_B1I_CH[ch].ch_time <= 5) {m_B1I_CH[ch].dcarr_phase_2 = m_B1I_CH[ch].dcarr_phase_1;m_B1I_CH[ch].dcarr_phase_1 = dcarr_phase;}else {C = (m_B1I_CH[ch].i_p_1*m_B1I_CH[ch].q_p - m_B1I_CH[ch].q_p_1*m_B1I_CH[ch].i_p);D = (m_B1I_CH[ch].i_p_1*m_B1I_CH[ch].i_p + m_B1I_CH[ch].q_p_1*m_B1I_CH[ch].q_p);M = lmag(C,D);dcarr_freq = (D >= 0) ? (C<<14)/M:-(C<<14)/M;b1i_pll1(ch, dcarr_phase, dcarr_freq, B1i_Pull_Carr_C0, B1i_Pull_Carr_C1, B1i_Pull_Carr_C2);}m_B1I_CH[ch].ch_time++;// Done with pull in. Wait until the end of a data bit.// Are we sure we even think we're data-locked now?// A bit transition will happen at the next dump.if ((m_B1I_CH[ch].ms_count == m_B1I_CH[ch].ms_maxval) &&(m_B1I_CH[ch].ch_time >= B1iPullInTime)) {// 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.// m_B1I_CH[ch].th_rms = fix_sqrt( m_B1I_CH[ch].th_rms / B1iPhaseTest);// Sufficient signal, transition to tracking mode// (12000*12000 >> 14) = 8789.0625// if (m_B1I_CH[ch].bit_sync && m_B1I_CH[ch].th_rms < 12000) {if (m_B1I_CH[ch].bit_sync && m_B1I_CH[ch].th_rms < (8789*B1iPhaseTest)) {// Bit edge was detected.// Sufficient signal, transition to tracking modem_B1I_CH[ch].i_p_20 = 0;m_B1I_CH[ch].i_e_20 = 0;m_B1I_CH[ch].i_l_20 = 0;m_B1I_CH[ch].q_p_20 = 0;m_B1I_CH[ch].q_e_20 = 0;m_B1I_CH[ch].q_l_20 = 0;
#ifdef B1I_CODE_LOOP_20MS_RATE
#endif // B1I_CODE_LOOP_20MS_RATE#ifdef B1I_FALSE_PHASE_LOCK_DETECTORm_B1I_CH[ch].i_p_20_1 = 0;m_B1I_CH[ch].q_p_20_1 = 0;m_B1I_CH[ch].c_fpld = 0;m_B1I_CH[ch].d_fpld = 0;m_B1I_CH[ch].cnt_fpld = 0;m_B1I_CH[ch].tot_fpld = 0;
#endif // B1I_FALSE_PHASE_LOCK_DETECTOR// Officially switch modesm_B1I_CH[ch].state = CHANNEL_LOCK;}else {// We lost the pullin. Eventually, do a nice transition back to// b1i_confirm and/or b1i_acquire. For now, to be paranoid, just kill// the channel.b1i_backto_acquire(ch);}}
}/*******************************************************************************FUNCTION b1i_lock( unsigned short ch)RETURNS  None.PARAMETERS  char ch  , channel numberPURPOSE track carrier and code, and partially decode the navigation message(to determine TOW, subframe etc.)Original function : lockWRITTEN BYClifford Kelleyadded Carrier Aiding as suggested by Jenna Cheng, UCRModified by Peter Mumford for namuru HW (2006)Modified by Cheng Huaide for HD-GR GNSS (2015)
*******************************************************************************/
static void b1i_lock( unsigned short ch)
{b1i_check_signal(ch);if (m_B1I_CH[ch].state != CHANNEL_LOCK) {return;}signed long C, D, M;signed long i_sum, q_sum;signed long dcode_freq, dcarr_phase, dcarr_freq;unsigned short D1Msg = IS_D1_NAVMESSAGE(m_B1I_CH[ch].prn);// Check and correct ms_count_20 according to epoch_codesif (m_B1I_CH[ch].epoch_codes < m_B1I_CH[ch].ms_count_20 && m_B1I_CH[ch].ms_count_20 != 19) {m_B1I_CH[ch].ms_count_20 = 19;}else {m_B1I_CH[ch].ms_count_20 = m_B1I_CH[ch].epoch_codes;}// Increment the time, in 20ms, 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.if (m_B1I_CH[ch].ms_count_20 == 19) {m_B1I_CH[ch].time_in_bits++;if (m_B1I_CH[ch].time_in_bits >= BITS_IN_WEEK_50HZ)m_B1I_CH[ch].time_in_bits -= BITS_IN_WEEK_50HZ;}// Update ms_countm_B1I_CH[ch].ms_count = D1Msg ? (m_B1I_CH[ch].ms_count_20):(m_B1I_CH[ch].epoch_codes & 1);// Carrier loopi_sum = m_B1I_CH[ch].i_p + m_B1I_CH[ch].i_e + m_B1I_CH[ch].i_l;q_sum = m_B1I_CH[ch].q_p + m_B1I_CH[ch].q_e + m_B1I_CH[ch].q_l;if ((i_sum != 0) || (q_sum != 0)) {C = (m_B1I_CH[ch].i_p_1*m_B1I_CH[ch].q_p - m_B1I_CH[ch].q_p_1*m_B1I_CH[ch].i_p);D = (m_B1I_CH[ch].i_p_1*m_B1I_CH[ch].i_p + m_B1I_CH[ch].q_p_1*m_B1I_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);b1i_pll1(ch, dcarr_phase, dcarr_freq, B1i_Lock_Carr_C0, B1i_Lock_Carr_C1, B1i_Lock_Carr_C2);}// 20ms or 2ms accumulatorif (D1Msg && m_D1SecCode[m_B1I_CH[ch].ms_count]) {m_B1I_CH[ch].i_p_20 -= m_B1I_CH[ch].i_p;m_B1I_CH[ch].i_e_20 -= m_B1I_CH[ch].i_e;m_B1I_CH[ch].i_l_20 -= m_B1I_CH[ch].i_l;m_B1I_CH[ch].q_p_20 -= m_B1I_CH[ch].q_p;m_B1I_CH[ch].q_e_20 -= m_B1I_CH[ch].q_e;m_B1I_CH[ch].q_l_20 -= m_B1I_CH[ch].q_l;}else {m_B1I_CH[ch].i_p_20 += m_B1I_CH[ch].i_p;m_B1I_CH[ch].i_e_20 += m_B1I_CH[ch].i_e;m_B1I_CH[ch].i_l_20 += m_B1I_CH[ch].i_l;m_B1I_CH[ch].q_p_20 += m_B1I_CH[ch].q_p;m_B1I_CH[ch].q_e_20 += m_B1I_CH[ch].q_e;m_B1I_CH[ch].q_l_20 += m_B1I_CH[ch].q_l;}#ifndef B1I_CODE_LOOP_20MS_RATE// Code tracking loop @ 1ms ratedcode_freq = m_B1I_CH[ch].e_mag + m_B1I_CH[ch].l_mag;if (dcode_freq != 0) {dcode_freq = ((m_B1I_CH[ch].e_mag - m_B1I_CH[ch].l_mag)<<14)/dcode_freq;b1i_dll(ch, dcode_freq, B1i_Pull_Code_TtwoTone, B1i_Pull_Code_DtTone);}
#endif // B1I_CODE_LOOP_20MS_RATE// Code tracking loop @ 20ms rateif (m_B1I_CH[ch].ms_count == m_B1I_CH[ch].ms_maxval) {/* Deleted by CHD -- 2020.4.29if (!m_B1I_CH[ch].bit_sync) {b1i_backto_acquire(ch);return;}Deleted by CHD -- 2020.4.29 */#ifdef B1I_CODE_LOOP_20MS_RATE// Code tracking loop @ 20ms ratesigned long e_mag_20 = lmag( m_B1I_CH[ch].i_e_20, m_B1I_CH[ch].q_e_20);signed long l_mag_20 = lmag( m_B1I_CH[ch].i_l_20, m_B1I_CH[ch].q_l_20);dcode_freq = e_mag_20 + l_mag_20;if (dcode_freq != 0) {dcode_freq = ((e_mag_20 - l_mag_20) << 14)/dcode_freq;if (D1Msg) {b1i_dll(ch, dcode_freq, D1_Lock_Code_TtwoTone, D1_Lock_Code_DtTone);}else {b1i_dll(ch, dcode_freq, D2_Lock_Code_TtwoTone, D2_Lock_Code_DtTone);}}#endif // B1I_CODE_LOOP_20MS_RATE// Data bit// m_B1I_CH[ch].bit = ((m_B1I_CH[ch].i_e_20 + m_B1I_CH[ch].i_l_20 + m_B1I_CH[ch].i_p_20) > 0);if ((m_B1I_CH[ch].i_e_20 + m_B1I_CH[ch].i_l_20 + m_B1I_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
#ifdef B1I_SYNC_PROCESS_FRAME_SYNCif (m_messages[ch].frame_sync) {g_channels_with_bits |= (1 << ch);}else {//	sync_frame(ch, m_B1I_CH[ch].bit);sync_frame(ch, (g_channel_bits & (1 << ch)) ? 1:0);}
#elseg_channels_with_bits |= (1 << ch);
#endif // B1I_SYNC_PROCESS_FRAME_SYNC#ifdef B1I_FALSE_PHASE_LOCK_DETECTORif (D1Msg) {m_B1I_CH[ch].tot_fpld ++;if (m_B1I_CH[ch].tot_fpld > 50) {dcarr_phase = b1i_cd1sec_FPLD(ch);if (dcarr_phase != 0) {m_B1I_CH[ch].carrier_freq += (dcarr_phase>0) ? (25/B1I_CARR_FREQ_RES):-(25/B1I_CARR_FREQ_RES);b1i_set_carrier_dco_rate( ch, m_B1I_CH[ch].carrier_freq);}}m_B1I_CH[ch].i_p_20_1 = m_B1I_CH[ch].i_p_20;m_B1I_CH[ch].q_p_20_1 = m_B1I_CH[ch].q_p_20;}
#endif // B1I_FALSE_PHASE_LOCK_DETECTOR// Clear coherent accumulationsm_B1I_CH[ch].i_p_20 = 0;m_B1I_CH[ch].i_e_20 = 0;m_B1I_CH[ch].i_l_20 = 0;m_B1I_CH[ch].q_p_20 = 0;m_B1I_CH[ch].q_e_20 = 0;m_B1I_CH[ch].q_l_20 = 0;}
}/*******************************************************************************FUNCTION b1i_accum_newdata(unsigned long new_data)RETURNS  None.PARAMETERS  unsigned long new_data		channel new data flagsPURPOSE Grab new accumulation data for each BDS channel.
*******************************************************************************/
void b1i_accum_newdata(unsigned long new_data)
{unsigned short ch, ch_index;// top of correlator block register mapch_index = B1I_CH00_BASE;// Sequentially check each channel for new data. for (ch = 0; ch < B1I_MAX_CHANNELS; ch++) {// if (new_data & (1 << ch))if (new_data & (1 << ch)) {m_B1I_CH[ch].i_p_1 = m_B1I_CH[ch].i_p;m_B1I_CH[ch].q_p_1 = m_B1I_CH[ch].q_p;// The built-in function IORD is used to maintain thread-safe operations. (pjm)
#ifdef ENABLE_32BIT_ACCUMULATORm_B1I_CH[ch].i_e = (signed long)(read_from_correlator( ch_index + I_EARLY ));m_B1I_CH[ch].q_e = (signed long)(read_from_correlator( ch_index + Q_EARLY ));m_B1I_CH[ch].i_p = (signed long)(read_from_correlator( ch_index + I_PROMPT ));m_B1I_CH[ch].q_p = (signed long)(read_from_correlator( ch_index + Q_PROMPT ));m_B1I_CH[ch].i_l = (signed long)(read_from_correlator( ch_index + I_LATE ));m_B1I_CH[ch].q_l = (signed long)(read_from_correlator( ch_index + Q_LATE ));#else // ENABLE_32BIT_ACCUMULATORm_B1I_CH[ch].i_e = (signed short)(read_from_correlator( ch_index + I_EARLY ));m_B1I_CH[ch].q_e = (signed short)(read_from_correlator( ch_index + Q_EARLY ));m_B1I_CH[ch].i_p = (signed short)(read_from_correlator( ch_index + I_PROMPT ));m_B1I_CH[ch].q_p = (signed short)(read_from_correlator( ch_index + Q_PROMPT ));m_B1I_CH[ch].i_l = (signed short)(read_from_correlator( ch_index + I_LATE ));m_B1I_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 b1i_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_B1I_CH[ch].load_1ms_epoch_count) {// Load 1 ms epoch counterwrite_to_correlator( ch_index + EPOCH_LOAD, 1 );m_B1I_CH[ch].epoch_codes = 1;m_B1I_CH[ch].load_1ms_epoch_count = 0;}else {m_B1I_CH[ch].epoch_codes = read_from_correlator( ch_index + EPOCH_CHECK ) & 0x1F;}// 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+SOW words.if (m_B1I_CH[ch].sync_20ms_epoch_count) {unsigned long epoch_temp;// 对于D1电文, 我们希望1ms epoch counter在信号失锁之前, 总是保持// 同步的. 当我们获得TLM+SOW字时, 刚好位于当前子帧的第60-bits,// 所以将它余50得10, 再左移5位得0x140.if (IS_D1_NAVMESSAGE(m_B1I_CH[ch].prn)) {epoch_temp = m_B1I_CH[ch].epoch_codes | 0x140;}// 对于D2电文, 1ms epoch counter的起点在位同步时只对齐了D2电文的// 2ms-位, 并没有与20ms(时钟)起点对齐, 此时才有机会完成它.//	1.当我们获得TLM+SOW字时, 刚好获得了当前页的第60-bits, 对应第//  120ms, 为20ms的倍数;//  2.如果对帧同步进行同步处理(在跟踪过程中直接调用sync_frame函数且这//	之后没有丢失累积数据), 则本次处理对应的是设置帧同步参数//  (sync_20ms_epoch_count)后的第1个累积数据, 因此应有：//	  (1) 1ms epoch counter = 0;//	  (2) 20ms epoch counter = (sync_20ms_epoch_count % 50) << 5.else {epoch_temp = ((m_B1I_CH[ch].sync_20ms_epoch_count % 50) << 5);m_B1I_CH[ch].epoch_codes = 0;}write_to_correlator( ch_index + EPOCH_LOAD, epoch_temp);m_B1I_CH[ch].sync_20ms_epoch_count = 0;}// lmag approximates sqrt(i^2 + q^2) for long'sm_B1I_CH[ch].e_mag = lmag(m_B1I_CH[ch].i_e, m_B1I_CH[ch].q_e);m_B1I_CH[ch].p_mag = lmag(m_B1I_CH[ch].i_p, m_B1I_CH[ch].q_p);m_B1I_CH[ch].l_mag = lmag(m_B1I_CH[ch].i_l, m_B1I_CH[ch].q_l);}// increment channel index to top of next channel mapch_index += CH_BASE_STEP;}
}/*******************************************************************************FUNCTION b1i_track_channels(unsigned long new_data)RETURNS  None.PARAMETERS  unsigned long new_data		channel new data flagsPURPOSE BDS channel signal acquisition and tracking main routine。
*******************************************************************************/
void b1i_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 < B1I_MAX_CHANNELS; ch++) {// if( (new_data & (1 << ch)) && (m_B1I_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_B1I_CH[ch].state != CHANNEL_OFF)) { // namuru (PJM)switch (m_B1I_CH[ch].state) {case CHANNEL_ACQUISITION:b1i_acquire( ch);break;case CHANNEL_CONFIRM:b1i_confirm( ch);break;case CHANNEL_FREQ_PULL:b1i_freq_pull( ch);break;case CHANNEL_PULL_IN:b1i_pull_in( ch);break;case CHANNEL_LOCK:if (m_B1I_CH[ch].backto_pull_in) {m_B1I_CH[ch].backto_pull_in = 0;b1i_enter_pull_in(ch);b1i_pull_in( ch);}else {b1i_lock( ch);}break;default:// TODO: assert an error herebreak;}}// If the channel is off, set a flag saying soif (m_B1I_CH[ch].state == CHANNEL_OFF) {g_channels_to_allocate |= (1 << ch);}}
}
  • 我的新浪博客账号
  • 我的存档免费软件
  • 我的存档学习资料

这篇关于18、BDS B1I信号捕获和跟踪实现的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/939559。

相关文章

hdu1043（八数码问题，广搜 + hash（实现状态压缩） ）

hdu1043（八数码问题，广搜 + hash（实现状态压缩） ）

利用康拓展开将一个排列映射成一个自然数，然后就变成了普通的广搜题。 #include<iostream>#include<algorithm>#include<string>#include<stack>#include<queue>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#inclu
阅读更多...
【C++】_list常用方法解析及模拟实现

【C++】_list常用方法解析及模拟实现

相信自己的力量，只要对自己始终保持信心，尽自己最大努力去完成任何事，就算事情最终结果是失败了，努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一：什么是list？ •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二：list常用接口 •🌰1.默认成员函数 🔥构造函数（⭐） 🔥析构函数 •🌰2.list对象
阅读更多...
【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

✨✨ 欢迎大家来到景天科技苑✨✨ 🎈🎈 养成好习惯，先赞后看哦~🎈🎈 🏆 作者简介：景天科技苑 🏆《头衔》：大厂架构师，华为云开发者社区专家博主，阿里云开发者社区专家博主，CSDN全栈领域优质创作者，掘金优秀博主，51CTO博客专家等。 🏆《博客》：Python全栈，前后端开发，小程序开发，人工智能，js逆向，App逆向，网络系统安全，数据分析，Django，fastapi
阅读更多...
让树莓派智能语音助手实现定时提醒功能

让树莓派智能语音助手实现定时提醒功能

最初的时候是想直接在rasa 的chatbot上实现，因为rasa本身是带有remindschedule模块的。不过经过一番折腾后，忽然发现，chatbot上实现的定时，语音助手不一定会有响应。因为，我目前语音助手的代码设置了长时间无应答会结束对话，这样一来，chatbot定时提醒的触发就不会被语音助手获悉。那怎么让语音助手也具有定时提醒功能呢？ 我最后选择的方法是用threading.Time
阅读更多...
Android实现任意版本设置默认的锁屏壁纸和桌面壁纸（两张壁纸可不一致）

Android实现任意版本设置默认的锁屏壁纸和桌面壁纸（两张壁纸可不一致）

客户有些需求需要设置默认壁纸和锁屏壁纸  在默认情况下 这两个壁纸是相同的  如果需要默认的锁屏壁纸和桌面壁纸不一样 需要额外修改 Android13实现 替换默认桌面壁纸： 将图片文件替换frameworks/base/core/res/res/drawable-nodpi/default_wallpaper.*  （注意不能是bmp格式） 替换默认锁屏壁纸： 将图片资源放入vendo
阅读更多...
C#实战|大乐透选号器[6]：实现实时显示已选择的红蓝球数量

C#实战|大乐透选号器[6]：实现实时显示已选择的红蓝球数量

哈喽，你好啊，我是雷工。 关于大乐透选号器在前面已经记录了5篇笔记，这是第6篇； 接下来实现实时显示当前选中红球数量，蓝球数量； 以下为练习笔记。 01 效果演示 当选择和取消选择红球或蓝球时，在对应的位置显示实时已选择的红球、蓝球的数量； 02 标签名称 分别设置Label标签名称为：lblRedCount、lblBlueCount
阅读更多...
Kubernetes PodSecurityPolicy：PSP能实现的5种主要安全策略

Kubernetes PodSecurityPolicy：PSP能实现的5种主要安全策略

Kubernetes PodSecurityPolicy：PSP能实现的5种主要安全策略 1. 特权模式限制2. 宿主机资源隔离3. 用户和组管理4. 权限提升控制5. SELinux配置 💖The Begin💖点点关注，收藏不迷路💖 Kubernetes的PodSecurityPolicy（PSP）是一个关键的安全特性，它在Pod创建之前实施安全策略，确保P
阅读更多...
工厂ERP管理系统实现源码（JAVA）

工厂ERP管理系统实现源码（JAVA）

工厂进销存管理系统是一个集采购管理、仓库管理、生产管理和销售管理于一体的综合解决方案。该系统旨在帮助企业优化流程、提高效率、降低成本，并实时掌握各环节的运营状况。 在采购管理方面，系统能够处理采购订单、供应商管理和采购入库等流程，确保采购过程的透明和高效。仓库管理方面，实现库存的精准管理，包括入库、出库、盘点等操作，确保库存数据的准确性和实时性。 生产管理模块则涵盖了生产计划制定、物料需求计划、
阅读更多...
C++——stack、queue的实现及deque的介绍

C++——stack、queue的实现及deque的介绍

目录 1.stack与queue的实现 1.1stack的实现  1.2 queue的实现 2.重温vector、list、stack、queue的介绍 2.1 STL标准库中stack和queue的底层结构  3.deque的简单介绍 3.1为什么选择deque作为stack和queue的底层默认容器  3.2 STL中对stack与queue的模拟实现 ①stack模拟实现
阅读更多...
基于51单片机的自动转向修复系统的设计与实现

基于51单片机的自动转向修复系统的设计与实现

文章目录 前言资料获取设计介绍功能介绍设计清单具体实现截图参考文献设计获取 前言 💗博主介绍：✌全网粉丝10W+,CSDN特邀作者、博客专家、CSDN新星计划导师，一名热衷于单片机技术探索与分享的博主、专注于 精通51/STM32/MSP430/AVR等单片机设计 主要对象是咱们电子相关专业的大学生，希望您们都共创辉煌！✌💗 👇🏻 精彩专栏 推荐订阅👇🏻 单片机
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2