gps.c

gps.c:GPS主代码 constant.h:GPS的头文件 PC temperature control .c:电脑温度控制 code.c:数字血压计的代码 Computer side.c

for (i = 0;i<=n-1;i++)
begin
    if (cl[i][i]== 0)
    begin
        printf("No unique solution");
    end
    else
    begin
        for (p = 0;p<=m-1;p++)
        begin
            x[i] = cl[i][n+p]/cl[i][i];
        end
    end
end

	
  
   
   
    
    // calculate 'obsPos' by adding 'x' to the current guess
    obsPos[0] = guessx+x[0];
    obsPos[1] = guessy+x[1];
    obsPos[2] = guessz+x[2];
 
  
    if (iters > 10) 
     begin
 	stop = 1;
     end
      guessx =  obsPos[0];
      guessy =  obsPos[1];
      guessz =  obsPos[2];
      
      
    end

 latlong( obsPos[0], obsPos[1], obsPos[2],0,2);
   printf(" THE user    ");
     putchar('\r');
   putchar('\n');   
   printf("  useralt:  %e" ,    useralt);
   printf("  userlong:  %e" ,    userlon);
   printf("  userlat:  %e" ,    userlat);

   putchar('\r');
   putchar('\n'); 

end



/*
 *The following fucntions ar responsible for all the mathematics needed to calculate orbits
 *satellites location,frame transformations, and the navigation solution  
 */
//***********************************************************************
void FindSat(char l)
begin 
int i;
   float dt ,t; 
   float Mcorr, M, E,cos2u,sin2u,omegatemp, omegaCorr, dfdE , dE,ft,sinnu,cosnu,nu,u,lcorr,rCorr,r,iCorr,ltemp,xp,yp, ECEFx,ECEFy,ECEFz,i0temp;
   
        // define orbital parameters
	//toe	                        % ephemeris reference time (seconds)
	//e                  		% eccentricity (unitless)
	//roota                  	% square root of semi-major axis (meters1/2)
	//omega                 	% argument of perigee (radians)
	//m0                        	% mean anomaly at reference time (radians)
	//omega0		        % right ascension at reference (radians)
	//omegadot              	% rate of right acension (radians/second)
	//deltan                 	% mean motion difference (radians/second)
	//i0               		% inclination angle at reference time (radians)
	//idot           		% inclination angle rate (radians/second)
	//cuc           		% latitude cosine harmonic correction (radians)
	//cus              		% latitude sine harmonic correction (radians)
	//crc              		% orbit radius cosine harmonic correction (meters)
	//crs                 		% orbit radius sine harmonic correction (meters)
	//cic                     	% inclination cosine harmonic correction (radians)
	//cis                		% inclination sine harmonic correction (radians) 
	
	//FIX DIMENSIONS
	omega[l] = omega[l]*PI ;		//% argument of perigee (radians)
	m0[l] =  m0[l]*PI;		//	% mean anomaly at reference time (radians)
 	omega0[l] = omega0[l]*PI ;		//	% right ascension at reference (radians)
 	omegadot[l] = omegadot[l]*PI ;	        //% rate of right acension (radians/second)
 	deltan[l] =  deltan[l]*PI;		//	% mean motion difference (radians/second)
 	i0[l] =i0[l]*PI; 		//% inclination angle at reference time (radians)
 	idot[l] =idot[l]*PI; 		//% inclination angle rate (radians/second)
	
// define time of position request and delta t from epoch; correct 
// for possible week crossovers; 604800 seconds in a GPS week
	t=(float)currentTime/1000;//32540800.0/1000.0;//325209900/1000;
	dt =t - toe[l];   //first toe should be t
	if(dt> 302400)
	begin 
	dt = dt - 604800; //if into the next week
	end
	if(dt <-302400)
	begin 
	dt = dt + 604800; //% if into the previous week
	end
// calculate mean anomaly with corrections
	Mcorr = deltan[l]* (float)dt;
	M = m0[l] + (sqrt(muearth) * pow(roota[l],-3)) *(float) dt + Mcorr; 
