10、相关数据累积任务实现

\qquad 下面是HD-GR GNSS导航软件的相关数据累积任务实现代码：

// main_accum_task.c -- Correlation Data accumulation task accum_task(…) and 
// supporting functions.

/* * 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 "gnsstime.h"
#include "gps_accum_task.h"
#include "b1i_accum_task.h"
#include "main_allocate.h"
#include "main_message.h"
#include "main_measure.h"
#include "main_position.h"

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

unsigned short m_status __attribute__ ((section(".isrdata.rwdata")));
OS_FLAGS g_channels_to_allocate __attribute__ ((section(".isrdata.rwdata")));
OS_FLAGS g_channels_with_bits __attribute__ ((section(".isrdata.rwdata")));
OS_FLAGS g_channel_bits __attribute__ ((section(".isrdata.rwdata")));

/****************************************************************************** * classic signum function written for the long datatype ******************************************************************************/
inline long sgn( long data)
{
    
	return (data < 0 ? -1: (data != 0) ? 1:0);
}

/****************************************************************************** * Compute the approximate magnitude (square norm) of 2 numbers * * The "correct" function is naturally sqrt(a^2+b^2). * This computation is too slow for our application however. * We use the leading order approximation (mag = a+b/2) for b<a with sgn fixes. * This is probably as good as possible without a multiply. * * Haven't tried a fit, but based on endpoints a+(sqrt(2)-1)*b is a better * approximation. Everything else seems to have either a couple multiplies and * a divide, or an actual square root. It's a fact that if there's no hardware * multiply the binary square root is actually faster than a multiply on a GP * machine, but since the ARM7TDMI has a multiply the root is slower for us. ******************************************************************************/
inline long lmag( long a, long b)
{
    
	a = labs(a);
	b = labs(b);
	if (a > b) return (a + (b >> 1));
	else return (b + (a >> 1));
}

/******************************************************************************* FUNCTION fix_atan2( long y,long x) RETURNS long integer PARAMETERS y long quadrature fixed point value x long in-phase fixed point value PURPOSE // This is the phase discriminator function. This function computes the fixed point arctangent represented by y and x in the parameter list 1 radian = 2^14 = 16384 based on the power series 2^14*( (y/x)-2/9*(y/x)^3 ) // This is not a particularly good approximation. // In particular, 0.2332025325081921, rather than 2/9 is the best // fit parameter. The next simplest thing to do is fit {x,x^3} // I'm assuming the interval of validity is [0,1). // Fitting {1,x,x^3} is bad because a discriminator must come smoothly to // zero. // I wonder, in the average math library, if it might be faster to do the // division un-signed? // It's not much more expensive to fit x+a*x^2+b*x^3 (one more add). // The compiler may not properly optimize ()/9. WRITTEN BY Clifford Kelley Fixed for y==x added special code for x==0 suggested by Joel Barnes, UNSW *******************************************************************************/
inline long fix_atan2( long y, long x)
{
    
	long result=0,n,n3;

	// 4 quadrants, one invalid case

	// Invalid case
	if ((x == 0) && (y == 0))
		return(result);

	if (x > 0 && x >= labs(y)) {
    
		n=(y<<14)/x;
		n3=((((n*n)>>14)*n)>>13)/9;
		result=n-n3;
	}
	else if (x <= 0 && -x >= labs(y)) {
    
		n  = (y<<14)/x;
		n3 = ((((n*n)>>14)*n)>>13)/9;
		if (y > 0)
			result=n-n3+PI_SHIFT14;
		else if (y <= 0)
			result=n-n3-PI_SHIFT14;
	}
	else if (y > 0 &&  y > labs(x)) {
    
		n  = (x<<14)/y;
		n3 = ((((n*n)>>14)*n)>>13)/9;
		result = PI_OVER2_SHIFT14 - n + n3;
	}
	else if (y < 0 && -y > labs(x)) {
    
		n  = (x<<14)/y;
		n3 = ((((n*n)>>14)*n)>>13)/9;
		result = -n + n3 - PI_OVER2_SHIFT14;
	}
	return(result);
}

