Here we will go through the code that derives the GPS Satellite position, the theory behind the method explained here has already been described and you will see that much of the code is very similar to those equations. As we said before this code has been developed using the C programming language as that is the easiest for embedded programming.
We begin by defining several variables that will be used during the calculation.
int eph_sat_pos(int chan, double gps_time) { GPS_structEphemeris *eph; GNSS_structPVT *spvt; double d_toc, n, d_tsv, M_k, E_k, t_k, v_k, theta_k, r_k, u_k, i_k, xp_k, yp_k, omega_k, M_kdot, E_kdot, v_kdot, u_kdot, r_kdot, i_kdot, xp_kdot, yp_kdot, omega_kdot, diff; int count; eph = &ephs[chan]; spvt = &rx_meas[chan].sat_pos.pvt;
t_k = gps_time - eph->toe; if(t_k>302400.0) t_k -= 604800.0; else if(t_k<-302400.00) t_k +=604800.0;
n = SQRTMU/pow(eph->sqrta,3)+eph->delta_n; M_k = eph->m0+n*t_k;
count = 0; E_k = M_k; do{ diff=M_k-(E_k-eph->ecc*sin(E_k)); E_k+=diff; count++; }while (fabs(diff) > 1.0e-12 && count < 30);
if(eph->ecc != 0.0) v_k = atan2(sqrt(1.0-pow(eph->ecc,2))*sin(E_k),cos(E_k)-eph->ecc); else v_k = E_k; theta_k = v_k + eph->w;
r_k = pow(eph->sqrta,2)*(1.0-eph->ecc*cos(E_k)); r_k+= eph->crc*cos(2.0*theta_k)+eph->crs*sin(2.0*theta_k); u_k = theta_k; u_k+= eph->cus*sin(2.0*theta_k)+eph->cuc*cos(2.0*theta_k); i_k = eph->i0+eph->cic*cos(2.0*theta_k)+eph->cis*sin(2.0*theta_k); i_k+= eph->idot*t_k;
xp_k = r_k*cos(u_k); yp_k = r_k*sin(u_k); omega_k = eph->omega0+(eph->omegadot-OMEGA_E)*t_k; omega_k-= OMEGA_E*eph->toe; omega_kdot = eph->omegadot - OMEGA_E; spvt->clk = d_tsv; spvt->x = xp_k*cos(omega_k)-yp_k*cos(i_k)*sin(omega_k); spvt->y = xp_k*sin(omega_k)+yp_k*cos(i_k)*cos(omega_k); spvt->z = yp_k*sin(i_k);
M_kdot = n; E_kdot = M_kdot/(1.0 - eph->ecc*cos(E_k)); v_kdot = sin(E_k)*E_kdot*(1.0+eph->ecc*cos(v_k)); v_kdot /= sin(v_k)*(1.0-eph->ecc*cos(E_k)); r_kdot = pow(eph->sqrta,2)*eph->ecc*sin(E_k)*n/(1.0-eph->ecc*cos(E_k)); r_kdot+= 2.0*(eph->crs*cos(2.0*u_k)-eph->crc*sin(2.0*u_k))*v_kdot; u_kdot = v_kdot + 2.0*(eph->cus*cos(2.0*u_k)-eph->cuc*sin(2.0*u_k))*v_kdot; i_kdot = eph->idot + 2.0*(eph->cis*cos(2.0*u_k)-eph->cic*sin(2.0*u_k))*v_kdot; xp_kdot = r_kdot*cos(u_k) - yp_k*u_kdot; yp_kdot = r_kdot*sin(u_k) + xp_k*u_kdot; spvt->vx = (xp_kdot-yp_k*cos(i_k)*omega_kdot )*cos(omega_k) - (xp_k*omega_kdot+yp_kdot*cos(i_k)-yp_k*sin(i_k)*i_kdot)*sin(omega_k); spvt->vy = (xp_kdot-yp_k*cos(i_k)*omega_kdot )*sin(omega_k) + (xp_k*omega_kdot+yp_kdot*cos(i_k)-yp_k*sin(i_k)*i_kdot)*cos(omega_k); spvt->vz = yp_kdot*sin(i_k) + yp_k*cos(i_k)*i_kdot; return(0); } © 2018