//compute the eccentric anomaly from mean anomaly using
// Newton-Raphson method to solve for 'E' in:  
//		f(E) = M - E + ecc * sin(E) = 0
	E = M;
   for( i =1; i<=10; i++)
   begin 
      ft = M - E + e[l] * sin(E);
		dfdE = - 1 + e[l] * cos(E);
		dE = - ft / dfdE;
		E = E + dE;
   end
//calculate true anomoly from eccentric anomoly
	sinnu = sqrt(1 -pow( e[l],2)) * sin(E) / (1 - e[l] * cos(E));
	cosnu = (cos(E) - e[l]) / (1 - e[l] * cos(E));
	nu = atan2(sinnu,cosnu);
//calculate the argument of latitude and the argument of perigee
//iteratively.
	omegatemp = omega[l];///
  for(  i =1; i<=5; i++)
      begin
		u = omegatemp + nu;
		cos2u = cos(2*u); 
      sin2u = sin(2*u);
		omegaCorr = cuc[l] * cos2u + cus[l] * sin2u; 
		omegatemp = omega[l] + omegaCorr;
      end
//calculate longitude of ascending node with correction
	lcorr = omegadot[l] * (float)dt;
        ltemp = omega0[l] - OmegaE * t + lcorr;
//calculate orbital radius with correction
	rCorr = crc[l] * cos2u + crs[l] * sin2u;
	r = (pow(roota[l],2)) * (1 - e[l] * cos(E)) + rCorr;
//calculate inclination with correction
	iCorr = cic[l] * cos2u + cis[l] * sin2u + idot[l] * (float)dt;
	i0temp = i0[l] + iCorr;
//find position in orbital plane
	u = omegatemp + nu;
	xp = r * cos(u);
	yp = r * sin(u);
//find satellite position in ECEF coordinates
	ECEFx = (xp * cos(ltemp)) - (yp * cos(i0temp) * sin(ltemp));
	ECEFy  = (xp * sin(ltemp)) + (yp * cos(i0temp) * cos(ltemp));
	ECEFz = (yp * sin(i0temp));   
   satellitesECEFx[l]=ECEFx; 
   satellitesECEFy[l]=ECEFy; 
   satellitesECEFz[l]=ECEFz;		
//convert satellite locations to WGS84
  latlong(ECEFx, ECEFy, ECEFz,l,1);	
end 
//**************************************************** 
void latlong( float ECEFx, float ECEFy, float ECEFz, char l, char type)
begin
char stop;
float p, lat0, N0, term, latitude, longitude, altitude; 
// compute the longitude which is an exact calculation
	longitude = atan2(ECEFy , ECEFx)/ degrad;	// radians
// compute the latitude using iteration 
	p = sqrt(pow(ECEFx,2) + pow(ECEFy,2)); 

	// compute approximate latitude
	lat0 = atan((ECEFz / p) / (1 - esquare));	
	stop = 0;
	while (stop == 0)
	begin
		N0 =  pow(AA,2) / (sqrt( pow(AA,2) * pow((cos(lat0)),2) + pow(BB,2) * pow((sin(lat0)),2)));
		altitude = (p / cos(lat0)) - N0; 		// meters
		// calculate improved latitude
		term = 1/(1 - esquare * (N0 / (N0 + altitude)));
		latitude = atan(ECEFz / p * term);			// radians
		// check if result is close enough, 
		if (fabs(latitude - lat0) <  1*pow(10,-32))
		begin
			stop = 1;
		end
		lat0 = latitude;	
	end
// convert the latitude to degrees
	latitude /= degrad;		// degrees
// return location in latitude-longitude-altitude coordinates 
if(type==1)
begin
	satelliteslat[l]= latitude;
	satelliteslon[l] = longitude;
	satellitesalt[l] = altitude;
end
if(type==0)
begin
        Receiverlat[l]= latitude;
	Receiverlon[l] = longitude;
	Receiveralt[l] = altitude;
end  
if(type==2)
begin
        userlat= latitude;
	userlon = longitude;
	useralt = altitude;
end
end 

