gps.c

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

#include <math.h>
#include <stdio.h>
#include <string.h>
#include <delay.h>
#include "constant.h"


int  sv[12];
float range[12]; 
float ReceiverECEFx[12]; //Satellites position in ECEF
float ReceiverECEFy[12];
float ReceiverECEFz[12]; 
float Receiverlon[12]; //Satellites position in ECEF
float Receiverlat[12];
float Receiveralt[12];
int totalsatellites;//how many satelliets are tracked and can be used to calculate navigation solution 
int checksum1;
char creturn1;
char cnewline1;

/*CurrentTime*/
long currentTime;

float userlon;//User calculated position
float userlat;
float useralt;

float guessx=  1673462.19520558  ;//Initial guess p
float guessy=  -4597799.59363829   ;
float guessz=4078126.98547942;

float satelliteslon[4]; //Satellites position in WGS84
float satelliteslat[4];
float satellitesalt[4];

float satellitesECEFx[4]; //Satellites position in ECEF
float satellitesECEFy[4];
float satellitesECEFz[4];


         	
/*Pseudo*/
float pseudoR[4];

/*Ephemeris*/ 
//" wn: " 
int wn[4];
//" tow: "
long   tow[4];
//" tgd: "
float tgd[4];
//" aodc: "
long  aodc[4];
//" toc: " 
long  toc[4];
//" af2: "
float af2[4];
//" af1: "
float af1[4];
//" af0: " 
float af0[4];
//" aode: " 
long aode[4];
//" deltan: " 
float deltan[4]; 
//" m0: "  
float m0[4];
//" e: " 
float e[4];
//" roota: " 
float roota[4];  
//" toe: " 
long  toe[4];
//" cic: " 
float cic[4];
//" crc: " 
float crc[4];
//" cis: "  
float cis[4];
//" crs: "
float crs[4];
//" cuc: " 
float cuc[4];
//" cus: " 
float cus[4];   
//" omega0: "
float omega0[4];
//" omega: " 
float omega[4];
//" i0: "
float i0[4];  
//" omegadot: "
float omegadot[4];           
//" idot: " 
float idot[4];   
//" accuracy: "
int accuracy[4];
//" health: "
int health[4];
//" fit: "  
int fit[4];
//" PRN: " 
char PRN[4];
//" res: " 
char res[4];
//" checksum: " 
int checksum[4];
//" creturn: " 
char creturn[4]; 
//" cnewline: " 
char cnewline[4];

void solvePos(void); 
void FindSat(char l);
void latlong( float ECEFx, float ECEFy, float ECEFz, char l,char type);

void SendEphem(void);
void SendSatellitesPositions(void);
void SendReceiverPositions(void); 
void SendSatellitesCoordinates(void);
void SendNavigationalSolution(void);
void SendPackage(void);

void GoIntoDaisyChainmode(void);
void GoOutofDaisyChainmode(void); 
void GetCurrentTime(void);
void GetEphem(void);

void ParseEphem(char l);
void ParseCurrentTime(void);  

float ReadFloat(void);
char  ReadChar(void);
unsigned long  ReadLong(void);
float ReadDouble(void);
int ReadShort(void);

void init(void);

void main(void)
begin 
   init();
   while(1)
   begin
   UCSRB = 0b00011000; 
   GetCurrentTime();
   ParseCurrentTime(); 
   GetEphem(); 
   ParseEphem(0);  
   ParseEphem(1);  
   ParseEphem(2);  
   ParseEphem(3);             
   FindSat(0);
   FindSat(1);
   FindSat(2); 
   FindSat(3); 
   SendPackage();
   delay_ms(1000);//in case we want to slow down the pace at which information is being sent
   UCSRB = 0b00000000; //hex 0x18
   end
end
 
/*
 *The following fucntion sets up the UART for serial communication withe the receiver
 *and the Satellite Tracker  
 */
//*****************************************************   
void init(void)
begin    
  //For Mega32: 
  //serial setup  
  //receive-complete interrupt 
  //Bit 4 enables the receiver
  //, and bit 3 the transmitter.
  UCSRB = 0b00011000; //hex 0x18
  UBRRL = 103; //using a 16 MHz crystal (9600 baud)           
 // enable interrupts
  #asm("sei")
end 

//This function calculates the position of the observation station
//*****************************************************

void solvePos(void)
begin   
int i =1; //necessary variables for Gauss Jordan!
int j=1;
int k =1;
int p; 
int n =4;
int m=1;
float T[5];
float ml[4][5];
float x[4];
float cl[4][5]; //augemnted matrix for Gauss-Jordan
float difference=10000;//for convergence;
float obsPos[3];
float satX[4];
float satY[4];
float satZ[4];
float delXYZ[4][3];
float a[4][4];
float b[4];
float rangel[4];
int iters,stop; 
iters=0;
stop=0;

satX[0]= ReceiverECEFx[0]; 
satX[1]= ReceiverECEFx[1];  
satX[2]= ReceiverECEFx[2];  
satX[3]= ReceiverECEFx[3]; 

satY[0]= ReceiverECEFy[0];   
satY[1]= ReceiverECEFy[1]; 
satY[2]= ReceiverECEFy[2];
satY[3]= ReceiverECEFy[3];

satZ[0]= ReceiverECEFz[0]; 
satZ[1]= ReceiverECEFz[1];
satZ[2]= ReceiverECEFz[2];
satZ[3]= ReceiverECEFz[3]; 

pseudoR[0] =range[0]*c; 
pseudoR[1] =range[1]*c;
pseudoR[2] =range[2]*c;
pseudoR[3] =range[3]*c*100000000;




