position.c

此程序为GPS接收机的源码

     TOE  - TIME OF EPHEMERIS FROM START OF WEEKLY EPOCH
     ecc  - ORBITAL INITIAL ECCENTRICITY
     TOA  - TIME OF APPLICABILITY FROM START OF WEEKLY EPOCH
     INC  - ORBITAL INCLINATION
     IDOT - RATE OF INCLINATION ANGLE
     CUC  - ARGUMENT OF LATITUDE COSINE CORRECTION TERM
     CUS  - ARGUMENT OF LATITUDE SINE   CORRECTION TERM
     CIC  - INCLINATION COSINE CORRECTION TERM
     CIS  - INCLINATION SINE   CORRECTION TERM
     RRA  - RATE OF RIGHT ASCENSION
     SQA  - SQUARE ROOT OF SEMIMAJOR AXIS
     LAN  - LONGITUDE OF NODE AT WEEKLY EPOCH
     AOP  - ARGUMENT OF PERIGEE
     MA   - MEAN ANOMALY AT TOA
     -> MA IS THE ANGLE FROM PERIGEE AT TOA
     DN   - MEAN MOTION DIFFERENCE

*******************************************************************************/

static xyzt_t
SatPosEphemeris( unsigned short ch)
{
    xyzt_t result;

    unsigned short i;

    double del_toc; // Time until/since clock correction reference time
    double del_toe; // Time until/since ephemeris correction reference time
    double del_t;   // Clock correction term
    double tc;      // Corrected time of satellite transmission
    
    /* Temporaries to save some float math. */
    double ea,ma,diff,ta,aol,delr,delal,delinc,r,inc,la,xp,yp;
    double mn;
    double s, c, sin_ea, cos_ea, e2, A, s_la, c_la;

    // Find the time difference in seconds for the clock correction. This must
    // be adjusted for wrapping at the end-of-week: see (BB p138) and (ICD
    // 20.3.3.3.3.1).
    del_toc = pr2[ch].sat_time - ephemeris[ch].toc;
    
    if( del_toc > SECONDS_IN_WEEK / 2)
        del_toc -= SECONDS_IN_WEEK;
    else if( del_toc < -SECONDS_IN_WEEK / 2)
        del_toc += SECONDS_IN_WEEK;

    // Calculate the time correction except for relativistic effects -- need
    // to calculate Ek first before we get those (BB p133).
    del_t =                ephemeris[ch].af0
              + del_toc * (ephemeris[ch].af1
              + del_toc *  ephemeris[ch].af2); // af2 is often zero

    // L1 - L2 Correction (group delay) for single freq. users: see (BB p134)
    // and (ICD 20.3.3.3.3.2)
    del_t -= ephemeris[ch].tgd;

    // Tsv corrected time (except relativistic effects)
    tc = pr2[ch].sat_time - del_t;

    // Find the time since orbital reference in seconds, adjusted for wrapping
    // at end-of-week. See: (ICD 20.3.3.4.3 & Table 20-IV) and (BB p138).
    // Note the time used is corrected EXCEPT for relativistic effects
    del_toe = tc - ephemeris[ch].toe;

    if( del_toe > SECONDS_IN_WEEK / 2)
        del_toe -= SECONDS_IN_WEEK;
    else if( del_toe < -SECONDS_IN_WEEK / 2)
        del_toe += SECONDS_IN_WEEK;

    /* Solve for the eccentric anomaly ("anomaly" means "angle" in old-speak)
     * From Webster's 1913:
     *    The angular distance of a planet from its perihelion,
     *    as seen from the sun. This is the true anomaly. The
     *    eccentric anomaly is a corresponding angle at the
     *    center of the elliptic orbit of the planet. The mean
     *    anomaly is what the anomaly would be if the planet's
     *    angular motion were uniform.
     *
     * ea is the eccentric anomaly to be found,
     * ma is the mean anomaly measured at the satellite's time of transmission,
     * ecc is the orbital eccentricity,
     * mn is the mean motion (proportional to the average sat.ang.velocity)
     * dn is the ephemeris correction to mn,
     */
    // The eccentric anomaly is a solution to the non-linear equation
    //   ea = ma + ecc * sin[ea]
    // Solve this equation by Newton's method
    // Set the initial value to the mean anomaly,
    // might do better with a better initial value (BBp164)
    // TODO: set the initial value using an extrapolation of the old ea

    // A = sqrt(Orbital major axis)^2 (we use this to find r also)
    A = ephemeris[ch].sqrtA * ephemeris[ch].sqrtA;

    // calculate the "mean motion" === (2pi / orbital period), i.e. the average
    // sat.ang.vel.  (mean motion) = sqrt(mu / A^3)
    mn = SQRTMU / (ephemeris[ch].sqrtA * A);

    // Prep the loop
    ma = ephemeris[ch].ma + del_toe * (mn + ephemeris[ch].dn);
    ea = ma;

    for( i = 0; i < 10; i++) /* Avoid possible endless loop. */
    {
        sin_ea = sin( ea);
        cos_ea = cos( ea);

        // Is it really better to use Newton's method as opposed to finding
        // the fixed point of Kepler's equation directly? The direct method
        // saves a cos() per iteration, so to be worth it Newton must be twice
        // as fast.

        diff = (ea - ephemeris[ch].ecc * sin_ea - ma) /
               (1 - ephemeris[ch].ecc * cos_ea);

        ea -= diff;
        if( fabs( diff) < 1.0e-12) // TODO justify this constant
            break;
    }

    /* ea may not converge. No error handling. */
    // It would be interesting to tag non-convergence, probably never happens.

    sin_ea = sin( ea); // Save some trig calculation
    cos_ea = cos( ea);

    // Now make the relativistic correction to the coarse time (BBp133)
    del_t += (F_RC * ephemeris[ch].ecc * ephemeris[ch].sqrtA * sin_ea);

    // Store the final clock corrections (Used for m_rho in position_thread.)
    result.tb = del_t;

    /* ta is the true anomaly (angle from perigee) */
    // This is clever because it magically gets the sign right
    // It can be done with an ArcCos and a simple sign fix-up, not sure what
    // is most efficient. (GBp48)
    e2 = ephemeris[ch].ecc * ephemeris[ch].ecc;
    ta = atan2( (sqrt( 1 - e2) * sin_ea), (cos_ea - ephemeris[ch].ecc));

    /* aol is the argument of latitude of the satellite */
    // w is the argument of perigee (GBp59)
    aol = ta + ephemeris[ch].w;

    /* Calculate the second harmonic perturbations of the orbit */
    c = cos( aol + aol);
    s = sin( aol + aol);
    delr   = ephemeris[ch].crc * c + ephemeris[ch].crs * s; // radius
    delal  = ephemeris[ch].cuc * c + ephemeris[ch].cus * s; // arg. of latitude
    delinc = ephemeris[ch].cic * c + ephemeris[ch].cis * s; // inclination

    /* r is the radius of satellite orbit at time pr2[ch].sat_time */
    r = A * (1 - ephemeris[ch].ecc * cos_ea) + delr;
    aol += delal;
    inc = ephemeris[ch].inc0 + delinc + ephemeris[ch].idot * del_toe;

    /* la is the corrected longitude of the ascending node */
    la = ephemeris[ch].w0 + del_toe * (ephemeris[ch].omegadot - OMEGA_E) -
            OMEGA_E * ephemeris[ch].toe;

    // xy position in orbital plane
    xp = r * cos( aol);
    yp = r * sin( aol);

    // transform to ECEF
    s_la = sin( la);
    c_la = cos( la);
    s = sin( inc); // This reuse may not be clever with optimization turned on
    c = cos( inc);

    result.x = xp * c_la - yp * c * s_la;
    result.y = xp * s_la + yp * c * c_la;
    result.z = yp * s;

    return( result);
}