inline long fix_atan(long y, long x)
{
    
	if (x < 0) {
    
		x = -x;
		y = -y;
	}
	return fix_atan2(y, x);
}

inline void set_tic_delay( int delay, unsigned short delay_life, unsigned short delay_range)
{
    
	unsigned long tic_delay = labs(delay);
	if (delay < 0) tic_delay |= (1 << 25);
	if (delay_life) tic_delay |= (1 << 26);
	if (delay_range) tic_delay |= (1 << 27);
	write_to_correlator( PROG_TIC_DELAY, tic_delay );
}

inline void set_tic_slew( int delay, int slew, int mod)
{
    
	unsigned long tic_slew = (1 << 31) | labs(delay) |
			(labs(slew) << 19) | (labs(mod) << 26);
	if (delay < 0) tic_slew |= (1 << 18);
	if (slew < 0) tic_slew |= (1 << 30);
	write_to_correlator( PROG_TIC_DELAY, tic_slew );
}

inline void load_tic_count( unsigned long count)
{
    
	write_to_correlator( PROG_TIC_LOAD, count );
}

void initialize_tracking( void)
{
    
	gps_initialize_tracking();
	b1i_initialize_tracking();
}


/******************************************************************************* FUNCTION accum_task(void* pdata) RETURNS None. PARAMETERS pdata: not used PURPOSE Main routine which runs on an accum_int. *******************************************************************************/

void accum_task(void* pdata)
{
    
	INT8U err;
	unsigned long new_data;

	while (1) {
    
		OSSemPend(m_SemISR, 0, &err);

		// NEW_DATA register shows which channels have new accumulation data available
		new_data = read_from_correlator(NEW_DATA);

		if (m_status & 0x1) {
    
			// We just had a Tic (~100ms) so update the receiver clock
			increment_time_with_tic();
	        // Grab the time in bits
	        grab_bit_times();
		}

		// 1+. The beginning of basic interaction with the baseband

		if (m_sys_posconst & POS_CONSTELL_GPS) {
    
			gps_accum_newdata(new_data);
		}
		if (m_sys_posconst & POS_CONSTELL_BDS) {
    
			b1i_accum_newdata(new_data >> BDMOD_GPS_CHANNELS);
		}

		// 1-. The end of basic interaction with the baseband

		// 2+. The beginning of baseband signal acquisition / tracking (loop control)

		// Clear the IPC shadows (see below).
		g_channels_to_allocate = 0;
		g_channels_with_bits = 0;
		g_channel_bits = 0;

		if (m_sys_posconst & POS_CONSTELL_BDS) {
    
			b1i_track_channels(new_data >> BDMOD_GPS_CHANNELS);
			g_channels_to_allocate <<= GPS_MAX_CHANNELS;
			g_channels_with_bits <<= GPS_MAX_CHANNELS;
			g_channel_bits <<= GPS_MAX_CHANNELS;
		}
		if (m_sys_posconst & POS_CONSTELL_GPS) {
    
			gps_track_channels(new_data);
		}

		// 2-. The end of baseband signal acquisition / tracking (loop control)

		// If any channels are off, signal the allocation thread that it should
		// allocate some off channels.
		if (g_channels_to_allocate) {
    
			OSFlagPost(m_AllocateFlag, g_channels_to_allocate, OS_FLAG_SET, &err);
		}

		// Set the flag IPC which starts up the message thread. No reason to call
		// it more than once, it's probably an expensive call, so we use the
		// g_channels_with_bits shadow.
		if (g_channels_with_bits) {
    
			OSFlagPost(m_MessageFlag, g_channels_with_bits, OS_FLAG_SET, &err);
		}

		// 3+. The beginning of navigation solution

		// Check status register for a TIC
		if (m_status & 0x1) {
    
			fill_ch_pars();
			OSSemPost(m_SemMeas);
		}

		// 3-. The end of navigation solution
	}
}