\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
}
}