GPS接收机捕获跟踪和PVT源代码

// 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