Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

gps.c@2:90292f8bd179, 2011-04-26 (annotated)

Committer:
gke
Date:
Tue Apr 26 12:12:29 2011 +0000
Revision:
2:90292f8bd179
Parent:
0:62a1c91a859a 
Not flightworthy. Posted for others to make use of the I2C SW code.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gke 0:62a1c91a859a 1 // ===============================================================================================
gke 0:62a1c91a859a 2 // = UAVXArm Quadrocopter Controller =
gke 0:62a1c91a859a 3 // = Copyright (c) 2008 by Prof. Greg Egan =
gke 0:62a1c91a859a 4 // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer =
gke 2:90292f8bd179 5 // = http://code.google.com/p/uavp-mods/ =
gke 0:62a1c91a859a 6 // ===============================================================================================
gke 0:62a1c91a859a 7
gke 0:62a1c91a859a 8 // This is part of UAVXArm.
gke 0:62a1c91a859a 9
gke 0:62a1c91a859a 10 // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
gke 0:62a1c91a859a 11 // General Public License as published by the Free Software Foundation, either version 3 of the
gke 0:62a1c91a859a 12 // License, or (at your option) any later version.
gke 0:62a1c91a859a 13
gke 0:62a1c91a859a 14 // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
gke 0:62a1c91a859a 15 // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
gke 0:62a1c91a859a 16 // See the GNU General Public License for more details.
gke 0:62a1c91a859a 17
gke 0:62a1c91a859a 18 // You should have received a copy of the GNU General Public License along with this program.
gke 0:62a1c91a859a 19 // If not, see http://www.gnu.org/licenses/
gke 0:62a1c91a859a 20
gke 0:62a1c91a859a 21 #include "UAVXArm.h"
gke 0:62a1c91a859a 22
gke 0:62a1c91a859a 23 void UpdateField(void);
gke 0:62a1c91a859a 24 int32 ConvertGPSToM(int32);
gke 0:62a1c91a859a 25 int32 ConvertMToGPS(int32);
gke 0:62a1c91a859a 26 int24 ConvertInt(uint8, uint8);
gke 0:62a1c91a859a 27 real32 ConvertReal(uint8, uint8);
gke 0:62a1c91a859a 28 int32 ConvertLatLonM(uint8, uint8);
gke 0:62a1c91a859a 29 void ConvertUTime(uint8, uint8);
gke 0:62a1c91a859a 30 void ConvertUDate(uint8, uint8);
gke 0:62a1c91a859a 31 void ParseGPGGASentence(void);
gke 0:62a1c91a859a 32 void ParseGPRMCSentence(void);
gke 0:62a1c91a859a 33 void SetGPSOrigin(void);
gke 0:62a1c91a859a 34 void ParseGPSSentence(void);
gke 0:62a1c91a859a 35 void RxGPSPacket(uint8);
gke 0:62a1c91a859a 36 void SetGPSOrigin(void);
gke 0:62a1c91a859a 37 void DoGPS(void);
gke 0:62a1c91a859a 38 void GPSTest(void);
gke 0:62a1c91a859a 39 void UpdateGPS(void);
gke 0:62a1c91a859a 40 void InitGPS(void);
gke 0:62a1c91a859a 41
gke 0:62a1c91a859a 42 const uint8 NMEATags[MAX_NMEA_SENTENCES][5]= {
gke 0:62a1c91a859a 43 // NMEA
gke 0:62a1c91a859a 44 {'G','P','G','G','A'}, // full positioning fix
gke 0:62a1c91a859a 45 {'G','P','R','M','C'}, // main current position and heading
gke 0:62a1c91a859a 46 };
gke 0:62a1c91a859a 47
gke 0:62a1c91a859a 48 NMEAStruct NMEA;
gke 0:62a1c91a859a 49
gke 0:62a1c91a859a 50 uint8 GPSPacketTag;
gke 0:62a1c91a859a 51 struct tm GPSTime;
gke 0:62a1c91a859a 52 int32 GPSStartTime, GPSSeconds;
gke 0:62a1c91a859a 53 int32 GPSLatitude, GPSLongitude;
gke 0:62a1c91a859a 54 int32 OriginLatitude, OriginLongitude;
gke 0:62a1c91a859a 55 int32 DesiredLatitude, DesiredLongitude;
gke 0:62a1c91a859a 56 int32 LatitudeP, LongitudeP, HoldLatitude, HoldLongitude;
gke 0:62a1c91a859a 57 real32 GPSAltitude, GPSRelAltitude, GPSOriginAltitude;
gke 0:62a1c91a859a 58 real32 GPSLongitudeCorrection;
gke 0:62a1c91a859a 59 real32 GPSHeading, GPSMagHeading, GPSMagDeviation, GPSVel, GPSVelp, GPSROC;
gke 0:62a1c91a859a 60 int8 GPSNoOfSats;
gke 0:62a1c91a859a 61 int8 GPSFix;
gke 0:62a1c91a859a 62 int16 GPSHDilute;
gke 0:62a1c91a859a 63
gke 0:62a1c91a859a 64 int32 FakeGPSLongitude, FakeGPSLatitude;
gke 0:62a1c91a859a 65
gke 0:62a1c91a859a 66 uint8 ll, tt, ss, RxCh;
gke 0:62a1c91a859a 67 uint8 GPSCheckSumChar, GPSTxCheckSum;
gke 0:62a1c91a859a 68 uint8 nll, cc, lo, hi;
gke 0:62a1c91a859a 69 boolean EmptyField;
gke 0:62a1c91a859a 70 int16 ValidGPSSentences;
gke 0:62a1c91a859a 71 real32 GPSdT, GPSdTR;
gke 0:62a1c91a859a 72 uint32 GPSuSp;
gke 0:62a1c91a859a 73
gke 0:62a1c91a859a 74 int32 SumGPSRelAltitude, SumBaroRelAltitude;
gke 0:62a1c91a859a 75
gke 0:62a1c91a859a 76 int32 ConvertGPSToM(int32 c) { // approximately 1.8553257183 cm per LSB at the Equator
gke 0:62a1c91a859a 77 // conversion max is 21Km
gke 0:62a1c91a859a 78 return ( ((int32)c * (int32)1855)/((int32)100000) );
gke 0:62a1c91a859a 79 } // ConvertGPSToM
gke 0:62a1c91a859a 80
gke 0:62a1c91a859a 81 int32 ConvertMToGPS(int32 c) {
gke 0:62a1c91a859a 82 // conversion max is 21Km
gke 0:62a1c91a859a 83 return ( ((int32)c * (int32)100000)/((int32)1855) );
gke 0:62a1c91a859a 84 } // ConvertMToGPS
gke 0:62a1c91a859a 85
gke 0:62a1c91a859a 86 int24 ConvertInt(uint8 lo, uint8 hi) {
gke 0:62a1c91a859a 87 static uint8 i;
gke 0:62a1c91a859a 88 static int24 r;
gke 0:62a1c91a859a 89
gke 0:62a1c91a859a 90 r = 0;
gke 0:62a1c91a859a 91 if ( !EmptyField )
gke 0:62a1c91a859a 92 for (i = lo; i <= hi ; i++ )
gke 0:62a1c91a859a 93 r = r * 10 + NMEA.s[i] - '0';
gke 0:62a1c91a859a 94
gke 0:62a1c91a859a 95 return (r);
gke 0:62a1c91a859a 96 } // ConvertInt
gke 0:62a1c91a859a 97
gke 0:62a1c91a859a 98 real32 ConvertReal(uint8 lo, uint8 hi) {
gke 0:62a1c91a859a 99 int16 i, n, dp;
gke 0:62a1c91a859a 100 boolean Positive;
gke 0:62a1c91a859a 101 int16 whole;
gke 0:62a1c91a859a 102 real32 rval;
gke 0:62a1c91a859a 103
gke 0:62a1c91a859a 104 if (EmptyField)
gke 0:62a1c91a859a 105 rval=0.0;
gke 0:62a1c91a859a 106 else {
gke 0:62a1c91a859a 107 if (NMEA.s[lo]=='-') {
gke 0:62a1c91a859a 108 Positive=false;
gke 0:62a1c91a859a 109 lo++;
gke 0:62a1c91a859a 110 } else
gke 0:62a1c91a859a 111 Positive=true;
gke 0:62a1c91a859a 112
gke 0:62a1c91a859a 113 dp=lo;
gke 0:62a1c91a859a 114 while ((NMEA.s[dp] != '.')&&(dp<=hi))
gke 0:62a1c91a859a 115 dp++;
gke 0:62a1c91a859a 116
gke 0:62a1c91a859a 117 whole=ConvertInt(lo, dp-1);
gke 0:62a1c91a859a 118 rval=ConvertInt(dp + 1, hi);
gke 0:62a1c91a859a 119
gke 0:62a1c91a859a 120 n=hi-dp;
gke 0:62a1c91a859a 121 for (i=1;i<=n;i++)
gke 0:62a1c91a859a 122 rval/=10.0;
gke 0:62a1c91a859a 123
gke 0:62a1c91a859a 124 if (Positive)
gke 0:62a1c91a859a 125 rval=(whole+rval);
gke 0:62a1c91a859a 126 else
gke 0:62a1c91a859a 127 rval=-(whole+rval);
gke 0:62a1c91a859a 128 }
gke 0:62a1c91a859a 129
gke 0:62a1c91a859a 130 return(rval);
gke 0:62a1c91a859a 131 } // ConvertReal
gke 0:62a1c91a859a 132
gke 0:62a1c91a859a 133 int32 ConvertLatLonM(uint8 lo, uint8 hi) { // NMEA coordinates normally assumed as DDDMM.MMMMM ie 5 decimal minute digits
gke 0:62a1c91a859a 134 // but code can deal with 4 and 5 decimal minutes
gke 0:62a1c91a859a 135 // Positions are stored at 5 decimal minute NMEA resolution which is
gke 0:62a1c91a859a 136 // approximately 1.855 cm per LSB at the Equator.
gke 0:62a1c91a859a 137 static int32 dd, mm, dm, r;
gke 0:62a1c91a859a 138 static uint8 dp;
gke 0:62a1c91a859a 139
gke 0:62a1c91a859a 140 r = 0;
gke 0:62a1c91a859a 141 if ( !EmptyField ) {
gke 0:62a1c91a859a 142 dp = lo + 4; // could do this in initialisation for Lat and Lon?
gke 0:62a1c91a859a 143 while ( NMEA.s[dp] != '.') dp++;
gke 0:62a1c91a859a 144
gke 0:62a1c91a859a 145 dd = ConvertInt(lo, dp - 3);
gke 0:62a1c91a859a 146 mm = ConvertInt(dp - 2 , dp - 1);
gke 0:62a1c91a859a 147 if ( ( hi - dp ) > (uint8)4 )
gke 0:62a1c91a859a 148 dm = ConvertInt(dp + 1, dp + 5);
gke 0:62a1c91a859a 149 else
gke 0:62a1c91a859a 150 dm = ConvertInt(dp + 1, dp + 4) * 10L;
gke 0:62a1c91a859a 151
gke 0:62a1c91a859a 152 r = dd * 6000000 + mm * 100000 + dm;
gke 0:62a1c91a859a 153 }
gke 0:62a1c91a859a 154
gke 0:62a1c91a859a 155 return(r);
gke 0:62a1c91a859a 156 } // ConvertLatLonM
gke 0:62a1c91a859a 157
gke 0:62a1c91a859a 158 void ConvertUTime(uint8 lo, uint8 hi) {
gke 0:62a1c91a859a 159
gke 0:62a1c91a859a 160 if ( !EmptyField ) {
gke 0:62a1c91a859a 161 GPSTime.tm_hour = ConvertInt(lo, lo+1);
gke 0:62a1c91a859a 162 GPSTime.tm_min = ConvertInt(lo+2, lo+3);
gke 0:62a1c91a859a 163 GPSTime.tm_sec = ConvertInt(lo+4, lo+5);
gke 0:62a1c91a859a 164 GPSSeconds = (int32)GPSTime.tm_hour * 3600 + GPSTime.tm_min * 60 + GPSTime.tm_sec;
gke 0:62a1c91a859a 165 }
gke 0:62a1c91a859a 166 } // ConvertUTime
gke 0:62a1c91a859a 167
gke 0:62a1c91a859a 168 void ConvertUDate(uint8 lo, uint8 hi) {
gke 0:62a1c91a859a 169
gke 0:62a1c91a859a 170 static time_t seconds;
gke 0:62a1c91a859a 171
gke 0:62a1c91a859a 172 // if ( !EmptyField && !F.RTCValid )
gke 0:62a1c91a859a 173 {
gke 0:62a1c91a859a 174 GPSTime.tm_mday = ConvertInt(lo, lo + 1);
gke 0:62a1c91a859a 175 GPSTime.tm_mon = ConvertInt(lo + 2, lo + 3) - 1;
gke 0:62a1c91a859a 176 GPSTime.tm_year = ConvertInt( lo + 4, hi ) + 100;
gke 0:62a1c91a859a 177 seconds = mktime ( &GPSTime );
gke 0:62a1c91a859a 178 set_time( seconds );
gke 0:62a1c91a859a 179
gke 0:62a1c91a859a 180 F.RTCValid = true;
gke 0:62a1c91a859a 181 }
gke 0:62a1c91a859a 182
gke 0:62a1c91a859a 183 } // ConvertUDate
gke 0:62a1c91a859a 184
gke 0:62a1c91a859a 185 void UpdateField(void) {
gke 0:62a1c91a859a 186 static uint8 ch;
gke 0:62a1c91a859a 187
gke 0:62a1c91a859a 188 lo = cc;
gke 0:62a1c91a859a 189
gke 0:62a1c91a859a 190 ch = NMEA.s[cc];
gke 0:62a1c91a859a 191 while (( ch != ',' ) && ( ch != '*' ) && ( cc < nll ))
gke 0:62a1c91a859a 192 ch = NMEA.s[++cc];
gke 0:62a1c91a859a 193
gke 0:62a1c91a859a 194 hi = cc - 1;
gke 0:62a1c91a859a 195 cc++;
gke 0:62a1c91a859a 196 EmptyField = hi < lo;
gke 0:62a1c91a859a 197 } // UpdateField
gke 0:62a1c91a859a 198
gke 0:62a1c91a859a 199 void ParseGPGGASentence(void) { // full position $GPGGA fix
gke 0:62a1c91a859a 200
gke 0:62a1c91a859a 201 UpdateField();
gke 0:62a1c91a859a 202
gke 0:62a1c91a859a 203 UpdateField(); //UTime
gke 0:62a1c91a859a 204 ConvertUTime(lo,hi);
gke 0:62a1c91a859a 205
gke 0:62a1c91a859a 206 UpdateField(); //Lat
gke 0:62a1c91a859a 207 GPSLatitude = ConvertLatLonM(lo, hi);
gke 0:62a1c91a859a 208 UpdateField(); //LatH
gke 0:62a1c91a859a 209 if (NMEA.s[lo] == 'S') GPSLatitude = -GPSLatitude;
gke 0:62a1c91a859a 210
gke 0:62a1c91a859a 211 UpdateField(); //Lon
gke 0:62a1c91a859a 212 GPSLongitude = ConvertLatLonM(lo, hi);
gke 0:62a1c91a859a 213 UpdateField(); //LonH
gke 0:62a1c91a859a 214 if (NMEA.s[lo] == 'W') GPSLongitude = -GPSLongitude;
gke 0:62a1c91a859a 215
gke 0:62a1c91a859a 216 UpdateField(); //Fix
gke 0:62a1c91a859a 217 GPSFix = (uint8)(ConvertInt(lo,hi));
gke 0:62a1c91a859a 218
gke 0:62a1c91a859a 219 UpdateField(); //Sats
gke 0:62a1c91a859a 220 GPSNoOfSats = (uint8)(ConvertInt(lo,hi));
gke 0:62a1c91a859a 221
gke 0:62a1c91a859a 222 UpdateField(); // HDilute
gke 0:62a1c91a859a 223 GPSHDilute = ConvertInt(lo, hi-3) * 100 + ConvertInt(hi-1, hi); // Cm
gke 0:62a1c91a859a 224
gke 0:62a1c91a859a 225 UpdateField(); // Alt
gke 0:62a1c91a859a 226 GPSAltitude = real32(ConvertInt(lo, hi-2) * 10L + ConvertInt(hi, hi)); // Metres
gke 0:62a1c91a859a 227
gke 0:62a1c91a859a 228 //UpdateField(); // AltUnit - assume Metres!
gke 0:62a1c91a859a 229
gke 0:62a1c91a859a 230 //UpdateField(); // GHeight
gke 0:62a1c91a859a 231 //UpdateField(); // GHeightUnit
gke 0:62a1c91a859a 232
gke 0:62a1c91a859a 233 F.GPSValid = (GPSFix >= GPS_MIN_FIX) && ( GPSNoOfSats >= GPS_MIN_SATELLITES );
gke 0:62a1c91a859a 234
gke 0:62a1c91a859a 235 if ( State == InFlight ) {
gke 0:62a1c91a859a 236 if ( GPSHDilute > Stats[MaxHDiluteS] ) {
gke 0:62a1c91a859a 237 Stats[MaxHDiluteS] = GPSHDilute;
gke 0:62a1c91a859a 238 F.GPSFailure = GPSHDilute > 150;
gke 0:62a1c91a859a 239 } else
gke 0:62a1c91a859a 240 if ( GPSHDilute < Stats[MinHDiluteS] )
gke 0:62a1c91a859a 241 Stats[MinHDiluteS] = GPSHDilute;
gke 0:62a1c91a859a 242
gke 0:62a1c91a859a 243 if ( GPSNoOfSats > Stats[GPSMaxSatsS] )
gke 0:62a1c91a859a 244 Stats[GPSMaxSatsS] = GPSNoOfSats;
gke 0:62a1c91a859a 245 else
gke 0:62a1c91a859a 246 if ( GPSNoOfSats < Stats[GPSMinSatsS] )
gke 0:62a1c91a859a 247 Stats[GPSMinSatsS] = GPSNoOfSats;
gke 0:62a1c91a859a 248 }
gke 0:62a1c91a859a 249 } // ParseGPGGASentence
gke 0:62a1c91a859a 250
gke 0:62a1c91a859a 251 void ParseGPRMCSentence() { // main current position and heading
gke 0:62a1c91a859a 252
gke 0:62a1c91a859a 253 // uint32 UTime;
gke 0:62a1c91a859a 254
gke 0:62a1c91a859a 255 UpdateField();
gke 0:62a1c91a859a 256
gke 0:62a1c91a859a 257 UpdateField(); //UTime
gke 0:62a1c91a859a 258 //UTime = ConvertUTime(lo,hi);
gke 0:62a1c91a859a 259 // GPSMissionTime =(int) (UTime-GPSStartTime);
gke 0:62a1c91a859a 260
gke 0:62a1c91a859a 261 UpdateField();
gke 0:62a1c91a859a 262 if ( NMEA.s[lo] == 'A' ) {
gke 0:62a1c91a859a 263 UpdateField(); //Lat
gke 0:62a1c91a859a 264 GPSLatitude = ConvertLatLonM(lo,hi);
gke 0:62a1c91a859a 265 UpdateField(); //LatH
gke 0:62a1c91a859a 266 if (NMEA.s[lo] == 'S')
gke 0:62a1c91a859a 267 GPSLatitude= -GPSLatitude;
gke 0:62a1c91a859a 268
gke 0:62a1c91a859a 269 UpdateField(); //Lon
gke 0:62a1c91a859a 270 GPSLongitude = ConvertLatLonM(lo,hi);
gke 0:62a1c91a859a 271
gke 0:62a1c91a859a 272 UpdateField(); //LonH
gke 0:62a1c91a859a 273 if ( NMEA.s[lo] == 'W' )
gke 0:62a1c91a859a 274 GPSLongitude = -GPSLongitude;
gke 0:62a1c91a859a 275
gke 0:62a1c91a859a 276 UpdateField(); // Groundspeed (Knots)
gke 0:62a1c91a859a 277 GPSVel = ConvertReal(lo, hi) * 0.514444444; // KTTOMPS
gke 0:62a1c91a859a 278
gke 0:62a1c91a859a 279 UpdateField(); // True course made good (Degrees)
gke 0:62a1c91a859a 280 GPSHeading = ConvertReal(lo, hi) * DEGRAD;
gke 0:62a1c91a859a 281
gke 0:62a1c91a859a 282 UpdateField(); //UDate
gke 0:62a1c91a859a 283 ConvertUDate(lo, hi);
gke 0:62a1c91a859a 284
gke 0:62a1c91a859a 285 UpdateField(); // Magnetic Deviation (Degrees)
gke 0:62a1c91a859a 286 GPSMagDeviation = ConvertReal(lo, hi) * DEGRAD;
gke 0:62a1c91a859a 287
gke 0:62a1c91a859a 288 UpdateField(); // East/West
gke 0:62a1c91a859a 289 if ( NMEA.s[lo] == 'W')
gke 0:62a1c91a859a 290 GPSMagHeading = GPSHeading - GPSMagDeviation; // need to check sign????
gke 0:62a1c91a859a 291 else
gke 0:62a1c91a859a 292 GPSMagHeading = GPSHeading + GPSMagDeviation;
gke 0:62a1c91a859a 293 F.HaveGPRMC = true;
gke 0:62a1c91a859a 294 } else
gke 0:62a1c91a859a 295 F.HaveGPRMC = false;
gke 0:62a1c91a859a 296
gke 0:62a1c91a859a 297 } // ParseGPRMCSentence
gke 0:62a1c91a859a 298
gke 0:62a1c91a859a 299 void SetGPSOrigin(void) {
gke 0:62a1c91a859a 300 if ( ( ValidGPSSentences == GPS_ORIGIN_SENTENCES ) && F.GPSValid ) {
gke 0:62a1c91a859a 301 GPSStartTime = GPSSeconds;
gke 0:62a1c91a859a 302 OriginLatitude = DesiredLatitude = HoldLatitude = LatitudeP = GPSLatitude;
gke 0:62a1c91a859a 303 OriginLongitude = DesiredLongitude = HoldLongitude = LongitudeP = GPSLongitude;
gke 0:62a1c91a859a 304 GPSVel = 0;
gke 0:62a1c91a859a 305
gke 0:62a1c91a859a 306 #ifdef SIMULATE
gke 0:62a1c91a859a 307 FakeGPSLongitude = GPSLongitude;
gke 0:62a1c91a859a 308 FakeGPSLatitude = GPSLatitude;
gke 0:62a1c91a859a 309 #endif // SIMULATE
gke 0:62a1c91a859a 310
gke 0:62a1c91a859a 311 mS[LastGPS] = mSClock();
gke 0:62a1c91a859a 312
gke 0:62a1c91a859a 313 GPSLongitudeCorrection = cos(fabs((real32)(GPSLatitude/600000L) * DEGRAD));
gke 0:62a1c91a859a 314
gke 0:62a1c91a859a 315 GPSOriginAltitude = GPSAltitude;
gke 0:62a1c91a859a 316
gke 0:62a1c91a859a 317 Write16PX(NAV_ORIGIN_ALT, (int16)(GPSAltitude/100));
gke 0:62a1c91a859a 318 Write32PX(NAV_ORIGIN_LAT, GPSLatitude);
gke 0:62a1c91a859a 319 Write32PX(NAV_ORIGIN_LON, GPSLongitude);
gke 0:62a1c91a859a 320
gke 0:62a1c91a859a 321 if ( !F.NavValid ) {
gke 0:62a1c91a859a 322 DoBeep100mS(2,0);
gke 0:62a1c91a859a 323 Stats[NavValidS] = true;
gke 0:62a1c91a859a 324 F.NavValid = true;
gke 0:62a1c91a859a 325 }
gke 0:62a1c91a859a 326 F.AcquireNewPosition = true;
gke 0:62a1c91a859a 327 }
gke 0:62a1c91a859a 328 } // SetGPSOrigin
gke 0:62a1c91a859a 329
gke 0:62a1c91a859a 330 void ParseGPSSentence(void) {
gke 0:62a1c91a859a 331 static uint32 Now;
gke 0:62a1c91a859a 332
gke 0:62a1c91a859a 333 #define FAKE_NORTH_WIND 0L
gke 0:62a1c91a859a 334 #define FAKE_EAST_WIND 0L
gke 0:62a1c91a859a 335 #define SCALE_VEL 64L
gke 0:62a1c91a859a 336
gke 0:62a1c91a859a 337 cc = 0;
gke 0:62a1c91a859a 338 nll = NMEA.length;
gke 0:62a1c91a859a 339
gke 0:62a1c91a859a 340 switch ( GPSPacketTag ) {
gke 0:62a1c91a859a 341 case GPGGAPacketTag:
gke 0:62a1c91a859a 342 ParseGPGGASentence();
gke 0:62a1c91a859a 343 break;
gke 0:62a1c91a859a 344 case GPRMCPacketTag:
gke 0:62a1c91a859a 345 ParseGPRMCSentence();
gke 0:62a1c91a859a 346 break;
gke 0:62a1c91a859a 347 }
gke 0:62a1c91a859a 348
gke 0:62a1c91a859a 349 if ( F.GPSValid ) {
gke 0:62a1c91a859a 350 // all coordinates in 0.00001 Minutes or ~1.8553cm relative to Origin
gke 0:62a1c91a859a 351 // There is a lot of jitter in position - could use Kalman Estimator?
gke 0:62a1c91a859a 352
gke 0:62a1c91a859a 353 Now = uSClock();
gke 0:62a1c91a859a 354 GPSdT = ( Now - GPSuSp) * 0.0000001;
gke 0:62a1c91a859a 355 GPSdTR = 1.0 / GPSdT;
gke 0:62a1c91a859a 356 GPSuSp = Now;
gke 0:62a1c91a859a 357
gke 0:62a1c91a859a 358 if ( ValidGPSSentences < GPS_ORIGIN_SENTENCES ) { // repetition to ensure GPGGA altitude is captured
gke 0:62a1c91a859a 359 F.GPSValid = false;
gke 0:62a1c91a859a 360
gke 0:62a1c91a859a 361 if ( ( GPSPacketTag == GPGGAPacketTag ) && ( GPSHDilute <= GPS_MIN_HDILUTE ))
gke 0:62a1c91a859a 362 ValidGPSSentences++;
gke 0:62a1c91a859a 363 else
gke 0:62a1c91a859a 364 ValidGPSSentences = 0;
gke 0:62a1c91a859a 365 }
gke 0:62a1c91a859a 366
gke 0:62a1c91a859a 367 #ifdef SIMULATE
gke 0:62a1c91a859a 368
gke 0:62a1c91a859a 369 if ( State == InFlight ) { // don't bother with GPS longitude correction for now?
gke 0:62a1c91a859a 370 CosH = int16cos(Heading);
gke 0:62a1c91a859a 371 SinH = int16sin(Heading);
gke 0:62a1c91a859a 372 GPSLongitude = FakeGPSLongitude + ((int32)(-FakeDesiredPitch) * SinH)/SCALE_VEL;
gke 0:62a1c91a859a 373 GPSLatitude = FakeGPSLatitude + ((int32)(-FakeDesiredPitch) * CosH)/SCALE_VEL;
gke 0:62a1c91a859a 374
gke 0:62a1c91a859a 375 A = Make2Pi(Heading + HALFMILLIPI);
gke 0:62a1c91a859a 376 CosH = int16cos(A);
gke 0:62a1c91a859a 377 SinH = int16sin(A);
gke 0:62a1c91a859a 378 GPSLongitude += ((int32)FakeDesiredRoll * SinH) / SCALE_VEL;
gke 0:62a1c91a859a 379 GPSLongitude += FAKE_EAST_WIND; // wind
gke 0:62a1c91a859a 380 GPSLatitude += ((int32)FakeDesiredRoll * CosH) / SCALE_VEL;
gke 0:62a1c91a859a 381 GPSLatitude += FAKE_NORTH_WIND; // wind
gke 0:62a1c91a859a 382
gke 0:62a1c91a859a 383 FakeGPSLongitude = GPSLongitude;
gke 0:62a1c91a859a 384 FakeGPSLatitude = GPSLatitude;
gke 0:62a1c91a859a 385
gke 0:62a1c91a859a 386 GPSRelAltitude = BaroRelAltitude;
gke 0:62a1c91a859a 387 }
gke 0:62a1c91a859a 388
gke 0:62a1c91a859a 389 #else
gke 0:62a1c91a859a 390 if (F.NavValid )
gke 0:62a1c91a859a 391 GPSRelAltitude = GPSAltitude - GPSOriginAltitude;
gke 0:62a1c91a859a 392
gke 0:62a1c91a859a 393 #endif // SIMULATE
gke 0:62a1c91a859a 394
gke 0:62a1c91a859a 395 // possibly add GPS filtering here
gke 0:62a1c91a859a 396
gke 0:62a1c91a859a 397 if ( State == InFlight ) {
gke 0:62a1c91a859a 398 if ( GPSRelAltitude > Stats[GPSAltitudeS] )
gke 0:62a1c91a859a 399 Stats[GPSAltitudeS] = GPSRelAltitude;
gke 0:62a1c91a859a 400
gke 0:62a1c91a859a 401 if ( GPSVel * 10.0 > Stats[GPSVelS] )
gke 0:62a1c91a859a 402 Stats[GPSVelS] = GPSVel * 10.0;
gke 0:62a1c91a859a 403
gke 0:62a1c91a859a 404 if (( BaroRelAltitude > 5.0 ) && ( BaroRelAltitude < 15.0 )) { // 5-15M
gke 0:62a1c91a859a 405 SumGPSRelAltitude += GPSRelAltitude;
gke 0:62a1c91a859a 406 SumBaroRelAltitude += BaroRelAltitude;
gke 0:62a1c91a859a 407 }
gke 0:62a1c91a859a 408 }
gke 0:62a1c91a859a 409 } else
gke 0:62a1c91a859a 410 if ( State == InFlight )
gke 0:62a1c91a859a 411 Stats[GPSInvalidS]++;
gke 0:62a1c91a859a 412
gke 0:62a1c91a859a 413 } // ParseGPSSentence
gke 0:62a1c91a859a 414
gke 0:62a1c91a859a 415 void RxGPSPacket(uint8 RxCh) {
gke 0:62a1c91a859a 416
gke 0:62a1c91a859a 417 switch ( RxState ) {
gke 0:62a1c91a859a 418 case WaitCheckSum:
gke 0:62a1c91a859a 419 if (GPSCheckSumChar < (uint8)2) {
gke 0:62a1c91a859a 420 GPSTxCheckSum *= 16;
gke 0:62a1c91a859a 421 if ( RxCh >= 'A' )
gke 0:62a1c91a859a 422 GPSTxCheckSum += ( RxCh - ('A' - 10) );
gke 0:62a1c91a859a 423 else
gke 0:62a1c91a859a 424 GPSTxCheckSum += ( RxCh - '0' );
gke 0:62a1c91a859a 425
gke 0:62a1c91a859a 426 GPSCheckSumChar++;
gke 0:62a1c91a859a 427 } else {
gke 0:62a1c91a859a 428 NMEA.length = ll;
gke 0:62a1c91a859a 429 F.GPSPacketReceived = GPSTxCheckSum == RxCheckSum;
gke 0:62a1c91a859a 430 RxState = WaitSentinel;
gke 0:62a1c91a859a 431 }
gke 0:62a1c91a859a 432 break;
gke 0:62a1c91a859a 433 case WaitBody:
gke 0:62a1c91a859a 434 if ( RxCh == '*' ) {
gke 0:62a1c91a859a 435 GPSCheckSumChar = GPSTxCheckSum = 0;
gke 0:62a1c91a859a 436 RxState = WaitCheckSum;
gke 0:62a1c91a859a 437 } else
gke 0:62a1c91a859a 438 if ( RxCh == '$' ) { // abort partial Sentence
gke 0:62a1c91a859a 439 ll = tt = RxCheckSum = 0;
gke 0:62a1c91a859a 440 RxState = WaitTag;
gke 0:62a1c91a859a 441 } else {
gke 0:62a1c91a859a 442 RxCheckSum ^= RxCh;
gke 0:62a1c91a859a 443 NMEA.s[ll++] = RxCh;
gke 0:62a1c91a859a 444 if ( ll > (uint8)( GPSRXBUFFLENGTH-1 ) )
gke 0:62a1c91a859a 445 RxState = WaitSentinel;
gke 0:62a1c91a859a 446 }
gke 0:62a1c91a859a 447
gke 0:62a1c91a859a 448 break;
gke 0:62a1c91a859a 449 case WaitTag:
gke 0:62a1c91a859a 450 RxCheckSum ^= RxCh;
gke 0:62a1c91a859a 451 while ( ( RxCh != NMEATags[ss][tt] ) && ( ss < MAX_NMEA_SENTENCES ) ) ss++;
gke 0:62a1c91a859a 452 if ( RxCh == NMEATags[ss][tt] )
gke 0:62a1c91a859a 453 if ( tt == (uint8)NMEA_TAG_INDEX ) {
gke 0:62a1c91a859a 454 GPSPacketTag = ss;
gke 0:62a1c91a859a 455 RxState = WaitBody;
gke 0:62a1c91a859a 456 } else
gke 0:62a1c91a859a 457 tt++;
gke 0:62a1c91a859a 458 else
gke 0:62a1c91a859a 459 RxState = WaitSentinel;
gke 0:62a1c91a859a 460 break;
gke 0:62a1c91a859a 461 case WaitSentinel: // highest priority skipping unused sentence types
gke 0:62a1c91a859a 462 if ( RxCh == '$' ) {
gke 0:62a1c91a859a 463 ll = tt = ss = RxCheckSum = 0;
gke 0:62a1c91a859a 464 RxState = WaitTag;
gke 0:62a1c91a859a 465 }
gke 0:62a1c91a859a 466 break;
gke 0:62a1c91a859a 467 }
gke 0:62a1c91a859a 468 } // RxGPSPacket
gke 0:62a1c91a859a 469
gke 0:62a1c91a859a 470 void UpdateGPS(void) {
gke 0:62a1c91a859a 471 if ( F.GPSPacketReceived ) {
gke 0:62a1c91a859a 472 LEDBlue_TOG;
gke 0:62a1c91a859a 473 F.GPSPacketReceived = false;
gke 0:62a1c91a859a 474 ParseGPSSentence();
gke 0:62a1c91a859a 475 if ( F.GPSValid ) {
gke 0:62a1c91a859a 476 F.NavComputed = false;
gke 0:62a1c91a859a 477 mS[GPSTimeout] = mSClock() + GPS_TIMEOUT_MS;
gke 0:62a1c91a859a 478 } else {
gke 0:62a1c91a859a 479 NavCorr[Pitch] = DecayX(NavCorr[Pitch], 2);
gke 0:62a1c91a859a 480 NavCorr[Roll] = DecayX(NavCorr[Roll], 2);
gke 0:62a1c91a859a 481 NavCorr[Yaw] = 0;
gke 0:62a1c91a859a 482 }
gke 0:62a1c91a859a 483 } else
gke 0:62a1c91a859a 484 if ( mSClock() > mS[GPSTimeout] )
gke 0:62a1c91a859a 485 F.GPSValid = false;
gke 0:62a1c91a859a 486
gke 0:62a1c91a859a 487 if ( F.GPSValid )
gke 0:62a1c91a859a 488 LEDRed_OFF;
gke 0:62a1c91a859a 489 else
gke 0:62a1c91a859a 490 LEDRed_ON;
gke 0:62a1c91a859a 491
gke 0:62a1c91a859a 492 } // UpdateGPS
gke 0:62a1c91a859a 493
gke 0:62a1c91a859a 494 void GPSTest(void) {
gke 0:62a1c91a859a 495
gke 0:62a1c91a859a 496 static uint8 i;
gke 0:62a1c91a859a 497
gke 0:62a1c91a859a 498 TxString("\r\nGPS test\r\n\r\n");
gke 0:62a1c91a859a 499
gke 0:62a1c91a859a 500 UpdateGPS();
gke 0:62a1c91a859a 501
gke 0:62a1c91a859a 502 UpdateRTC();
gke 0:62a1c91a859a 503 i = 0;
gke 0:62a1c91a859a 504 while ( RTCString[i] != NULL )
gke 0:62a1c91a859a 505 TxChar(RTCString[i++]);
gke 0:62a1c91a859a 506 TxNextLine();
gke 0:62a1c91a859a 507 TxString("Fix: \t");
gke 0:62a1c91a859a 508 TxVal32(GPSFix,0,0);
gke 0:62a1c91a859a 509 TxNextLine();
gke 0:62a1c91a859a 510 TxString("Sats: \t");
gke 0:62a1c91a859a 511 TxVal32(GPSNoOfSats,0,0);
gke 0:62a1c91a859a 512 TxNextLine();
gke 0:62a1c91a859a 513 TxString("HDilute: \t");
gke 0:62a1c91a859a 514 TxVal32(GPSHDilute,2,0);
gke 0:62a1c91a859a 515 TxNextLine();
gke 0:62a1c91a859a 516 TxString("Alt: \t");
gke 0:62a1c91a859a 517 TxVal32(GPSAltitude,1,0);
gke 0:62a1c91a859a 518 TxNextLine();
gke 0:62a1c91a859a 519 TxString("Lat: \t");
gke 0:62a1c91a859a 520 TxVal32(GPSLatitude/600, 4, 0);
gke 0:62a1c91a859a 521 TxNextLine();
gke 0:62a1c91a859a 522 TxString("Lon: \t");
gke 0:62a1c91a859a 523 TxVal32(GPSLongitude/600, 4, 0);
gke 0:62a1c91a859a 524 if ( F.HaveGPRMC ) {
gke 0:62a1c91a859a 525 TxString("\r\nVel. :\t");
gke 0:62a1c91a859a 526 TxVal32(GPSVel * 10.0 , 1, 0);
gke 0:62a1c91a859a 527 TxString("\r\nHeading:\t");
gke 0:62a1c91a859a 528 TxVal32(GPSHeading * RADDEG * 10.0 , 1, 0);
gke 0:62a1c91a859a 529 TxString("\r\nMDev. :\t");
gke 0:62a1c91a859a 530 TxVal32(GPSMagDeviation * RADDEG * 10.0 , 1, 0);
gke 0:62a1c91a859a 531 TxNextLine();
gke 0:62a1c91a859a 532 }
gke 0:62a1c91a859a 533 TxNextLine();
gke 0:62a1c91a859a 534 } // GPSTest
gke 0:62a1c91a859a 535
gke 0:62a1c91a859a 536 void InitGPS(void) {
gke 0:62a1c91a859a 537 cc = 0;
gke 0:62a1c91a859a 538
gke 0:62a1c91a859a 539 GPSuSp = uSClock();
gke 0:62a1c91a859a 540
gke 0:62a1c91a859a 541 GPSLongitudeCorrection = 1.0;
gke 0:62a1c91a859a 542 GPSSeconds = GPSFix = GPSNoOfSats = GPSHDilute = 0;
gke 0:62a1c91a859a 543 GPSRelAltitude = GPSAltitude = GPSMagDeviation = GPSHeading = GPSVel = 0.0;
gke 0:62a1c91a859a 544 GPSPacketTag = GPSUnknownPacketTag;
gke 0:62a1c91a859a 545
gke 0:62a1c91a859a 546 GPSTime.tm_hour = GPSTime.tm_min = GPSTime.tm_sec = GPSTime.tm_mday = GPSTime.tm_mon = GPSTime.tm_year = 0;
gke 0:62a1c91a859a 547
gke 0:62a1c91a859a 548 OriginLatitude = DesiredLatitude = HoldLatitude = LatitudeP = GPSLatitude = 0;
gke 0:62a1c91a859a 549 OriginLongitude = DesiredLongitude = HoldLongitude = LongitudeP = GPSLongitude = 0;
gke 0:62a1c91a859a 550
gke 0:62a1c91a859a 551 Write32PX(NAV_ORIGIN_LAT, 0);
gke 0:62a1c91a859a 552 Write32PX(NAV_ORIGIN_LON, 0);
gke 0:62a1c91a859a 553 Write16PX(NAV_ORIGIN_ALT, 0);
gke 0:62a1c91a859a 554
gke 0:62a1c91a859a 555 ValidGPSSentences = 0;
gke 0:62a1c91a859a 556
gke 0:62a1c91a859a 557 SumGPSRelAltitude = SumBaroRelAltitude = 0;
gke 0:62a1c91a859a 558
gke 0:62a1c91a859a 559 F.NavValid = F.GPSValid = F.GPSPacketReceived = false;
gke 0:62a1c91a859a 560 RxState = WaitSentinel;
gke 0:62a1c91a859a 561
gke 0:62a1c91a859a 562 } // InitGPS