Prof Greg Egan
UAVX Multicopter Flight Controller.
Diff: gps.c
- Revision:
- 0:62a1c91a859a
- Child:
- 2:90292f8bd179
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/gps.c Fri Feb 18 22:28:05 2011 +0000
@@ -0,0 +1,562 @@
+// ===============================================================================================
+// = 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/ http://uavp.ch =
+// ===============================================================================================
+
+// 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