/*
 *The following fucntions ar responsible for sending data to the 
 *Satelliet tracker
 *
 */
//***********************************************************************
 void SendEphem(void)
   begin
   putchar('E'); 
   printf(" wn:  %i" ,   wn[0]);
   printf(" wn:  %i" ,   wn[1]);
   printf(" wn:  %i" ,   wn[2]);
   printf(" wn:  %i" ,   wn[3]);
   putchar('\r');
   putchar('\n'); 
   printf(" tow:  %ld" ,   tow[0]);
   printf(" tow:  %ld" ,   tow[1]);
   printf(" tow:  %ld" ,   tow[2]);
   printf(" tow:  %ld" ,   tow[3]);
   putchar('\r');
   putchar('\n');
   printf(" tgd:  %e" ,   tgd[0]);
   printf(" tgd:  %e" ,   tgd[1]);
   printf(" tgd:  %e" ,   tgd[2]);
   printf(" tgd:  %e" ,   tgd[3]);
   putchar('\r');
   putchar('\n');
   printf(" aodc:  %ld" ,   aodc[0]);
   printf(" aodc:  %ld" ,   aodc[1]);
   printf(" aodc:  %ld" ,   aodc[2]);
   printf(" aodc:  %ld" ,   aodc[3]);
   putchar('\r');
   putchar('\n');
   printf(" toc:  %ld" ,  toc[0]);
   printf(" toc:  %ld" ,   toc[1]);
   printf(" toc:  %ld" ,   toc[2]);
   printf(" toc:  %ld" ,   toc[3]);
   putchar('\r');
   putchar('\n');
   printf(" af2:  %e" ,   af2[0]);
   printf(" af2:  %e" ,   af2[1]);
   printf(" af2:  %e" ,   af2[2]);
   printf(" af2:  %e" ,   af2[3]);
   putchar('\r');
   putchar('\n'); 
   printf(" af1:  %e" ,   af1[0]);
   printf(" af1:  %e" ,   af1[1]);
   printf(" af1:  %e" ,   af1[2]);
   printf(" af1:  %e" ,   af1[3]);
   putchar('\r');
   putchar('\n'); 
   printf(" af0:  %e" ,   af0[0]);
   printf(" af0:  %e" ,   af0[1]);
   printf(" af0:  %e" ,   af0[2]);
   printf(" af0:  %e" ,   af0[3]);
   putchar('\r');
   putchar('\n');
   printf(" aode:  %ld" ,   aode[0]);
   printf(" aode:  %ld" ,   aode[1]);
   printf(" aode:  %ld" ,   aode[2]);
   printf(" aode:  %ld" ,   aode[3]);
   putchar('\r');
   putchar('\n');
   printf(" deltan:  %e" ,   deltan[0]);
   printf(" deltan:  %e" ,   deltan[1]);
   printf(" deltan:  %e" ,   deltan[2]);
   printf(" deltan:  %e" ,   deltan[3]);
   putchar('\r');
   putchar('\n'); 
   printf(" m0:  %e" ,   m0[0]);
   printf(" m0:  %e" ,   m0[1]);
   printf(" m0:  %e" ,   m0[2]);
   printf(" m0:  %e" ,   m0[3]);
   putchar('\r');
   putchar('\n'); 
   printf(" e:  %e" ,   e[0]);
   printf(" e:  %e" ,   e[1]);
   printf(" e:  %e" ,   e[2]);
   printf(" e:  %e" ,   e[3]);
   putchar('\r');
   putchar('\n');  
   printf(" roota:  %e" ,   roota[0]);
   printf(" roota:  %e" ,   roota[1]);
   printf(" roota:  %e" ,   roota[2]);
   printf(" roota:  %e" ,   roota[3]);
   putchar('\r');
   putchar('\n');  
   printf(" toe:  %ld" ,   toe[0]);
   printf(" toe:  %ld" ,   toe[1]);
   printf(" toe:  %ld" ,   toe[2]);
   printf(" toe:  %ld" ,   toe[3]);
   putchar('\r');
   putchar('\n');  
   printf(" cic:  %e" ,   cic[0]);
   printf(" cic:  %e" ,   cic[1]);
   printf(" cic:  %e" ,   cic[2]);
   printf(" cic:  %e" ,   cic[3]);
   putchar('\r');
   putchar('\n'); 
   printf(" crc:  %e" ,   crc[0]);
   printf(" crc:  %e" ,   crc[1]);
   printf(" crc:  %e" ,   crc[2]);
   printf(" crc:  %e" ,   crc[3]);
   putchar('\r');
   putchar('\n');
   printf(" cis:  %e" ,   cis[0]);
   printf(" cis:  %e" ,   cis[1]);
   printf(" cis:  %e" ,   cis[2]);
   printf(" cis:  %e" ,   cis[3]);
   putchar('\r');
   putchar('\n');
   printf(" crs:  %e" ,   crs[0]);
   printf(" crs:  %e" ,   crs[1]);
   printf(" crs:  %e" ,   crs[2]);
   printf(" crs:  %e" ,   crs[3]);
   putchar('\r');
   putchar('\n');
   printf(" cuc:  %e" ,   cuc[0]);
   printf(" cuc:  %e" ,   cuc[1]);
   printf(" cuc:  %e" ,   cuc[2]);
   printf(" cuc:  %e" ,   cuc[3]);
   putchar('\r');
   putchar('\n'); 
   printf(" cus:  %e" ,   cus[0]);
   printf(" cus:  %e" ,   cus[1]);
   printf(" cus:  %e" ,   cus[2]);
   printf(" cus:  %e" ,   cus[3]);
   putchar('\r');
   putchar('\n');
   printf(" omega0:  %e" ,   omega0[0]);
   printf(" omega0:  %e" ,   omega0[1]);
   printf(" omega0:  %e" ,   omega0[2]);
   printf(" omega0:  %e" ,   omega0[3]);
   putchar('\r');
   putchar('\n'); 
   printf(" omega:  %e" ,   omega[0]);
   printf(" omega:  %e" ,   omega[1]);
   printf(" omega:  %e" ,   omega[2]);
   printf(" omega:  %e" ,   omega[3]);
   putchar('\r');
   putchar('\n');
   printf(" i0:  %e" ,   i0[0]);
   printf(" i0:  %e" ,   i0[1]);
   printf(" i0:  %e" ,   i0[2]);
   printf(" i0:  %e" ,   i0[3]);
   putchar('\r');
   putchar('\n'); 
   printf(" omegadot:  %e" ,   omegadot[0]);
   printf(" omegadot:  %e" ,   omegadot[1]);
   printf(" omegadot:  %e" ,   omegadot[2]);
   printf(" omegadot:  %e" ,   omegadot[3]);
   putchar('\r');
   putchar('\n'); 
   printf(" idot:  %e" ,   idot[0]);
   printf(" idot:  %e" ,   idot[1]);
   printf(" idot:  %e" ,   idot[2]);
   printf(" idot:  %e" ,   idot[3]);
   putchar('\r');
   putchar('\n');   
   printf(" accuracy: %i",  accuracy[0]);
   printf(" accuracy: %i",  accuracy[1]);
   printf(" accuracy: %i",  accuracy[2]);
   printf(" accuracy: %i",  accuracy[3]); 
   putchar('\r');
   putchar('\n');
   printf(" health: %i",  health[0]);
   printf(" health: %i",  health[1]);
   printf(" health: %i",  health[2]);
   printf(" health: %i",  health[3]); 
   putchar('\r');
   putchar('\n');
   printf(" fit: %i",  fit[0]);
   printf(" fit: %i",  fit[1]);
   printf(" fit: %i",  fit[2]);
   printf(" fit: %i",  fit[3]); 
   putchar('\r');
   putchar('\n');
   printf(" PRN: %i" , (int) PRN[0]);
   printf(" PRN: %i" , (int) PRN[1]);
   printf(" PRN: %i" , (int) PRN[2]);
   printf(" PRN: %i" , (int) PRN[3]);  
   putchar('\r');
   putchar('\n'); 
   printf(" res: %i" , (int) res[0]);