satpos.c

来自「根据GPS官方网站提供的gps定位方法而编写的C语言gps定位代码」· C语言 代码 · 共 81 行

#include <math.h>
#include "gpsconst.h"

void satpos(double *eph, double Ttr, double * Trel, double * X)
{
  	/* IN:	ephemeris				eph 	*/
  	/* IN:	satellite GPS time			Ttr 	*/
  	/* OUT:	relativistic correction term		Trel 	*/
  	/* OUT:	satellite position			X	*/
  	double 	M0, dn, ec, A, W0, i0, w, Wdot, Cuc, Cus, Crc, Crs, Cic, Cis, Toe,
	 	Idot, T, n0, n, M, E, Eold, snu, cnu, nu, phi, du, dr, di, u, r, i,
	 	Xdash, Ydash, Wc;

  	/*for clarity of code, copy the ephemeris parameters and convert to radians*/
  	Crs = eph[0];
 	dn = eph[1] * pi;
  	M0 = eph[2] * pi;
  	Cuc = eph[3];
  	ec = eph[4];
  	Cus = eph[5];
  	A = eph[6] * eph[6];
  	Toe = eph[7];
  	Cic = eph[8];
  	W0 = eph[9] * pi;
  	Cis = eph[10];
  	i0 = eph[11] * pi;
  	Crc = eph[12];
  	w = eph[13] * pi;
  	Wdot = eph[14] * pi;
  	Idot = eph[15] * pi;

 
  	T = Ttr - Toe;
  	if (T > 302400L)
	{
    		T -= 604800L;
	}
  	if (T < -302400L)
	{
    		T += 604800L;
	}

  	n0 = sqrt(mu / (A * A * A));
  	n = n0 + dn;

  	M = M0 + n * T;	
  	E = M;   /*start value for E*/
  	do 
	{
    		Eold = E;
    		E = M + ec * sin(E);
  	} while (fabs(E - Eold) >= 1.0e-8);

  	snu = sqrt(1 - ec * ec) * sin(E) / (1 - ec * cos(E));
  	cnu = (cos(E) - ec) / (1 - ec * cos(E));

  	nu = atan2(snu, cnu);

  	phi = nu + w;

  	du = Cuc * cos(2 * phi) + Cus * sin(2 * phi);
  	dr = Crc * cos(2 * phi) + Crs * sin(2 * phi);
  	di = Cic * cos(2 * phi) + Cis * sin(2 * phi);

  	u = phi + du;
  	r = A * (1 - ec * cos(E)) + dr;
  	i = i0 + Idot * T + di;

  	Xdash = r * cos(u);
  	Ydash = r * sin(u);

  	Wc = W0 + (Wdot - Wedot) * T - Wedot * Toe;

  	X[0] = Xdash * cos(Wc) - Ydash * cos(i) * sin(Wc);
  	X[1] = Xdash * sin(Wc) + Ydash * cos(i) * cos(Wc);
  	X[2] = Ydash * sin(i);

  	*Trel = F * ec * eph[6] * sin(E);   /*relativistic correction term*/

}