Diff: _GPS/Dec_GPS.cpp

Revision:

1:197b9fed6092

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/_GPS/Dec_GPS.cpp	Thu Apr 30 12:24:33 2015 +0000
@@ -0,0 +1,195 @@
+#include "Dec_GPS.h"
+
+ unsigned char _DL,_Npt,_Pospt;
+ unsigned char _Data[16],_PosV[16];
+ unsigned char _MsgGPSRx[100];   
+
+//------------------------------------------------------------------------------
+//Calcul de la longueur des sous message
+unsigned char Get_Length(unsigned short index)
+ {
+    static unsigned char LongData;
+    LongData=_PosV[index+1]-_PosV[index]-1;
+    return LongData;
+ }
+//Décomposition du message en Sub message
+void Set_Data(unsigned char index){
+    static unsigned char ii,jj;
+    static unsigned char Start,End;
+    ii=0;
+    Start = _PosV[index]+1;    
+    End   = _PosV[index+1];  
+    
+    for(jj=Start;jj<End;jj++){
+       _Data[ii] = _MsgGPSRx[jj];      
+       ii++;
+    }
+ } 
+//Vérification des sub message
+void Validation_StrToFloat(unsigned char LongData)
+ {
+    unsigned char ii;
+    _Npt   = 0;
+    _Pospt = 0;
+    for(ii=0;ii<LongData;ii++){
+       if(_Data[ii]=='.')
+        {
+          _Npt++;
+          _Pospt=ii;
+        }
+        else if((_Data[ii]<48)||(_Data[ii]>57)) _Npt=_Npt+2;
+     }
+ }
+
+void DecodageGPGGA(unsigned char *_MsgGPS_Rx,unsigned char Ldata, StructGPS *GpsData)
+{
+    static unsigned char CommaNbr;
+    static unsigned char ii,jj;
+    StructGPS L_GpsData;
+    
+    unsigned char _hh,_mm,_ss;
+    unsigned char _LatDeg,_LatMin,_LatMin1,_LatMin2,_LatDir;
+    unsigned char _LonDeg,_LonMin,_LonMin1,_LonMin2,_LonDir;
+    unsigned char _Qual;
+    unsigned char _HMSL_L,_HMSL_H;
+    char          _SignHMSL,_HUnite;
+    float         _fLatGpsDD,_fLonGpsDD;
+     
+    ii=0;
+    while(ii<=Ldata){
+     _MsgGPSRx[ii]=*_MsgGPS_Rx;
+     _MsgGPS_Rx++;
+     ii++;
+    }
+    L_GpsData.GGA_Valid  = 0;
+    CommaNbr = 0;
+    for(ii=0;ii<Ldata;ii++) {
+     if(_MsgGPSRx[ii]==',') {
+        _PosV[CommaNbr] = ii;
+        CommaNbr++;
+      }
+    }
+    if(CommaNbr<11)goto LabFin; //Vérification de la longueur du message GPGGA
+                                //par le nombre des vergules
+//UTC of position
+    //extraction du temps UTC
+    jj=0;//Identification de la position de la première virgule dans le MsgGPS
+    _DL=Get_Length(jj);
+    if((_DL<9)||(_DL>10)) goto LabFin;//Vérification de la longueur du message de temps
+    Set_Data(jj);
+    Validation_StrToFloat(_DL);
+    if(_Npt  !=1)    goto LabFin;
+    if(_Pospt!=6)    goto LabFin;
+    //Vérification de Time "hhmmss"
+    _hh=10*(_Data[0]-48)+(_Data[1]-48);//Extraction de l'heur
+    _mm=10*(_Data[2]-48)+(_Data[3]-48);//Extraction des minutes
+    _ss=10*(_Data[4]-48)+(_Data[5]-48);//Extraction des secondes
+    if((_hh>24)||(_mm>59)||(_ss>59))goto LabFin;
+//Latitude of position
+    //extraction du Lattitude
+    jj=1;//Identification de la position de la deuxième virgule dans le MsgGPS
+    _DL=Get_Length(jj);
+    if((_DL<9)||(_DL>10)) goto LabFin;//Vérification de la longueur du message de lattitude
+    Set_Data(jj);
+    Validation_StrToFloat(_DL);
+    if(_Npt  !=1)    goto LabFin;
+    if(_Pospt!=4)    goto LabFin;
+    
+    //Vérification du Lattitude "LatDegLatMin,LatMin1LatMin2"
+    _LatDeg =10*(_Data[0]-48)+(_Data[1]-48);//Extraction des degrés du lattitude
+    _LatMin =10*(_Data[2]-48)+(_Data[3]-48);//Extraction des minutes du lattitude
+    _LatMin1=10*(_Data[5]-48)+(_Data[6]-48);//Extraction des deux chiffres minutes après la virgule du lattitude
+    _LatMin2=10*(_Data[7]-48)+(_Data[8]-48);//Extraction des deux chiffres minutes après la virgule du lattitude
+    if((_LatDeg>90)||(_LatMin>59)||(_LatMin1>99)||(_LatMin2>99))goto LabFin;
+    //Vérification de la direction du Lattitude LatDir(N ou S)
+    jj=2;//Identification de la position de la troisième virgule dans le MsgGPS
+    _DL=Get_Length(jj);
+    if(_DL!=1) goto LabFin;//Vérification de la longueur du message de direction de latittude
+    Set_Data(jj);
+    _LatDir=_Data[0];//Extraction de le direction du latitude
+    if((_LatDir!='N')&&(_LatDir!='S'))goto LabFin;
+    // 01 byte
+    //Longitude of position
+    //extraction de la Longitude
+    jj=3;//Identification de la position de la quatrième virgule dans le MsgGPS
+    _DL=Get_Length(jj);
+    if((_DL<10)||(_DL>11)) goto LabFin;//Vérification de la longueur du message de la longitude
+    Set_Data(jj);
+    Validation_StrToFloat(_DL);
+    if(_Npt  !=1)    goto LabFin;
+    if(_Pospt!=5)    goto LabFin;
+    //Vérification du La Longitude "LongDegLongMin,LongMin1LongMin2"
+    _LonDeg =100*(_Data[0]-48)+10*(_Data[1]-48)+(_Data[2]-48);//Extraction des degrés de la longitude
+    _LonMin =10*(_Data[3]-48)+(_Data[4]-48);//Extraction des minutes de la longitude
+    _LonMin1=10*(_Data[6]-48)+(_Data[7]-48);//Extraction des deux chiffres minutes après la virgule de la longitude
+    _LonMin2=10*(_Data[8]-48)+(_Data[9]-48);//Extraction des deux chiffres minutes après la virgule de la longitude
+    if((_LonDeg>180)||(_LonMin>59)||(_LonMin1>99)||(_LonMin2>99))goto LabFin;
+    //Vérification de la direction du Longitude LonDir(E ou W)
+    jj=4;//Identification de la position de la cinquième virgule dans le MsgGPS
+    _DL=Get_Length(jj);
+    if(_DL!=1) goto LabFin;//Vérification de la longueur du message de direction de la longitude
+    Set_Data(jj);
+    _LonDir=_Data[0];//Extraction de le direction de la longitude
+    if((_LonDir!='E')&&(_LonDir!='W'))goto LabFin;
+//GPS Quality indicator
+    jj=5;//Identification de la position de la sixième virgule dans le MsgGPS
+    _DL=Get_Length(jj);
+    if(_DL!=1) goto LabFin;//Vérification de la longueur du message de l'ndicateur de qualité GPS
+    Set_Data(jj);
+    Validation_StrToFloat(_DL);
+    if(_Npt  !=0)    goto LabFin;
+    if(_Pospt!=0)    goto LabFin;
+    _Qual=_Data[0]-48;//Extraction de l'information de la qualité du MsgGPs
+    if((_Qual<1)||(_Qual>2))goto LabFin;
+//Antenna altitude above mean-sea-level "SignHMSLHMSL_HHMSL_L,HMSL_1HMSL_2"  .
+    jj=8;//Identification de la position de la neuvième virgule dans le MsgGPS
+    _DL=Get_Length(jj);
+    if(_DL<3) goto LabFin;//Vérification de la longueur du message de l'altitude
+    Set_Data(jj);
+    _SignHMSL='+';
+    
+    if(_Data[0]=='-'){
+      _DL=_DL-1;
+      _SignHMSL='-';
+      for(ii=0;ii<_DL;ii++) _Data[ii]=_Data[ii+1];
+    }
+    Validation_StrToFloat(_DL);
+    if(_Npt   !=1)  goto LabFin;
+    if(_Pospt ==0)  goto LabFin;  
+      
+    switch (_Pospt) {
+      case 1: {_HMSL_L = (_Data[0]-48)                 ; _HMSL_H = 0; break;}
+      case 2: {_HMSL_L = (_Data[1]-48)+10*(_Data[0]-48); _HMSL_H = 0; break;}
+      case 3: {_HMSL_L = (_Data[2]-48)+10*(_Data[1]-48); _HMSL_H = (_Data[0]-48); break;}
+      case 4: {_HMSL_L = (_Data[3]-48)+10*(_Data[2]-48); _HMSL_H = (_Data[1]-48)+10*(_Data[0]-48); break;}
+      default : break;
+    }
+    //Antenna height unit
+     jj=9; // Identification de la position de la dixième virgule dans le MsgGPS
+    _DL=Get_Length(jj);
+    if(_DL!=1) goto LabFin;//Vérification de la longueur du message de l'unité de la hauteur
+    Set_Data(jj);
+    _HUnite=_Data[0];//Extraction de l'unité de la hauteur
+    if((_HMSL_H>35)||(_HUnite!='M'))goto LabFin;
+
+          L_GpsData.hh = _hh;
+          L_GpsData.mm = _mm;
+          L_GpsData.ss = _ss;
+         //----------------------------------------------------------------------------
+          L_GpsData.LatDeg   = _LatDeg;
+         //-----------------------------------------------------------------------------
+          _fLatGpsDD = 1000000.0*_LatDeg+100000.0*_LatMin/6.0+10.0*(_LatMin1*100+_LatMin2)/6.0;
+          _fLonGpsDD = 1000000.0*_LonDeg+100000.0*_LonMin/6.0+10.0*(_LonMin1*100+_LonMin2)/6.0;
+          
+          L_GpsData.LatDeg = (_fLatGpsDD);//*0.99998537);
+          L_GpsData.LonDeg = (_fLonGpsDD);//*0.99981997);              
+         //-----------------------------------------------------------------------------
+          L_GpsData.Qual     = _Qual;
+         //-----------------------------------------------------------------------------
+          L_GpsData.HMSL     = _HMSL_H*100+_HMSL_L;
+          if (_SignHMSL=='-') L_GpsData.HMSL = -L_GpsData.HMSL;          
+          L_GpsData.GGA_Valid = 1;
+          *GpsData=L_GpsData;
+LabFin:
+asm{NOP}
+}
\ No newline at end of file