ahmed ahmed / Ahmed_Embarq_Prog_300415

Ahmed_PFE_Embarq_300415

Dependencies:   C12832 LM75B mbed

Fork of Ahmed_Embarq_Prog by ahmed ahmed

_GPS/Dec_GPS.cpp@1:197b9fed6092, 2015-04-30 (annotated)

Committer:
pfe
Date:
Thu Apr 30 12:24:33 2015 +0000
Revision:
1:197b9fed6092
Ahmed_PFE_Embarq_300415

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pfe 1:197b9fed6092 1 #include "Dec_GPS.h"
pfe 1:197b9fed6092 2
pfe 1:197b9fed6092 3 unsigned char _DL,_Npt,_Pospt;
pfe 1:197b9fed6092 4 unsigned char _Data[16],_PosV[16];
pfe 1:197b9fed6092 5 unsigned char _MsgGPSRx[100];
pfe 1:197b9fed6092 6
pfe 1:197b9fed6092 7 //------------------------------------------------------------------------------
pfe 1:197b9fed6092 8 //Calcul de la longueur des sous message
pfe 1:197b9fed6092 9 unsigned char Get_Length(unsigned short index)
pfe 1:197b9fed6092 10 {
pfe 1:197b9fed6092 11 static unsigned char LongData;
pfe 1:197b9fed6092 12 LongData=_PosV[index+1]-_PosV[index]-1;
pfe 1:197b9fed6092 13 return LongData;
pfe 1:197b9fed6092 14 }
pfe 1:197b9fed6092 15 //Décomposition du message en Sub message
pfe 1:197b9fed6092 16 void Set_Data(unsigned char index){
pfe 1:197b9fed6092 17 static unsigned char ii,jj;
pfe 1:197b9fed6092 18 static unsigned char Start,End;
pfe 1:197b9fed6092 19 ii=0;
pfe 1:197b9fed6092 20 Start = _PosV[index]+1;
pfe 1:197b9fed6092 21 End = _PosV[index+1];
pfe 1:197b9fed6092 22
pfe 1:197b9fed6092 23 for(jj=Start;jj<End;jj++){
pfe 1:197b9fed6092 24 _Data[ii] = _MsgGPSRx[jj];
pfe 1:197b9fed6092 25 ii++;
pfe 1:197b9fed6092 26 }
pfe 1:197b9fed6092 27 }
pfe 1:197b9fed6092 28 //Vérification des sub message
pfe 1:197b9fed6092 29 void Validation_StrToFloat(unsigned char LongData)
pfe 1:197b9fed6092 30 {
pfe 1:197b9fed6092 31 unsigned char ii;
pfe 1:197b9fed6092 32 _Npt = 0;
pfe 1:197b9fed6092 33 _Pospt = 0;
pfe 1:197b9fed6092 34 for(ii=0;ii<LongData;ii++){
pfe 1:197b9fed6092 35 if(_Data[ii]=='.')
pfe 1:197b9fed6092 36 {
pfe 1:197b9fed6092 37 _Npt++;
pfe 1:197b9fed6092 38 _Pospt=ii;
pfe 1:197b9fed6092 39 }
pfe 1:197b9fed6092 40 else if((_Data[ii]<48)||(_Data[ii]>57)) _Npt=_Npt+2;
pfe 1:197b9fed6092 41 }
pfe 1:197b9fed6092 42 }
pfe 1:197b9fed6092 43
pfe 1:197b9fed6092 44 void DecodageGPGGA(unsigned char *_MsgGPS_Rx,unsigned char Ldata, StructGPS *GpsData)
pfe 1:197b9fed6092 45 {
pfe 1:197b9fed6092 46 static unsigned char CommaNbr;
pfe 1:197b9fed6092 47 static unsigned char ii,jj;
pfe 1:197b9fed6092 48 StructGPS L_GpsData;
pfe 1:197b9fed6092 49
pfe 1:197b9fed6092 50 unsigned char _hh,_mm,_ss;
pfe 1:197b9fed6092 51 unsigned char _LatDeg,_LatMin,_LatMin1,_LatMin2,_LatDir;
pfe 1:197b9fed6092 52 unsigned char _LonDeg,_LonMin,_LonMin1,_LonMin2,_LonDir;
pfe 1:197b9fed6092 53 unsigned char _Qual;
pfe 1:197b9fed6092 54 unsigned char _HMSL_L,_HMSL_H;
pfe 1:197b9fed6092 55 char _SignHMSL,_HUnite;
pfe 1:197b9fed6092 56 float _fLatGpsDD,_fLonGpsDD;
pfe 1:197b9fed6092 57
pfe 1:197b9fed6092 58 ii=0;
pfe 1:197b9fed6092 59 while(ii<=Ldata){
pfe 1:197b9fed6092 60 _MsgGPSRx[ii]=*_MsgGPS_Rx;
pfe 1:197b9fed6092 61 _MsgGPS_Rx++;
pfe 1:197b9fed6092 62 ii++;
pfe 1:197b9fed6092 63 }
pfe 1:197b9fed6092 64 L_GpsData.GGA_Valid = 0;
pfe 1:197b9fed6092 65 CommaNbr = 0;
pfe 1:197b9fed6092 66 for(ii=0;ii<Ldata;ii++) {
pfe 1:197b9fed6092 67 if(_MsgGPSRx[ii]==',') {
pfe 1:197b9fed6092 68 _PosV[CommaNbr] = ii;
pfe 1:197b9fed6092 69 CommaNbr++;
pfe 1:197b9fed6092 70 }
pfe 1:197b9fed6092 71 }
pfe 1:197b9fed6092 72 if(CommaNbr<11)goto LabFin; //Vérification de la longueur du message GPGGA
pfe 1:197b9fed6092 73 //par le nombre des vergules
pfe 1:197b9fed6092 74 //UTC of position
pfe 1:197b9fed6092 75 //extraction du temps UTC
pfe 1:197b9fed6092 76 jj=0;//Identification de la position de la première virgule dans le MsgGPS
pfe 1:197b9fed6092 77 _DL=Get_Length(jj);
pfe 1:197b9fed6092 78 if((_DL<9)||(_DL>10)) goto LabFin;//Vérification de la longueur du message de temps
pfe 1:197b9fed6092 79 Set_Data(jj);
pfe 1:197b9fed6092 80 Validation_StrToFloat(_DL);
pfe 1:197b9fed6092 81 if(_Npt !=1) goto LabFin;
pfe 1:197b9fed6092 82 if(_Pospt!=6) goto LabFin;
pfe 1:197b9fed6092 83 //Vérification de Time "hhmmss"
pfe 1:197b9fed6092 84 _hh=10*(_Data[0]-48)+(_Data[1]-48);//Extraction de l'heur
pfe 1:197b9fed6092 85 _mm=10*(_Data[2]-48)+(_Data[3]-48);//Extraction des minutes
pfe 1:197b9fed6092 86 _ss=10*(_Data[4]-48)+(_Data[5]-48);//Extraction des secondes
pfe 1:197b9fed6092 87 if((_hh>24)||(_mm>59)||(_ss>59))goto LabFin;
pfe 1:197b9fed6092 88 //Latitude of position
pfe 1:197b9fed6092 89 //extraction du Lattitude
pfe 1:197b9fed6092 90 jj=1;//Identification de la position de la deuxième virgule dans le MsgGPS
pfe 1:197b9fed6092 91 _DL=Get_Length(jj);
pfe 1:197b9fed6092 92 if((_DL<9)||(_DL>10)) goto LabFin;//Vérification de la longueur du message de lattitude
pfe 1:197b9fed6092 93 Set_Data(jj);
pfe 1:197b9fed6092 94 Validation_StrToFloat(_DL);
pfe 1:197b9fed6092 95 if(_Npt !=1) goto LabFin;
pfe 1:197b9fed6092 96 if(_Pospt!=4) goto LabFin;
pfe 1:197b9fed6092 97
pfe 1:197b9fed6092 98 //Vérification du Lattitude "LatDegLatMin,LatMin1LatMin2"
pfe 1:197b9fed6092 99 _LatDeg =10*(_Data[0]-48)+(_Data[1]-48);//Extraction des degrés du lattitude
pfe 1:197b9fed6092 100 _LatMin =10*(_Data[2]-48)+(_Data[3]-48);//Extraction des minutes du lattitude
pfe 1:197b9fed6092 101 _LatMin1=10*(_Data[5]-48)+(_Data[6]-48);//Extraction des deux chiffres minutes après la virgule du lattitude
pfe 1:197b9fed6092 102 _LatMin2=10*(_Data[7]-48)+(_Data[8]-48);//Extraction des deux chiffres minutes après la virgule du lattitude
pfe 1:197b9fed6092 103 if((_LatDeg>90)||(_LatMin>59)||(_LatMin1>99)||(_LatMin2>99))goto LabFin;
pfe 1:197b9fed6092 104 //Vérification de la direction du Lattitude LatDir(N ou S)
pfe 1:197b9fed6092 105 jj=2;//Identification de la position de la troisième virgule dans le MsgGPS
pfe 1:197b9fed6092 106 _DL=Get_Length(jj);
pfe 1:197b9fed6092 107 if(_DL!=1) goto LabFin;//Vérification de la longueur du message de direction de latittude
pfe 1:197b9fed6092 108 Set_Data(jj);
pfe 1:197b9fed6092 109 _LatDir=_Data[0];//Extraction de le direction du latitude
pfe 1:197b9fed6092 110 if((_LatDir!='N')&&(_LatDir!='S'))goto LabFin;
pfe 1:197b9fed6092 111 // 01 byte
pfe 1:197b9fed6092 112 //Longitude of position
pfe 1:197b9fed6092 113 //extraction de la Longitude
pfe 1:197b9fed6092 114 jj=3;//Identification de la position de la quatrième virgule dans le MsgGPS
pfe 1:197b9fed6092 115 _DL=Get_Length(jj);
pfe 1:197b9fed6092 116 if((_DL<10)||(_DL>11)) goto LabFin;//Vérification de la longueur du message de la longitude
pfe 1:197b9fed6092 117 Set_Data(jj);
pfe 1:197b9fed6092 118 Validation_StrToFloat(_DL);
pfe 1:197b9fed6092 119 if(_Npt !=1) goto LabFin;
pfe 1:197b9fed6092 120 if(_Pospt!=5) goto LabFin;
pfe 1:197b9fed6092 121 //Vérification du La Longitude "LongDegLongMin,LongMin1LongMin2"
pfe 1:197b9fed6092 122 _LonDeg =100*(_Data[0]-48)+10*(_Data[1]-48)+(_Data[2]-48);//Extraction des degrés de la longitude
pfe 1:197b9fed6092 123 _LonMin =10*(_Data[3]-48)+(_Data[4]-48);//Extraction des minutes de la longitude
pfe 1:197b9fed6092 124 _LonMin1=10*(_Data[6]-48)+(_Data[7]-48);//Extraction des deux chiffres minutes après la virgule de la longitude
pfe 1:197b9fed6092 125 _LonMin2=10*(_Data[8]-48)+(_Data[9]-48);//Extraction des deux chiffres minutes après la virgule de la longitude
pfe 1:197b9fed6092 126 if((_LonDeg>180)||(_LonMin>59)||(_LonMin1>99)||(_LonMin2>99))goto LabFin;
pfe 1:197b9fed6092 127 //Vérification de la direction du Longitude LonDir(E ou W)
pfe 1:197b9fed6092 128 jj=4;//Identification de la position de la cinquième virgule dans le MsgGPS
pfe 1:197b9fed6092 129 _DL=Get_Length(jj);
pfe 1:197b9fed6092 130 if(_DL!=1) goto LabFin;//Vérification de la longueur du message de direction de la longitude
pfe 1:197b9fed6092 131 Set_Data(jj);
pfe 1:197b9fed6092 132 _LonDir=_Data[0];//Extraction de le direction de la longitude
pfe 1:197b9fed6092 133 if((_LonDir!='E')&&(_LonDir!='W'))goto LabFin;
pfe 1:197b9fed6092 134 //GPS Quality indicator
pfe 1:197b9fed6092 135 jj=5;//Identification de la position de la sixième virgule dans le MsgGPS
pfe 1:197b9fed6092 136 _DL=Get_Length(jj);
pfe 1:197b9fed6092 137 if(_DL!=1) goto LabFin;//Vérification de la longueur du message de l'ndicateur de qualité GPS
pfe 1:197b9fed6092 138 Set_Data(jj);
pfe 1:197b9fed6092 139 Validation_StrToFloat(_DL);
pfe 1:197b9fed6092 140 if(_Npt !=0) goto LabFin;
pfe 1:197b9fed6092 141 if(_Pospt!=0) goto LabFin;
pfe 1:197b9fed6092 142 _Qual=_Data[0]-48;//Extraction de l'information de la qualité du MsgGPs
pfe 1:197b9fed6092 143 if((_Qual<1)||(_Qual>2))goto LabFin;
pfe 1:197b9fed6092 144 //Antenna altitude above mean-sea-level "SignHMSLHMSL_HHMSL_L,HMSL_1HMSL_2" .
pfe 1:197b9fed6092 145 jj=8;//Identification de la position de la neuvième virgule dans le MsgGPS
pfe 1:197b9fed6092 146 _DL=Get_Length(jj);
pfe 1:197b9fed6092 147 if(_DL<3) goto LabFin;//Vérification de la longueur du message de l'altitude
pfe 1:197b9fed6092 148 Set_Data(jj);
pfe 1:197b9fed6092 149 _SignHMSL='+';
pfe 1:197b9fed6092 150
pfe 1:197b9fed6092 151 if(_Data[0]=='-'){
pfe 1:197b9fed6092 152 _DL=_DL-1;
pfe 1:197b9fed6092 153 _SignHMSL='-';
pfe 1:197b9fed6092 154 for(ii=0;ii<_DL;ii++) _Data[ii]=_Data[ii+1];
pfe 1:197b9fed6092 155 }
pfe 1:197b9fed6092 156 Validation_StrToFloat(_DL);
pfe 1:197b9fed6092 157 if(_Npt !=1) goto LabFin;
pfe 1:197b9fed6092 158 if(_Pospt ==0) goto LabFin;
pfe 1:197b9fed6092 159
pfe 1:197b9fed6092 160 switch (_Pospt) {
pfe 1:197b9fed6092 161 case 1: {_HMSL_L = (_Data[0]-48) ; _HMSL_H = 0; break;}
pfe 1:197b9fed6092 162 case 2: {_HMSL_L = (_Data[1]-48)+10*(_Data[0]-48); _HMSL_H = 0; break;}
pfe 1:197b9fed6092 163 case 3: {_HMSL_L = (_Data[2]-48)+10*(_Data[1]-48); _HMSL_H = (_Data[0]-48); break;}
pfe 1:197b9fed6092 164 case 4: {_HMSL_L = (_Data[3]-48)+10*(_Data[2]-48); _HMSL_H = (_Data[1]-48)+10*(_Data[0]-48); break;}
pfe 1:197b9fed6092 165 default : break;
pfe 1:197b9fed6092 166 }
pfe 1:197b9fed6092 167 //Antenna height unit
pfe 1:197b9fed6092 168 jj=9; // Identification de la position de la dixième virgule dans le MsgGPS
pfe 1:197b9fed6092 169 _DL=Get_Length(jj);
pfe 1:197b9fed6092 170 if(_DL!=1) goto LabFin;//Vérification de la longueur du message de l'unité de la hauteur
pfe 1:197b9fed6092 171 Set_Data(jj);
pfe 1:197b9fed6092 172 _HUnite=_Data[0];//Extraction de l'unité de la hauteur
pfe 1:197b9fed6092 173 if((_HMSL_H>35)||(_HUnite!='M'))goto LabFin;
pfe 1:197b9fed6092 174
pfe 1:197b9fed6092 175 L_GpsData.hh = _hh;
pfe 1:197b9fed6092 176 L_GpsData.mm = _mm;
pfe 1:197b9fed6092 177 L_GpsData.ss = _ss;
pfe 1:197b9fed6092 178 //----------------------------------------------------------------------------
pfe 1:197b9fed6092 179 L_GpsData.LatDeg = _LatDeg;
pfe 1:197b9fed6092 180 //-----------------------------------------------------------------------------
pfe 1:197b9fed6092 181 _fLatGpsDD = 1000000.0*_LatDeg+100000.0*_LatMin/6.0+10.0*(_LatMin1*100+_LatMin2)/6.0;
pfe 1:197b9fed6092 182 _fLonGpsDD = 1000000.0*_LonDeg+100000.0*_LonMin/6.0+10.0*(_LonMin1*100+_LonMin2)/6.0;
pfe 1:197b9fed6092 183
pfe 1:197b9fed6092 184 L_GpsData.LatDeg = (_fLatGpsDD);//*0.99998537);
pfe 1:197b9fed6092 185 L_GpsData.LonDeg = (_fLonGpsDD);//*0.99981997);
pfe 1:197b9fed6092 186 //-----------------------------------------------------------------------------
pfe 1:197b9fed6092 187 L_GpsData.Qual = _Qual;
pfe 1:197b9fed6092 188 //-----------------------------------------------------------------------------
pfe 1:197b9fed6092 189 L_GpsData.HMSL = _HMSL_H*100+_HMSL_L;
pfe 1:197b9fed6092 190 if (_SignHMSL=='-') L_GpsData.HMSL = -L_GpsData.HMSL;
pfe 1:197b9fed6092 191 L_GpsData.GGA_Valid = 1;
pfe 1:197b9fed6092 192 *GpsData=L_GpsData;
pfe 1:197b9fed6092 193 LabFin:
pfe 1:197b9fed6092 194 asm{NOP}
pfe 1:197b9fed6092 195 }