Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
gps.c
- Committer:
- gke
- Date:
- 2011-04-26
- Revision:
- 2:90292f8bd179
- Parent:
- 0:62a1c91a859a
File content as of revision 2:90292f8bd179:
// =============================================================================================== // = UAVXArm Quadrocopter Controller = // = Copyright (c) 2008 by Prof. Greg Egan = // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = // = http://code.google.com/p/uavp-mods/ = // =============================================================================================== // This is part of UAVXArm. // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU // General Public License as published by the Free Software Foundation, either version 3 of the // License, or (at your option) any later version. // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. // See the GNU General Public License for more details. // You should have received a copy of the GNU General Public License along with this program. // If not, see http://www.gnu.org/licenses/ #include "UAVXArm.h" void UpdateField(void); int32 ConvertGPSToM(int32); int32 ConvertMToGPS(int32); int24 ConvertInt(uint8, uint8); real32 ConvertReal(uint8, uint8); int32 ConvertLatLonM(uint8, uint8); void ConvertUTime(uint8, uint8); void ConvertUDate(uint8, uint8); void ParseGPGGASentence(void); void ParseGPRMCSentence(void); void SetGPSOrigin(void); void ParseGPSSentence(void); void RxGPSPacket(uint8); void SetGPSOrigin(void); void DoGPS(void); void GPSTest(void); void UpdateGPS(void); void InitGPS(void); const uint8 NMEATags[MAX_NMEA_SENTENCES][5]= { // NMEA {'G','P','G','G','A'}, // full positioning fix {'G','P','R','M','C'}, // main current position and heading }; NMEAStruct NMEA; uint8 GPSPacketTag; struct tm GPSTime; int32 GPSStartTime, GPSSeconds; int32 GPSLatitude, GPSLongitude; int32 OriginLatitude, OriginLongitude; int32 DesiredLatitude, DesiredLongitude; int32 LatitudeP, LongitudeP, HoldLatitude, HoldLongitude; real32 GPSAltitude, GPSRelAltitude, GPSOriginAltitude; real32 GPSLongitudeCorrection; real32 GPSHeading, GPSMagHeading, GPSMagDeviation, GPSVel, GPSVelp, GPSROC; int8 GPSNoOfSats; int8 GPSFix; int16 GPSHDilute; int32 FakeGPSLongitude, FakeGPSLatitude; uint8 ll, tt, ss, RxCh; uint8 GPSCheckSumChar, GPSTxCheckSum; uint8 nll, cc, lo, hi; boolean EmptyField; int16 ValidGPSSentences; real32 GPSdT, GPSdTR; uint32 GPSuSp; int32 SumGPSRelAltitude, SumBaroRelAltitude; int32 ConvertGPSToM(int32 c) { // approximately 1.8553257183 cm per LSB at the Equator // conversion max is 21Km return ( ((int32)c * (int32)1855)/((int32)100000) ); } // ConvertGPSToM int32 ConvertMToGPS(int32 c) { // conversion max is 21Km return ( ((int32)c * (int32)100000)/((int32)1855) ); } // ConvertMToGPS int24 ConvertInt(uint8 lo, uint8 hi) { static uint8 i; static int24 r; r = 0; if ( !EmptyField ) for (i = lo; i <= hi ; i++ ) r = r * 10 + NMEA.s[i] - '0'; return (r); } // ConvertInt real32 ConvertReal(uint8 lo, uint8 hi) { int16 i, n, dp; boolean Positive; int16 whole; real32 rval; if (EmptyField) rval=0.0; else { if (NMEA.s[lo]=='-') { Positive=false; lo++; } else Positive=true; dp=lo; while ((NMEA.s[dp] != '.')&&(dp<=hi)) dp++; whole=ConvertInt(lo, dp-1); rval=ConvertInt(dp + 1, hi); n=hi-dp; for (i=1;i<=n;i++) rval/=10.0; if (Positive) rval=(whole+rval); else rval=-(whole+rval); } return(rval); } // ConvertReal int32 ConvertLatLonM(uint8 lo, uint8 hi) { // NMEA coordinates normally assumed as DDDMM.MMMMM ie 5 decimal minute digits // but code can deal with 4 and 5 decimal minutes // Positions are stored at 5 decimal minute NMEA resolution which is // approximately 1.855 cm per LSB at the Equator. static int32 dd, mm, dm, r; static uint8 dp; r = 0; if ( !EmptyField ) { dp = lo + 4; // could do this in initialisation for Lat and Lon? while ( NMEA.s[dp] != '.') dp++; dd = ConvertInt(lo, dp - 3); mm = ConvertInt(dp - 2 , dp - 1); if ( ( hi - dp ) > (uint8)4 ) dm = ConvertInt(dp + 1, dp + 5); else dm = ConvertInt(dp + 1, dp + 4) * 10L; r = dd * 6000000 + mm * 100000 + dm; } return(r); } // ConvertLatLonM void ConvertUTime(uint8 lo, uint8 hi) { if ( !EmptyField ) { GPSTime.tm_hour = ConvertInt(lo, lo+1); GPSTime.tm_min = ConvertInt(lo+2, lo+3); GPSTime.tm_sec = ConvertInt(lo+4, lo+5); GPSSeconds = (int32)GPSTime.tm_hour * 3600 + GPSTime.tm_min * 60 + GPSTime.tm_sec; } } // ConvertUTime void ConvertUDate(uint8 lo, uint8 hi) { static time_t seconds; // if ( !EmptyField && !F.RTCValid ) { GPSTime.tm_mday = ConvertInt(lo, lo + 1); GPSTime.tm_mon = ConvertInt(lo + 2, lo + 3) - 1; GPSTime.tm_year = ConvertInt( lo + 4, hi ) + 100; seconds = mktime ( &GPSTime ); set_time( seconds ); F.RTCValid = true; } } // ConvertUDate void UpdateField(void) { static uint8 ch; lo = cc; ch = NMEA.s[cc]; while (( ch != ',' ) && ( ch != '*' ) && ( cc < nll )) ch = NMEA.s[++cc]; hi = cc - 1; cc++; EmptyField = hi < lo; } // UpdateField void ParseGPGGASentence(void) { // full position $GPGGA fix UpdateField(); UpdateField(); //UTime ConvertUTime(lo,hi); UpdateField(); //Lat GPSLatitude = ConvertLatLonM(lo, hi); UpdateField(); //LatH if (NMEA.s[lo] == 'S') GPSLatitude = -GPSLatitude; UpdateField(); //Lon GPSLongitude = ConvertLatLonM(lo, hi); UpdateField(); //LonH if (NMEA.s[lo] == 'W') GPSLongitude = -GPSLongitude; UpdateField(); //Fix GPSFix = (uint8)(ConvertInt(lo,hi)); UpdateField(); //Sats GPSNoOfSats = (uint8)(ConvertInt(lo,hi)); UpdateField(); // HDilute GPSHDilute = ConvertInt(lo, hi-3) * 100 + ConvertInt(hi-1, hi); // Cm UpdateField(); // Alt GPSAltitude = real32(ConvertInt(lo, hi-2) * 10L + ConvertInt(hi, hi)); // Metres //UpdateField(); // AltUnit - assume Metres! //UpdateField(); // GHeight //UpdateField(); // GHeightUnit F.GPSValid = (GPSFix >= GPS_MIN_FIX) && ( GPSNoOfSats >= GPS_MIN_SATELLITES ); if ( State == InFlight ) { if ( GPSHDilute > Stats[MaxHDiluteS] ) { Stats[MaxHDiluteS] = GPSHDilute; F.GPSFailure = GPSHDilute > 150; } else if ( GPSHDilute < Stats[MinHDiluteS] ) Stats[MinHDiluteS] = GPSHDilute; if ( GPSNoOfSats > Stats[GPSMaxSatsS] ) Stats[GPSMaxSatsS] = GPSNoOfSats; else if ( GPSNoOfSats < Stats[GPSMinSatsS] ) Stats[GPSMinSatsS] = GPSNoOfSats; } } // ParseGPGGASentence void ParseGPRMCSentence() { // main current position and heading // uint32 UTime; UpdateField(); UpdateField(); //UTime //UTime = ConvertUTime(lo,hi); // GPSMissionTime =(int) (UTime-GPSStartTime); UpdateField(); if ( NMEA.s[lo] == 'A' ) { UpdateField(); //Lat GPSLatitude = ConvertLatLonM(lo,hi); UpdateField(); //LatH if (NMEA.s[lo] == 'S') GPSLatitude= -GPSLatitude; UpdateField(); //Lon GPSLongitude = ConvertLatLonM(lo,hi); UpdateField(); //LonH if ( NMEA.s[lo] == 'W' ) GPSLongitude = -GPSLongitude; UpdateField(); // Groundspeed (Knots) GPSVel = ConvertReal(lo, hi) * 0.514444444; // KTTOMPS UpdateField(); // True course made good (Degrees) GPSHeading = ConvertReal(lo, hi) * DEGRAD; UpdateField(); //UDate ConvertUDate(lo, hi); UpdateField(); // Magnetic Deviation (Degrees) GPSMagDeviation = ConvertReal(lo, hi) * DEGRAD; UpdateField(); // East/West if ( NMEA.s[lo] == 'W') GPSMagHeading = GPSHeading - GPSMagDeviation; // need to check sign???? else GPSMagHeading = GPSHeading + GPSMagDeviation; F.HaveGPRMC = true; } else F.HaveGPRMC = false; } // ParseGPRMCSentence void SetGPSOrigin(void) { if ( ( ValidGPSSentences == GPS_ORIGIN_SENTENCES ) && F.GPSValid ) { GPSStartTime = GPSSeconds; OriginLatitude = DesiredLatitude = HoldLatitude = LatitudeP = GPSLatitude; OriginLongitude = DesiredLongitude = HoldLongitude = LongitudeP = GPSLongitude; GPSVel = 0; #ifdef SIMULATE FakeGPSLongitude = GPSLongitude; FakeGPSLatitude = GPSLatitude; #endif // SIMULATE mS[LastGPS] = mSClock(); GPSLongitudeCorrection = cos(fabs((real32)(GPSLatitude/600000L) * DEGRAD)); GPSOriginAltitude = GPSAltitude; Write16PX(NAV_ORIGIN_ALT, (int16)(GPSAltitude/100)); Write32PX(NAV_ORIGIN_LAT, GPSLatitude); Write32PX(NAV_ORIGIN_LON, GPSLongitude); if ( !F.NavValid ) { DoBeep100mS(2,0); Stats[NavValidS] = true; F.NavValid = true; } F.AcquireNewPosition = true; } } // SetGPSOrigin void ParseGPSSentence(void) { static uint32 Now; #define FAKE_NORTH_WIND 0L #define FAKE_EAST_WIND 0L #define SCALE_VEL 64L cc = 0; nll = NMEA.length; switch ( GPSPacketTag ) { case GPGGAPacketTag: ParseGPGGASentence(); break; case GPRMCPacketTag: ParseGPRMCSentence(); break; } if ( F.GPSValid ) { // all coordinates in 0.00001 Minutes or ~1.8553cm relative to Origin // There is a lot of jitter in position - could use Kalman Estimator? Now = uSClock(); GPSdT = ( Now - GPSuSp) * 0.0000001; GPSdTR = 1.0 / GPSdT; GPSuSp = Now; if ( ValidGPSSentences < GPS_ORIGIN_SENTENCES ) { // repetition to ensure GPGGA altitude is captured F.GPSValid = false; if ( ( GPSPacketTag == GPGGAPacketTag ) && ( GPSHDilute <= GPS_MIN_HDILUTE )) ValidGPSSentences++; else ValidGPSSentences = 0; } #ifdef SIMULATE if ( State == InFlight ) { // don't bother with GPS longitude correction for now? CosH = int16cos(Heading); SinH = int16sin(Heading); GPSLongitude = FakeGPSLongitude + ((int32)(-FakeDesiredPitch) * SinH)/SCALE_VEL; GPSLatitude = FakeGPSLatitude + ((int32)(-FakeDesiredPitch) * CosH)/SCALE_VEL; A = Make2Pi(Heading + HALFMILLIPI); CosH = int16cos(A); SinH = int16sin(A); GPSLongitude += ((int32)FakeDesiredRoll * SinH) / SCALE_VEL; GPSLongitude += FAKE_EAST_WIND; // wind GPSLatitude += ((int32)FakeDesiredRoll * CosH) / SCALE_VEL; GPSLatitude += FAKE_NORTH_WIND; // wind FakeGPSLongitude = GPSLongitude; FakeGPSLatitude = GPSLatitude; GPSRelAltitude = BaroRelAltitude; } #else if (F.NavValid ) GPSRelAltitude = GPSAltitude - GPSOriginAltitude; #endif // SIMULATE // possibly add GPS filtering here if ( State == InFlight ) { if ( GPSRelAltitude > Stats[GPSAltitudeS] ) Stats[GPSAltitudeS] = GPSRelAltitude; if ( GPSVel * 10.0 > Stats[GPSVelS] ) Stats[GPSVelS] = GPSVel * 10.0; if (( BaroRelAltitude > 5.0 ) && ( BaroRelAltitude < 15.0 )) { // 5-15M SumGPSRelAltitude += GPSRelAltitude; SumBaroRelAltitude += BaroRelAltitude; } } } else if ( State == InFlight ) Stats[GPSInvalidS]++; } // ParseGPSSentence void RxGPSPacket(uint8 RxCh) { switch ( RxState ) { case WaitCheckSum: if (GPSCheckSumChar < (uint8)2) { GPSTxCheckSum *= 16; if ( RxCh >= 'A' ) GPSTxCheckSum += ( RxCh - ('A' - 10) ); else GPSTxCheckSum += ( RxCh - '0' ); GPSCheckSumChar++; } else { NMEA.length = ll; F.GPSPacketReceived = GPSTxCheckSum == RxCheckSum; RxState = WaitSentinel; } break; case WaitBody: if ( RxCh == '*' ) { GPSCheckSumChar = GPSTxCheckSum = 0; RxState = WaitCheckSum; } else if ( RxCh == '$' ) { // abort partial Sentence ll = tt = RxCheckSum = 0; RxState = WaitTag; } else { RxCheckSum ^= RxCh; NMEA.s[ll++] = RxCh; if ( ll > (uint8)( GPSRXBUFFLENGTH-1 ) ) RxState = WaitSentinel; } break; case WaitTag: RxCheckSum ^= RxCh; while ( ( RxCh != NMEATags[ss][tt] ) && ( ss < MAX_NMEA_SENTENCES ) ) ss++; if ( RxCh == NMEATags[ss][tt] ) if ( tt == (uint8)NMEA_TAG_INDEX ) { GPSPacketTag = ss; RxState = WaitBody; } else tt++; else RxState = WaitSentinel; break; case WaitSentinel: // highest priority skipping unused sentence types if ( RxCh == '$' ) { ll = tt = ss = RxCheckSum = 0; RxState = WaitTag; } break; } } // RxGPSPacket void UpdateGPS(void) { if ( F.GPSPacketReceived ) { LEDBlue_TOG; F.GPSPacketReceived = false; ParseGPSSentence(); if ( F.GPSValid ) { F.NavComputed = false; mS[GPSTimeout] = mSClock() + GPS_TIMEOUT_MS; } else { NavCorr[Pitch] = DecayX(NavCorr[Pitch], 2); NavCorr[Roll] = DecayX(NavCorr[Roll], 2); NavCorr[Yaw] = 0; } } else if ( mSClock() > mS[GPSTimeout] ) F.GPSValid = false; if ( F.GPSValid ) LEDRed_OFF; else LEDRed_ON; } // UpdateGPS void GPSTest(void) { static uint8 i; TxString("\r\nGPS test\r\n\r\n"); UpdateGPS(); UpdateRTC(); i = 0; while ( RTCString[i] != NULL ) TxChar(RTCString[i++]); TxNextLine(); TxString("Fix: \t"); TxVal32(GPSFix,0,0); TxNextLine(); TxString("Sats: \t"); TxVal32(GPSNoOfSats,0,0); TxNextLine(); TxString("HDilute: \t"); TxVal32(GPSHDilute,2,0); TxNextLine(); TxString("Alt: \t"); TxVal32(GPSAltitude,1,0); TxNextLine(); TxString("Lat: \t"); TxVal32(GPSLatitude/600, 4, 0); TxNextLine(); TxString("Lon: \t"); TxVal32(GPSLongitude/600, 4, 0); if ( F.HaveGPRMC ) { TxString("\r\nVel. :\t"); TxVal32(GPSVel * 10.0 , 1, 0); TxString("\r\nHeading:\t"); TxVal32(GPSHeading * RADDEG * 10.0 , 1, 0); TxString("\r\nMDev. :\t"); TxVal32(GPSMagDeviation * RADDEG * 10.0 , 1, 0); TxNextLine(); } TxNextLine(); } // GPSTest void InitGPS(void) { cc = 0; GPSuSp = uSClock(); GPSLongitudeCorrection = 1.0; GPSSeconds = GPSFix = GPSNoOfSats = GPSHDilute = 0; GPSRelAltitude = GPSAltitude = GPSMagDeviation = GPSHeading = GPSVel = 0.0; GPSPacketTag = GPSUnknownPacketTag; GPSTime.tm_hour = GPSTime.tm_min = GPSTime.tm_sec = GPSTime.tm_mday = GPSTime.tm_mon = GPSTime.tm_year = 0; OriginLatitude = DesiredLatitude = HoldLatitude = LatitudeP = GPSLatitude = 0; OriginLongitude = DesiredLongitude = HoldLongitude = LongitudeP = GPSLongitude = 0; Write32PX(NAV_ORIGIN_LAT, 0); Write32PX(NAV_ORIGIN_LON, 0); Write16PX(NAV_ORIGIN_ALT, 0); ValidGPSSentences = 0; SumGPSRelAltitude = SumBaroRelAltitude = 0; F.NavValid = F.GPSValid = F.GPSPacketReceived = false; RxState = WaitSentinel; } // InitGPS