//solve for position iteratively until solution is within an 
// acceptable error
   while (stop == 0)
   begin
      iters=iters+1;
      		  
      delXYZ[0][0] =  (satX[0]-guessx); 
      delXYZ[1][0] =  (satX[1]-guessx); 
      delXYZ[2][0] =  (satX[2]-guessx); 
      delXYZ[3][0] =  (satX[3]-guessx);  
      
      delXYZ[0][1] =   (satY[0]-guessy); 
      delXYZ[1][1] =   (satY[1]-guessy); 
      delXYZ[2][1] =  (satY[2]-guessy); 
      delXYZ[3][1] =   (satY[3]-guessy); 
      
      delXYZ[0][2] =   (satZ[0]-guessz);
      delXYZ[1][2] =  (satZ[1]-guessz);
      delXYZ[2][2] =   (satZ[2]-guessz);
      delXYZ[3][2] =   (satZ[3]-guessz);
      
      
      rangel[0] =  sqrt(pow((satX[0]-guessx),2) + pow((satY[0]-guessy),2) + pow((satZ[0]-guessz),2));     
      rangel[1] =  sqrt(pow((satX[1]-guessx),2) + pow((satY[1]-guessy),2) + pow((satZ[1]-guessz),2));  
      rangel[2] =  sqrt(pow((satX[2]-guessx),2) + pow((satY[2]-guessy),2) + pow((satZ[2]-guessz),2));   
      rangel[3]=   sqrt(pow((satX[3]-guessx),2) + pow((satY[3]-guessy),2) + pow((satZ[3]-guessz),2)); 
      

   //  form the vector 'l' and the matrix 'A'    
   b[0] =   pseudoR[0]-rangel[0];
   b[1] =   pseudoR[1]-rangel[1]; 
   b[2] =   pseudoR[2]-rangel[2];  
   b[3] =   pseudoR[3]-rangel[3];   
       
    
            
      //Example of A entry: d/dx(range(1))=-(Xsat-Xguess)/Range(guess)
      //range(j)=> of jth sat, delXYZ(j,i)=>jth sat, i is X,Y,orZ 
 
        
     a[0][0] =  -(delXYZ[0][0])/rangel[0]; 
     a[1][0] =   -(delXYZ[1][0])/rangel[1]; 
     a[2][0] =  -(delXYZ[2][0])/rangel[2]; 
     a[3][0] =  -(delXYZ[3][0])/rangel[3]; 

     a[0][1] =  -(delXYZ[0][1])/rangel[0]; 
     a[1][1] =  -(delXYZ[1][1])/rangel[1]; 
     a[2][1] =  -(delXYZ[2][1])/rangel[2]; 
     a[3][1] =  -(delXYZ[3][1])/rangel[3]; 
     
     a[0][2] =  -(delXYZ[0][2])/rangel[0]; 
     a[1][2] =  -(delXYZ[1][2])/rangel[1]; 
     a[2][2] =  -(delXYZ[2][2])/rangel[2]; 
     a[3][2] =  -(delXYZ[3][2])/rangel[3]; 
      
     a[0][3] =  -1.0; 
     a[1][3] =  -1.0; 
     a[2][3] =  -1.0; 
     a[3][3] =  -1.0; 
    
  
        
   //solve for  'deltaPos' which is contains dx, dy, dz, and  dt
 
    //produces the augmented matrix
     cl[0][0] =   a[0][0];
     cl[1][0] =   a[1][0];
     cl[2][0] =   a[2][0];
     cl[3][0] =   a[3][0];

     cl[0][1] =   a[0][1]; 
     cl[1][1] =   a[1][1];
     cl[2][1] =   a[2][1];
     cl[3][1] =   a[3][1];
     
     cl[0][2] =  a[0][2];
     cl[1][2] =  a[1][2];
     cl[2][2] =  a[2][2]; 
     cl[3][2] =  a[3][2]; 
      
     cl[0][3] =  a[0][3]; 
     cl[1][3] =  a[1][3];
     cl[2][3] =  a[2][3];
     cl[3][3] =  a[3][3];  

     cl[0][4] =  b[0];
     cl[1][4] =  b[1];
     cl[2][4] =  b[2];
     cl[3][4] =  b[3];

//elimination process
for (i = 0;i<=n-1;i++)
begin
    p = i;
    //trivial pivoting will take place if one of the pivot is zero
    while (cl[p][i]== 0 && p <= n-1)
    begin
        p = p+1;
    end
    if (p == n)
    begin
        printf("No unique solution1");
        break;
    end
    else
    begin
        if(p != i)
        begin
            T[0] = cl[i][0];
            T[1] = cl[i][1];
            T[2] = cl[i][2];
            T[3] = cl[i][3];
            T[4] = cl[i][4];
            
            cl[i][0] = cl[p][0];
            cl[i][1] = cl[p][1];
            cl[i][2] = cl[p][2];
            cl[i][3] = cl[p][3];
            cl[i][4] = cl[p][4];
             
            cl[p][0] =  T[0];
            cl[p][1] =  T[1];
            cl[p][2] =  T[2];
            cl[p][3] =  T[3];
            cl[p][4] =  T[4];        
        end
    end
 


    for (j = 0;j<=i-1;j++)
    begin
        ml[j][i] = cl[j][i]/cl[i][i];
        for (k = i+1;k<=(n-1+m); k++) 
        begin
            cl[j][k] = cl[j][k] - ml[j][i]*cl[i][k];
        end
        
    end
    
    for (j = i+1;j<=n-1;j++)
    begin
        ml[j][i] = cl[j][i]/cl[i][i];
        for(k = i+1;k<=(n-1+m); k++) 
        begin
            cl[j][k] = cl[j][k] - ml[j][i]*cl[i][k];
        end
    end
end



//the solutions
for(p = 0; p<=m-1; p++)
begin
    x[n] = cl[n][n+p]/cl[n][n];
end