tracking.c

此程序为GPS接收机的源码

// tracking.c 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.
#include <cyg/kernel/kapi.h>

#include <stdlib.h>
#include <cyg/infra/diag.h>
#include "constants.h"
#include "tracking.h"
#include "allocate.h"
#include "gp4020.h"
#include "gp4020_io.h"
#include "message.h"

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

// Later on, we scale 1 radian = 2^^14
#define PI_SHIFT14       (long)(0.5 + PI * (1 << 14)) // 51472
#define PI_OVER2_SHIFT14 (long)(0.5 + PI * (1 << 13)) // 25736

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

chan_t CH[N_CHANNELS];

unsigned short accum_status_a; // set in accum_isr
unsigned short accum_status_b; // set in accum_isr

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

cyg_flag_value_t nav_bits_ready;

static unsigned short CarrSrchWidth;
static unsigned short AcqThresh;

static short CarrSrchStep;
static short PullCodeK;
static short PullCodeD;
static short PullInTime;
static short PhaseTest;
static short PullCarrK;
static short PullCarrD;
static short Rms;
static short CodeCorr;
static short TrackCarrK;
static short TrackCarrD;
static short TrackCodeK;
static short TrackCodeD;
static short TrackDiv;

/*******************************************************************************
 * Write 32 bits to the 16 bit code DCO rate and carrier DCO rate registers
 ******************************************************************************/
inline void
set_code_dco_rate( unsigned long ch, unsigned long freq)
{
    volatile union _gp4020_channel_control *channel_control =
        (volatile union _gp4020_channel_control *)GP4020_CORR_CHANNEL_CONTROL;

    out16( GP4020_CORR_X_DCO_INCR_HIGH, (unsigned short)(freq >> 16));
    channel_control[ch].write.code_dco_incr_low = (unsigned short)freq;
}

inline void
set_carrier_dco_rate( unsigned short ch, unsigned long freq)
{
    volatile union _gp4020_channel_control *channel_control =
        (volatile union _gp4020_channel_control *)GP4020_CORR_CHANNEL_CONTROL;

    out16( GP4020_CORR_X_DCO_INCR_HIGH, (unsigned short)(freq >> 16));
    channel_control[ch].write.carrier_dco_incr_low = (unsigned short)freq;
}


/******************************************************************************
 * Turn a correlator channel on or off
 ******************************************************************************/
void
channel_power_control( unsigned short ch, unsigned short on)
{
    static unsigned short reset_control_shadow = 0xffff;

    if( on)
        reset_control_shadow |=  (1 << ch);
    else
        reset_control_shadow &= ~(1 << ch);

    out16( GP4020_CORR_RESET_CONTROL, reset_control_shadow);
}


/******************************************************************************
 * classic signum function written for the short datatype
 ******************************************************************************/
static inline short
sgn( short data)
// It bugs me this has to be re-invented.
// It bugs me that this could be written better.
{
    return( data < 0 ? -1: data != 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.
 ******************************************************************************/
static long
lmag( long a, long b)
{
    if( a | b)
    {
        long c, d;
        c = labs( a);
        d = labs( b);

        if( c > d)
            return( c + (d >> 1));
        else
            return( d + (c >> 1));
    }
    else return( 0);
}

// Added smag() to deal with shorts only (should speed things up)
/* Point #1, this is quite possibly _not_ faster, because the ARM internally is
 * 32bit, so each op has to be masked to 16bits. Only 16bit memory accesses for
 * data are speeded up, which this has few of.
 * Point #2, do you want to return unsigned? perhaps this doesn't matter. I'm
 * not enough of a C-head. Is this only a compile-time thing or will it force
 * some sort of run-time conversion? Signed seems safer.
 * Point #3, avoiding the library call (labs) _shouldn't_ be faster, but i
 * concede it might be, and it almost certainly isn't slower.
 */
static unsigned short
smag( short a, short b)
{
    if( a < 0) a = -a;
    if( b < 0) b = -b;

    if( a > b)
        return( a + (b >> 1));
    else
        return( b + (a >> 1));
}

/*******************************************************************************
FUNCTION fix_sqrt(long x)
RETURNS  long integer

PARAMETERS
        x long integer

PURPOSE
        This function finds the fixed point square root of a long integer

WRITTEN BY
        Clifford Kelley

*******************************************************************************/
// Same name comment as above lsqrt
// Probably a _much_ more efficient alg., but just guessing.
static long
fix_sqrt( long x)
{
    long xt,scr;
    int i;

    i=0;
    xt=x;
    do
    {
        xt=xt>>1;
        i++;
    } while( xt>0);

    i=(i>>1)+1;
    xt=x>>i;
    do
    {
        scr=xt*xt;
        scr=x-scr;
        scr=scr>>1;
        scr=scr/xt;
        xt=scr+xt;
    } while( scr!=0);

    xt=xt<<7;
    return( xt);
}


/*******************************************************************************
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. (I think it does.)

WRITTEN BY
    Clifford Kelley
    Fixed for y==x  added special code for x==0 suggested by Joel Barnes, UNSW
*******************************************************************************/
static inline long
fix_atan2( long y, long x)
{
    long result=0,n,n3;

    // 4 quadrants, one invalid case

    if( (x == 0) && (y == 0)) /* Invalid case */
        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);
}


/******************************************************************************
 * Need to set up CH[] structure and initialize the loop dynamic parameters.
 *
 * Currently called from cyg_user_start() in gpl-gps.c
 ******************************************************************************/
void
initialize_tracking( void)
{
    unsigned short ch;

    // Why are these a good choices?
    CarrSrchWidth = 40;    // search 20 frequency steps on either side
    CarrSrchStep = 4698;   // search step width = 200 Hz

    /*AcqThresh = 554;  // 35 dBHz */
    AcqThresh = 622;    /* 36 dBHz */ // Be nice to justify choices

    PullCodeK = 111;
    PullCodeD = 7;
    PullCarrK = -12;
    PullCarrD = 28;

    PullInTime = 1000;  /* 1 second */
    PhaseTest  = 500;   /* last 1/2 second of pull in, start phase test */
    Rms = 312;

    CodeCorr = 0;

    TrackCodeK = 55;
    TrackCodeD = 3;
    TrackCarrK = -9;
    TrackCarrD = 21;
    TrackDiv = 19643;

    for( ch = 0; ch < N_CHANNELS; ch++)
        if( CH[ch].state != CHANNEL_OFF)
            diag_printf("INITIALIZE TRACKING: CHANNEL NOT OFF!\n\r");

#if 0

    // Removed; trying to track this bug down. The compiler should init the
    // global structure to zero.

    /* Allocate channels */
    for( ch = 0; ch < N_CHANNELS; ch++)
        CH[ch].state = CHANNEL_OFF; // This fixes a bug, but shouldn't unless
                                   // the CH[] structure is getting overwritten
                                  // although is relying on compiler init good