gps.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 void UpdateField(void);
00024 int32 ConvertGPSToM(int32);
00025 int32 ConvertMToGPS(int32);
00026 int24 ConvertInt(uint8, uint8);
00027 real32 ConvertReal(uint8, uint8);
00028 int32 ConvertLatLonM(uint8, uint8);
00029 void ConvertUTime(uint8, uint8);
00030 void ConvertUDate(uint8, uint8);
00031 void ParseGPGGASentence(void);
00032 void ParseGPRMCSentence(void);
00033 void SetGPSOrigin(void);
00034 void ParseGPSSentence(void);
00035 void RxGPSPacket(uint8);
00036 void SetGPSOrigin(void);
00037 void DoGPS(void);
00038 void GPSTest(void);
00039 void UpdateGPS(void);
00040 void InitGPS(void);
00041 
00042 const uint8 NMEATags[MAX_NMEA_SENTENCES][5]= {
00043     // NMEA
00044     {'G','P','G','G','A'}, // full positioning fix
00045     {'G','P','R','M','C'}, // main current position and heading
00046 };
00047 
00048 NMEAStruct NMEA;
00049 
00050 uint8     GPSPacketTag;
00051 struct    tm  GPSTime;
00052 int32     GPSStartTime, GPSSeconds;
00053 int32     GPSLatitude, GPSLongitude;
00054 int32     OriginLatitude, OriginLongitude;
00055 int32     DesiredLatitude, DesiredLongitude;
00056 int32     LatitudeP, LongitudeP, HoldLatitude, HoldLongitude;
00057 real32    GPSAltitude, GPSRelAltitude, GPSOriginAltitude;
00058 real32    GPSLongitudeCorrection;
00059 real32    GPSHeading, GPSMagHeading, GPSMagDeviation, GPSVel, GPSVelp, GPSROC;
00060 int8      GPSNoOfSats;
00061 int8      GPSFix;
00062 int16     GPSHDilute;
00063 
00064 int32     FakeGPSLongitude, FakeGPSLatitude;
00065 
00066 uint8     ll, tt, ss, RxCh;
00067 uint8     GPSCheckSumChar, GPSTxCheckSum;
00068 uint8     nll, cc, lo, hi;
00069 boolean   EmptyField;
00070 int16     ValidGPSSentences;
00071 real32    GPSdT, GPSdTR;
00072 uint32    GPSuSp;
00073 
00074 int32     SumGPSRelAltitude, SumBaroRelAltitude;
00075 
00076 int32 ConvertGPSToM(int32 c) {   // approximately 1.8553257183 cm per LSB at the Equator
00077     // conversion max is 21Km
00078     return ( ((int32)c * (int32)1855)/((int32)100000) );
00079 } // ConvertGPSToM
00080 
00081 int32 ConvertMToGPS(int32 c) {
00082     // conversion max is 21Km
00083     return ( ((int32)c * (int32)100000)/((int32)1855) );
00084 } // ConvertMToGPS
00085 
00086 int24 ConvertInt(uint8 lo, uint8 hi) {
00087     static uint8 i;
00088     static int24 r;
00089 
00090     r = 0;
00091     if ( !EmptyField )
00092         for (i = lo; i <= hi ; i++ )
00093             r = r * 10 + NMEA.s[i] - '0';
00094 
00095     return (r);
00096 } // ConvertInt
00097 
00098 real32 ConvertReal(uint8 lo, uint8 hi) {
00099     int16 i, n, dp;
00100     boolean Positive;
00101     int16 whole;
00102     real32 rval;
00103 
00104     if (EmptyField)
00105         rval=0.0;
00106     else {
00107         if (NMEA.s[lo]=='-') {
00108             Positive=false;
00109             lo++;
00110         } else
00111             Positive=true;
00112 
00113         dp=lo;
00114         while ((NMEA.s[dp] != '.')&&(dp<=hi))
00115             dp++;
00116 
00117         whole=ConvertInt(lo, dp-1);
00118         rval=ConvertInt(dp + 1, hi);
00119 
00120         n=hi-dp;
00121         for (i=1;i<=n;i++)
00122             rval/=10.0;
00123 
00124         if (Positive)
00125             rval=(whole+rval);
00126         else
00127             rval=-(whole+rval);
00128     }
00129 
00130     return(rval);
00131 } // ConvertReal
00132 
00133 int32 ConvertLatLonM(uint8 lo, uint8 hi) {    // NMEA coordinates normally assumed as DDDMM.MMMMM ie 5 decimal minute digits
00134     // but code can deal with 4 and 5 decimal minutes
00135     // Positions are stored at 5 decimal minute NMEA resolution which is
00136     // approximately 1.855 cm per LSB at the Equator.
00137     static int32 dd, mm, dm, r;
00138     static uint8 dp;
00139 
00140     r = 0;
00141     if ( !EmptyField ) {
00142         dp = lo + 4; // could do this in initialisation for Lat and Lon?
00143         while ( NMEA.s[dp] != '.') dp++;
00144 
00145         dd = ConvertInt(lo, dp - 3);
00146         mm = ConvertInt(dp - 2 , dp - 1);
00147         if ( ( hi - dp ) > (uint8)4 )
00148             dm = ConvertInt(dp + 1, dp + 5);
00149         else
00150             dm = ConvertInt(dp + 1, dp + 4) * 10L;
00151 
00152         r = dd * 6000000 + mm * 100000 + dm;
00153     }
00154 
00155     return(r);
00156 } // ConvertLatLonM
00157 
00158 void ConvertUTime(uint8 lo, uint8 hi) {
00159 
00160     if ( !EmptyField ) {
00161         GPSTime.tm_hour = ConvertInt(lo, lo+1);
00162         GPSTime.tm_min = ConvertInt(lo+2, lo+3);
00163         GPSTime.tm_sec = ConvertInt(lo+4, lo+5);
00164         GPSSeconds = (int32)GPSTime.tm_hour * 3600 + GPSTime.tm_min * 60 + GPSTime.tm_sec;
00165     }
00166 } // ConvertUTime
00167 
00168 void ConvertUDate(uint8 lo, uint8 hi) {
00169 
00170     static time_t seconds;
00171 
00172 //   if ( !EmptyField && !F.RTCValid )
00173     {
00174         GPSTime.tm_mday = ConvertInt(lo, lo + 1);
00175         GPSTime.tm_mon = ConvertInt(lo + 2, lo + 3) - 1;
00176         GPSTime.tm_year = ConvertInt( lo + 4, hi ) + 100;
00177         seconds = mktime ( &GPSTime );
00178         set_time( seconds );
00179 
00180         F.RTCValid = true;
00181     }
00182 
00183 } // ConvertUDate
00184 
00185 void UpdateField(void) {
00186     static uint8 ch;
00187 
00188     lo = cc;
00189 
00190     ch = NMEA.s[cc];
00191     while (( ch != ',' ) && ( ch != '*' ) && ( cc < nll ))
00192         ch = NMEA.s[++cc];
00193 
00194     hi = cc - 1;
00195     cc++;
00196     EmptyField = hi < lo;
00197 } // UpdateField
00198 
00199 void ParseGPGGASentence(void) {    // full position $GPGGA fix
00200 
00201     UpdateField();
00202 
00203     UpdateField();   //UTime
00204     ConvertUTime(lo,hi);
00205 
00206     UpdateField();       //Lat
00207     GPSLatitude = ConvertLatLonM(lo, hi);
00208     UpdateField();   //LatH
00209     if (NMEA.s[lo] == 'S') GPSLatitude = -GPSLatitude;
00210 
00211     UpdateField();       //Lon
00212     GPSLongitude = ConvertLatLonM(lo, hi);
00213     UpdateField();       //LonH
00214     if (NMEA.s[lo] == 'W') GPSLongitude = -GPSLongitude;
00215 
00216     UpdateField();       //Fix
00217     GPSFix = (uint8)(ConvertInt(lo,hi));
00218 
00219     UpdateField();       //Sats
00220     GPSNoOfSats = (uint8)(ConvertInt(lo,hi));
00221 
00222     UpdateField();       // HDilute
00223     GPSHDilute = ConvertInt(lo, hi-3) * 100 + ConvertInt(hi-1, hi); // Cm
00224 
00225     UpdateField();       // Alt
00226     GPSAltitude = real32(ConvertInt(lo, hi-2) * 10L + ConvertInt(hi, hi)); // Metres
00227 
00228     //UpdateField();   // AltUnit - assume Metres!
00229 
00230     //UpdateField();   // GHeight
00231     //UpdateField();   // GHeightUnit
00232 
00233     F.GPSValid = (GPSFix >= GPS_MIN_FIX) && ( GPSNoOfSats >= GPS_MIN_SATELLITES );
00234 
00235     if ( State == InFlight ) {
00236         if ( GPSHDilute > Stats[MaxHDiluteS] ) {
00237             Stats[MaxHDiluteS] = GPSHDilute;
00238             F.GPSFailure = GPSHDilute > 150;
00239         } else
00240             if ( GPSHDilute < Stats[MinHDiluteS] )
00241                 Stats[MinHDiluteS] = GPSHDilute;
00242 
00243         if ( GPSNoOfSats > Stats[GPSMaxSatsS] )
00244             Stats[GPSMaxSatsS] = GPSNoOfSats;
00245         else
00246             if ( GPSNoOfSats < Stats[GPSMinSatsS] )
00247                 Stats[GPSMinSatsS] = GPSNoOfSats;
00248     }
00249 } // ParseGPGGASentence
00250 
00251 void ParseGPRMCSentence() { // main current position and heading
00252 
00253     // uint32 UTime;
00254 
00255     UpdateField();
00256 
00257     UpdateField();   //UTime
00258     //UTime = ConvertUTime(lo,hi);
00259     // GPSMissionTime =(int) (UTime-GPSStartTime);
00260 
00261     UpdateField();
00262     if ( NMEA.s[lo] == 'A' ) {
00263         UpdateField();   //Lat
00264         GPSLatitude = ConvertLatLonM(lo,hi);
00265         UpdateField();   //LatH
00266         if (NMEA.s[lo] == 'S')
00267             GPSLatitude= -GPSLatitude;
00268 
00269         UpdateField();   //Lon
00270         GPSLongitude = ConvertLatLonM(lo,hi);
00271 
00272         UpdateField();   //LonH
00273         if ( NMEA.s[lo] == 'W' )
00274            GPSLongitude = -GPSLongitude;
00275 
00276         UpdateField();   // Groundspeed (Knots)
00277         GPSVel = ConvertReal(lo, hi) * 0.514444444; // KTTOMPS
00278 
00279         UpdateField();   // True course made good (Degrees)
00280         GPSHeading = ConvertReal(lo, hi) * DEGRAD;
00281 
00282         UpdateField();   //UDate
00283         ConvertUDate(lo, hi);
00284 
00285         UpdateField();   // Magnetic Deviation (Degrees)
00286         GPSMagDeviation = ConvertReal(lo, hi) * DEGRAD;
00287 
00288         UpdateField();   // East/West
00289         if ( NMEA.s[lo] == 'W')
00290             GPSMagHeading = GPSHeading - GPSMagDeviation; // need to check sign????
00291         else
00292             GPSMagHeading = GPSHeading +  GPSMagDeviation;
00293         F.HaveGPRMC = true;
00294     } else
00295         F.HaveGPRMC = false;
00296 
00297 } // ParseGPRMCSentence
00298 
00299 void SetGPSOrigin(void) {
00300     if ( ( ValidGPSSentences == GPS_ORIGIN_SENTENCES ) && F.GPSValid ) {
00301         GPSStartTime = GPSSeconds;
00302         OriginLatitude = DesiredLatitude = HoldLatitude = LatitudeP = GPSLatitude;
00303         OriginLongitude = DesiredLongitude = HoldLongitude = LongitudeP = GPSLongitude;
00304         GPSVel = 0;
00305 
00306 #ifdef SIMULATE
00307         FakeGPSLongitude = GPSLongitude;
00308         FakeGPSLatitude = GPSLatitude;
00309 #endif // SIMULATE
00310 
00311         mS[LastGPS] = mSClock();
00312 
00313         GPSLongitudeCorrection = cos(fabs((real32)(GPSLatitude/600000L) * DEGRAD));
00314 
00315         GPSOriginAltitude = GPSAltitude;
00316 
00317         Write16PX(NAV_ORIGIN_ALT, (int16)(GPSAltitude/100));
00318         Write32PX(NAV_ORIGIN_LAT, GPSLatitude);
00319         Write32PX(NAV_ORIGIN_LON, GPSLongitude);
00320 
00321         if ( !F.NavValid ) {
00322             DoBeep100mS(2,0);
00323             Stats[NavValidS] = true;
00324             F.NavValid = true;
00325         }
00326         F.AcquireNewPosition = true;
00327     }
00328 } // SetGPSOrigin
00329 
00330 void ParseGPSSentence(void) {
00331     static uint32 Now;
00332 
00333 #define FAKE_NORTH_WIND       0L
00334 #define FAKE_EAST_WIND        0L
00335 #define SCALE_VEL             64L
00336 
00337     cc = 0;
00338     nll = NMEA.length;
00339 
00340     switch ( GPSPacketTag ) {
00341         case GPGGAPacketTag:
00342             ParseGPGGASentence();
00343             break;
00344         case GPRMCPacketTag:
00345             ParseGPRMCSentence();
00346             break;
00347     }
00348 
00349     if ( F.GPSValid ) {
00350         // all coordinates in 0.00001 Minutes or ~1.8553cm relative to Origin
00351         // There is a lot of jitter in position - could use Kalman Estimator?
00352 
00353         Now = uSClock();
00354         GPSdT = ( Now - GPSuSp) * 0.0000001;
00355         GPSdTR = 1.0 / GPSdT;
00356         GPSuSp = Now;
00357 
00358         if ( ValidGPSSentences <  GPS_ORIGIN_SENTENCES ) {  // repetition to ensure GPGGA altitude is captured
00359             F.GPSValid = false;
00360 
00361             if ( ( GPSPacketTag == GPGGAPacketTag ) && ( GPSHDilute <= GPS_MIN_HDILUTE ))
00362                 ValidGPSSentences++;
00363             else
00364                 ValidGPSSentences = 0;
00365         }
00366 
00367 #ifdef SIMULATE
00368 
00369         if ( State == InFlight ) { // don't bother with GPS longitude correction for now?
00370             CosH = int16cos(Heading);
00371             SinH = int16sin(Heading);
00372             GPSLongitude = FakeGPSLongitude + ((int32)(-FakeDesiredPitch) * SinH)/SCALE_VEL;
00373             GPSLatitude = FakeGPSLatitude + ((int32)(-FakeDesiredPitch) * CosH)/SCALE_VEL;
00374 
00375             A = Make2Pi(Heading + HALFMILLIPI);
00376             CosH = int16cos(A);
00377             SinH = int16sin(A);
00378             GPSLongitude += ((int32)FakeDesiredRoll * SinH) / SCALE_VEL;
00379             GPSLongitude += FAKE_EAST_WIND; // wind
00380             GPSLatitude += ((int32)FakeDesiredRoll * CosH) / SCALE_VEL;
00381             GPSLatitude += FAKE_NORTH_WIND; // wind
00382 
00383             FakeGPSLongitude = GPSLongitude;
00384             FakeGPSLatitude = GPSLatitude;
00385 
00386             GPSRelAltitude = BaroRelAltitude;
00387         }
00388 
00389 #else
00390         if (F.NavValid )
00391             GPSRelAltitude = GPSAltitude - GPSOriginAltitude;
00392 
00393 #endif // SIMULATE
00394 
00395         // possibly add GPS filtering here
00396 
00397         if ( State == InFlight ) {
00398             if ( GPSRelAltitude > Stats[GPSAltitudeS] )
00399                 Stats[GPSAltitudeS] = GPSRelAltitude;
00400 
00401             if ( GPSVel * 10.0 > Stats[GPSVelS] )
00402                 Stats[GPSVelS] = GPSVel * 10.0;
00403 
00404             if (( BaroRelAltitude > 5.0 ) && ( BaroRelAltitude < 15.0 )) { // 5-15M
00405                 SumGPSRelAltitude += GPSRelAltitude;
00406                 SumBaroRelAltitude += BaroRelAltitude;
00407             }
00408         }
00409     } else
00410         if ( State == InFlight )
00411             Stats[GPSInvalidS]++;
00412 
00413 } // ParseGPSSentence
00414 
00415 void RxGPSPacket(uint8 RxCh) {
00416 
00417     switch ( RxState ) {
00418         case WaitCheckSum:
00419             if (GPSCheckSumChar < (uint8)2) {
00420                 GPSTxCheckSum *= 16;
00421                 if ( RxCh >= 'A' )
00422                     GPSTxCheckSum += ( RxCh - ('A' - 10) );
00423                 else
00424                     GPSTxCheckSum += ( RxCh - '0' );
00425 
00426                 GPSCheckSumChar++;
00427             } else {
00428                 NMEA.length = ll;
00429                 F.GPSPacketReceived = GPSTxCheckSum == RxCheckSum;
00430                 RxState = WaitSentinel;
00431             }
00432             break;
00433         case WaitBody:
00434             if ( RxCh == '*' ) {
00435                 GPSCheckSumChar = GPSTxCheckSum = 0;
00436                 RxState = WaitCheckSum;
00437             } else
00438                 if ( RxCh == '$' ) { // abort partial Sentence
00439                     ll = tt = RxCheckSum = 0;
00440                     RxState = WaitTag;
00441                 } else {
00442                     RxCheckSum ^= RxCh;
00443                     NMEA.s[ll++] = RxCh;
00444                     if ( ll > (uint8)( GPSRXBUFFLENGTH-1 ) )
00445                         RxState = WaitSentinel;
00446                 }
00447 
00448             break;
00449         case WaitTag:
00450             RxCheckSum ^= RxCh;
00451             while ( ( RxCh != NMEATags[ss][tt] ) && ( ss < MAX_NMEA_SENTENCES ) ) ss++;
00452             if ( RxCh == NMEATags[ss][tt] )
00453                 if ( tt == (uint8)NMEA_TAG_INDEX ) {
00454                     GPSPacketTag = ss;
00455                     RxState = WaitBody;
00456                 } else
00457                     tt++;
00458             else
00459                 RxState = WaitSentinel;
00460             break;
00461         case WaitSentinel: // highest priority skipping unused sentence types
00462             if ( RxCh == '$' ) {
00463                 ll = tt = ss = RxCheckSum = 0;
00464                 RxState = WaitTag;
00465             }
00466             break;
00467     }
00468 } // RxGPSPacket
00469 
00470 void UpdateGPS(void) {
00471     if ( F.GPSPacketReceived ) {
00472         LEDBlue_TOG;
00473         F.GPSPacketReceived = false;
00474         ParseGPSSentence();
00475         if ( F.GPSValid ) {
00476             F.NavComputed = false;
00477             mS[GPSTimeout] = mSClock() + GPS_TIMEOUT_MS;
00478         } else {
00479             NavCorr[Pitch] = DecayX(NavCorr[Pitch], 2);
00480             NavCorr[Roll] = DecayX(NavCorr[Roll], 2);
00481             NavCorr[Yaw] = 0;
00482         }
00483     } else
00484         if ( mSClock() > mS[GPSTimeout] )
00485             F.GPSValid = false;
00486 
00487     if ( F.GPSValid )
00488         LEDRed_OFF;
00489     else
00490         LEDRed_ON;
00491 
00492 } // UpdateGPS
00493 
00494 void GPSTest(void) {
00495 
00496     static uint8 i;
00497 
00498     TxString("\r\nGPS test\r\n\r\n");
00499 
00500     UpdateGPS();
00501 
00502     UpdateRTC();
00503     i = 0;
00504     while ( RTCString[i] != NULL )
00505         TxChar(RTCString[i++]);
00506     TxNextLine();
00507     TxString("Fix:     \t");
00508     TxVal32(GPSFix,0,0);
00509     TxNextLine();
00510     TxString("Sats:    \t");
00511     TxVal32(GPSNoOfSats,0,0);
00512     TxNextLine();
00513     TxString("HDilute: \t");
00514     TxVal32(GPSHDilute,2,0);
00515     TxNextLine();
00516     TxString("Alt:     \t");
00517     TxVal32(GPSAltitude,1,0);
00518     TxNextLine();
00519     TxString("Lat:     \t");
00520     TxVal32(GPSLatitude/600, 4, 0);
00521     TxNextLine();
00522     TxString("Lon:     \t");
00523     TxVal32(GPSLongitude/600, 4, 0);
00524     if ( F.HaveGPRMC ) {
00525         TxString("\r\nVel.   :\t");
00526         TxVal32(GPSVel * 10.0 , 1, 0);
00527         TxString("\r\nHeading:\t");
00528         TxVal32(GPSHeading * RADDEG * 10.0 , 1, 0);
00529         TxString("\r\nMDev.  :\t");
00530         TxVal32(GPSMagDeviation * RADDEG * 10.0 , 1, 0);
00531         TxNextLine();
00532     }
00533     TxNextLine();
00534 } // GPSTest
00535 
00536 void InitGPS(void) {
00537     cc = 0;
00538 
00539     GPSuSp = uSClock();
00540 
00541     GPSLongitudeCorrection = 1.0;
00542     GPSSeconds = GPSFix = GPSNoOfSats = GPSHDilute = 0;
00543     GPSRelAltitude =  GPSAltitude = GPSMagDeviation = GPSHeading = GPSVel = 0.0;
00544     GPSPacketTag = GPSUnknownPacketTag;
00545 
00546     GPSTime.tm_hour = GPSTime.tm_min = GPSTime.tm_sec = GPSTime.tm_mday = GPSTime.tm_mon = GPSTime.tm_year = 0;
00547 
00548     OriginLatitude = DesiredLatitude = HoldLatitude = LatitudeP = GPSLatitude = 0;
00549     OriginLongitude = DesiredLongitude = HoldLongitude = LongitudeP = GPSLongitude = 0;
00550 
00551     Write32PX(NAV_ORIGIN_LAT, 0);
00552     Write32PX(NAV_ORIGIN_LON, 0);
00553     Write16PX(NAV_ORIGIN_ALT, 0);
00554 
00555     ValidGPSSentences = 0;
00556 
00557     SumGPSRelAltitude = SumBaroRelAltitude = 0;
00558 
00559     F.NavValid = F.GPSValid = F.GPSPacketReceived = false;
00560     RxState = WaitSentinel;
00561 
00562 } // InitGPS