/******************************************************************************
 * Wake up on valid measurements and produce pseudoranges. Flag the navigation
 * thread if we have four or more valid pseudoranges
 ******************************************************************************/
void
position_thread( CYG_ADDRWORD data) // input 'data' not used
{
    unsigned short  ch;
    unsigned short  satnum;
    xyz_t           ecef_temp;

    // There's no way that we're going to get a bit before this thread
    // is first executed, so it's ok to run the flag init here.
    cyg_semaphore_init( &position_semaphore, 0);

    while(1)
    {
        // Block until there's a valid set of measurements posted from the
        // measurement thread.
        cyg_semaphore_wait( &position_semaphore);

        setbits32( GPS4020_GPIO_WRITE, LED7); // DEBUG

        // OK we're awake: use a COPY of the PRs/measurements, because they're
        // going to change in about 100ms and there's NO WAY IN HELL we're
        // going to be done by then. And check that the ephemeris is still
        // good while we're at it.
        satnum = 0;
        for( ch = 0; ch < N_CHANNELS; ++ch)
        {
            if( ephemeris[ch].valid && pr2[ch].valid)
            {
                // get the satellite ECEF XYZ + time (correction?) of the
                // satellite. Note that sat_position is ECEF*T* type.
                sat_pos_by_ch[ch] = SatPosEphemeris( ch);

                if (display_command == DISPLAY_POSITION)
                   sat_azel[ch] = satellite_azel( sat_pos_by_ch[ch]);

                // Pack the satellite positions into an array for efficiency
                // in the calculate_position function.
		// Probably should not be doing this copy???
                sat_position[satnum].x = sat_pos_by_ch[ch].x;
                sat_position[satnum].y = sat_pos_by_ch[ch].y;
                sat_position[satnum].z = sat_pos_by_ch[ch].z;
                sat_position[satnum].tb = sat_pos_by_ch[ch].tb;
                sat_position[satnum].channel = ch;
                sat_position[satnum].prn = pr2[ch].prn;

                // Doppler measurement is not supported.
                // SHOCK!! SHOCK, I TELL YOU.

                // Pseudorange measurement. No ionospheric delay correction.
                m_rho[satnum] = pr2[ch].range +
                    (SPEED_OF_LIGHT * sat_pos_by_ch[ch].tb);
                // (.tb) is the per-satellite clock correction calculated from
                // ephemeris data.

                satnum++; // Number of valid satellites
            }
        }

        // If we've got 4 or more satellites, position!
        if( satnum >= 4)
        {
            receiver_pos = calculate_position( satnum);

            if( receiver_pos.valid)
            {
                // Move from meters to seconds
                receiver_pos.b /= SPEED_OF_LIGHT;

                // Correct the clock with the latest bias. But note that the
                // clock correction function may be smoothing the correction
                // and doing other funky things.
//                 set_clock_correction( receiver_pos.b);

                // Put receiver position in LLH
                if (display_command == DISPLAY_POSITION)
                {
                    ecef_temp.x = receiver_pos.x;
                    ecef_temp.y = receiver_pos.y;
                    ecef_temp.z = receiver_pos.z;
                    receiver_llh = ecef_to_llh( ecef_temp);
                }
            }
            // Log position, if enabled
#ifdef ENABLE_POSITION_LOG
            log_position();
#endif
        }
        else
        {
            // Not enough sats to calc position, but let the world know how
            // many satellites we do currently have.
            receiver_pos.positioning = 0;
            receiver_pos.n = satnum;
        }

        // FIXME: shouldn't be called from this thread. HACK!
#ifdef ENABLE_EPHEMERIS_LOG
        log_ephemeris();
#endif

        // Flag pseudorange.c that we've used the pseudorange data and are
	// ready for a new set in pr2.
        pr2_data_fresh = 0;
        clearbits32( GPS4020_GPIO_WRITE, LED7); // DEBUG
    }
}