public GpsPosition GetPosition() { GpsPosition gpsPosition = null; if (Opened) { if(useGccDll) { short hour; short min; short sec; short msec; double latitude; double longitude; double hdop; double altitude; double geoid_sep; int max_snr; int num_sat; double speed; double heading; short day; short month; short year; int status = GccReadGps(out hour, out min, out sec, out msec, out latitude, out longitude, out num_sat, out hdop, out altitude, out geoid_sep, out max_snr, out speed, out heading, out day, out month, out year); /* defines from GccGPS READ_NO_ERRORS 0x01 READ_HAS_DATA 0x02 READ_HAS_GPGSV 0x04 READ_HAS_GPGGA 0x08 READ_HAS_GPRMC 0x10 */ // no data read from GPS port if((status & 0x01) == 0) { return gpsPosition; } // has some data - create GpsPosition structure if((status & 0x02) != 0) { gpsPosition = new GpsPosition(); // we set version 2 for our stuff. Use some of the fields in the structure for our use gpsPosition.dwVersion = 2; gpsPosition.dwValidFields = 0; // GPGSV is OK if((status & 0x04) != 0) { // use flVerticalDilutionOfPrecision field for SNR (do not want to fill SNR for all sats!) gpsPosition.flVerticalDilutionOfPrecision = (float) max_snr; gpsPosition.dwValidFields |= 0x00000400; if(num_sat != -1) { gpsPosition.dwSatellitesInView = num_sat % 100; gpsPosition.dwSatelliteCount = num_sat / 100; gpsPosition.dwValidFields |= 0x00002800; } } // GPGGA is OK if((status & 0x08) != 0) { /* time and position only from GPRMC because of possible date and time unconsistency // use dwFlags to fill today time (do not want to fill stUTCTime!) if( hour != -1) { // encode in 3 lower bytes gpsPosition.dwFlags = 0; gpsPosition.dwFlags |= (sec & 0xFF); gpsPosition.dwFlags |= ((min & 0xFF) << 8); gpsPosition.dwFlags |= ((hour & 0xFF) << 16); gpsPosition.dwValidFields |= 0x00000001; } if( (latitude != Int16.MinValue) && (longitude != Int16.MinValue) ) { gpsPosition.dblLatitude = latitude; gpsPosition.dblLongitude = longitude; gpsPosition.dwValidFields |= 0x00000002; gpsPosition.dwValidFields |= 0x00000004; } */ if(hdop != Int16.MinValue) { gpsPosition.flHorizontalDilutionOfPrecision = (float) hdop; gpsPosition.dwValidFields |= 0x00000200; } if(altitude != Int16.MinValue) { gpsPosition.flAltitudeWRTSeaLevel = (float) altitude; gpsPosition.dwValidFields |= 0x00000040; } if (altitude != Int16.MinValue && geoid_sep != Int16.MinValue) { gpsPosition.flAltitudeWRTEllipsoid = (float)(altitude + geoid_sep); gpsPosition.dwValidFields |= 0x00000080; } } // GPRMC is OK if((status & 0x10) != 0) { if (hour != -1 && day != -1) { gpsPosition.stUTCTime.millisecond = msec; gpsPosition.stUTCTime.second = sec; gpsPosition.stUTCTime.minute = min; gpsPosition.stUTCTime.hour = hour; gpsPosition.stUTCTime.day = day; gpsPosition.stUTCTime.month = month; gpsPosition.stUTCTime.year = (short)(year + 2000); gpsPosition.dwValidFields |= 0x00000001; } if ((latitude != Int16.MinValue) && (longitude != Int16.MinValue)) { gpsPosition.dblLatitude = latitude; gpsPosition.dblLongitude = longitude; gpsPosition.dwValidFields |= 0x00000002; gpsPosition.dwValidFields |= 0x00000004; } if(speed != -1.0) { gpsPosition.flSpeed = (float) speed; gpsPosition.dwValidFields |= 0x00000008; } if(heading != -1.0) { gpsPosition.flHeading = (float) heading; gpsPosition.dwValidFields |= 0x00000010; } } } } else { // allocate the necessary memory on the native side. We have a class (GpsPosition) that // has the same memory layout as its native counterpart IntPtr ptr = Utils.LocalAlloc(Marshal.SizeOf(typeof(GpsPosition))); // fill in the required fields gpsPosition = new GpsPosition(); gpsPosition.dwVersion = 1; gpsPosition.dwSize = Marshal.SizeOf(typeof(GpsPosition)); // Marshal our data to the native pointer we allocated. Marshal.StructureToPtr(gpsPosition, ptr, false); // call native method passing in our native buffer int result = GPSGetPosition(gpsHandle, ptr, 500000, 0); if (result == 0) { // native call succeeded, marshal native data to our managed data gpsPosition = (GpsPosition)Marshal.PtrToStructure(ptr, typeof(GpsPosition)); } // free our native memory Utils.LocalFree(ptr); } } return gpsPosition; }
// main logging function to receive date from GPS private void GetGpsData() { #if testClickCurrentPos return; #endif //numsamples++; //debug CurrentGpsLedColor = Color.Red; position = gps.GetPosition(); if (position != null) { bool positionAltitudeValid; double positionAltitude; if (checkWgs84Alt.Checked) // altitude related to WGS84ellipsoid instead of MeanSeaLevel { positionAltitude = position.EllipsoidAltitude; positionAltitudeValid = position.EllipsoidAltitudeValid; } else { positionAltitude = position.SeaLevelAltitude; positionAltitudeValid = position.SeaLevelAltitudeValid; } Heading = 720; //invalid, but still head up if (position.HeadingValid) Heading = (int)position.Heading; if (position.TimeValid && position.LatitudeValid && position.LongitudeValid) { TimeSpan deltaT = position.Time - LastPointUtc; double deltaT_s = deltaT.TotalSeconds; if (deltaT_s > 0) { // OK, time is increasing -> data is valid //goodsamples++; //debug int avg = (int)numericAvg.Value; //int avg = 1; //debug switch (GpsDataState) { case GpsNotOk: //GpsDataState is last state if (configSyncSystemTime && (state != State.logging || PlotCount == 0)) //don't change system time in the middle of a log SyncSystemTime(); FirstSampleDropCount = dropFirst[comboDropFirst.SelectedIndex]; goto case GpsDrop; case GpsDrop: case GpsBecameValid: if (FirstSampleDropCount > 0) // wait first few samples to get a "better grip" ! { FirstSampleDropCount--; GpsSearchCount = 0; //Point received (even if it's dropped). Start search to search for the next one CurrentGpsLedColor = Color.Yellow; GpsDataState = GpsDrop; } else { //GpsDataState = GpsBecameValid; if (checkBeepOnFix.Checked) Utils.MessageBeep(Utils.BeepType.Ok); AvgCount = avg; CurrentLat = position.Latitude; //initialize Old and Current variables CurrentLong = position.Longitude; if (!utmUtil.referenceSet) { //is executed after StartNewTrace() utmUtil.setReferencePoint(CurrentLat, CurrentLong); StartLat = CurrentLat; StartLong = CurrentLong; } utmUtil.getXY(position.Latitude, position.Longitude, out CurrentX, out CurrentY); OldX = CurrentX; OldY = CurrentY; CurrentAltInvalid = true; ReferencAltSlope = Int16.MinValue; if (MainConfigSpeedSource != 1 && position.SpeedValid) { CurrentSpeed = position.Speed * 1.852; } else { CurrentSpeed = Int16.MinValue * 0.1; CurrentV = Int16.MinValue * 0.1; } CurrentVx = 0.0; CurrentVy = 0.0; CurrentGpsLedColor = Color.LightGreen; GpsDataState = GpsInitVelo; //mapUtil.nav.ShortSearch = 0; //initiate new search of minDistance } break; case GpsInitVelo: case GpsAvg: case GpsOk: GpsSearchCount = 0; CurrentGpsLedColor = Color.LightGreen; double x, y; utmUtil.getXY(position.Latitude, position.Longitude, out x, out y); // Averaging CurrentLat = (CurrentLat * (avg - 1) + position.Latitude) / avg; CurrentLong = (CurrentLong * (avg - 1) + position.Longitude) / avg; //CurrentX = (CurrentX * (avg - 1) + x) / avg; //CurrentY = (CurrentY * (avg - 1) + y) / avg; if (deltaT_s < 43200) //deltaT_s < 12h - on date change and slight difference on GPS time and system time it can happen (because date comes from system, not GPS) { CurrentX = ((CurrentX + CurrentVx * deltaT_s) * (avg - 1) + x) / avg; //CurrentVx from last sample CurrentY = ((CurrentY + CurrentVy * deltaT_s) * (avg - 1) + y) / avg; } else { //skip averaging CurrentX = x; CurrentY = y; } double deltaX = CurrentX - OldX; double deltaY = CurrentY - OldY; OldX = CurrentX; OldY = CurrentY; if (positionAltitudeValid) { if (CurrentAltInvalid) { // initialize CurrentAlt = positionAltitude - (double)numericGeoID.Value; CurrentAltInvalid = false; ReferencAltSlope = CurrentAlt; ReferenceXSlope = CurrentX; ReferenceYSlope = CurrentY; ElevationSlope = 0.0; } else { //averaging CurrentAlt = (CurrentAlt * (avg - 1) + positionAltitude - (double)numericGeoID.Value) / avg; } } if (deltaT_s < 43200) //deltaT_s < 12h - on date change and slight difference on GPS time and system time it can happen (because date comes from system, not GPS) { if (GpsDataState == GpsInitVelo) { CurrentVx = deltaX / deltaT_s; // m/s CurrentVy = deltaY / deltaT_s; CurrentV = 3.6 * Math.Sqrt(CurrentVx * CurrentVx + CurrentVy * CurrentVy); // 3.6* -> km/h GpsDataState++; //=GpsAvg } else { CurrentVx = (CurrentVx * (2 * avg - 1) + (deltaX / deltaT_s)) / (2 * avg); CurrentVy = (CurrentVy * (2 * avg - 1) + (deltaY / deltaT_s)) / (2 * avg); CurrentV = 3.6 * Math.Sqrt(CurrentVx * CurrentVx + CurrentVy * CurrentVy); } } if (ReferencAltSlope != Int16.MinValue) // slope { double deltaS = Math.Sqrt((CurrentX - ReferenceXSlope) * (CurrentX - ReferenceXSlope) + (CurrentY - ReferenceYSlope) * (CurrentY - ReferenceYSlope)); if (deltaS > 2) // not too short distance { ElevationSlope = (ElevationSlope * (2*avg - 1) + ((CurrentAlt - ReferencAltSlope) / deltaS)) / (2*avg); ReferencAltSlope = CurrentAlt; ReferenceXSlope = CurrentX; ReferenceYSlope = CurrentY; } } if (MainConfigSpeedSource == 1) { CurrentSpeed = CurrentV; //CurrentVx,y is averaged - average CurrentV anyway? better increase avg of CurrentVx,y because of sign-information } else { // speed in in kmh - converted from knots (see top of this file) if (position.SpeedValid) //invalid? leave old value { if (CurrentSpeed == Int16.MinValue * 0.1) CurrentSpeed = position.Speed * 1.852; //initialize else CurrentSpeed = (CurrentSpeed * (avg - 1) + position.Speed * 1.852) / avg; } } //process the data if (comboGpsPoll.SelectedIndex < IndexSuspendMode || --AvgCount <= 0) // in suspend mode wait for averaging { // AvgCount can run negative - for 68 years if(GpsDataState == GpsAvg) GpsDataState = GpsOk; CurrentGpsLedColor = Color.Green; double DeltaDistance = Math.Sqrt((CurrentX - ReferenceXDist) * (CurrentX - ReferenceXDist) + (CurrentY - ReferenceYDist) * (CurrentY - ReferenceYDist)); if ((state == State.logging && (GpsLogCounter <= 0 || DeltaDistance >= GpsLogDistance)) || PlotCount == 0) //if PlotCount==0 calculate vars temporarily { if (comboGpsPoll.SelectedIndex < IndexDistanceMode) { GpsLogCounter = PollGpsTimeSec[comboGpsPoll.SelectedIndex]; GpsLogDistance = 10000000.0; } else if (comboGpsPoll.SelectedIndex < IndexSuspendMode) { GpsLogDistance = PollGpsTimeSec[comboGpsPoll.SelectedIndex]; GpsLogCounter = 30; //after 30s force a log } // save and write starting position if (TSummary.StartTime == DateTime.MinValue) { //StartLat = CurrentLat; is done on GpsBecameValid after setReferencePoint //StartLong = CurrentLong; TSummary.StartTime = DateTime.Now; StartTimeUtc = DateTime.UtcNow; LastBatterySave = StartTimeUtc; StartBattery = Utils.GetBatteryStatus(); StartAlt = Int16.MinValue; ReferenceXDist = CurrentX; ReferenceYDist = CurrentY; DeltaDistance = 0.0; ReferenceAlt = Int16.MaxValue; //TSummary.AltitudeMax = Int16.MinValue; //TSummary.AltitudeMin = Int16.MaxValue; LapStartD = 0; LapStartT = 0; LapNumber = 0; lapManualDistance = 0; lapManualClick = false; currentLap = ""; lastLap = ""; textLapOptions.Text = ""; moving = false; //utmUtil.setReferencePoint(StartLat, StartLong); //OldX = 0.0; OldY = 0.0; //VeloAvgState = 2; //start velocity calculation new because of setReferencePoint if (state == State.logging) { try { WriteStartDateTime(); writer.Write((double)StartLat); writer.Write((double)StartLong); } catch (Exception e) { Utils.log.Error(" GetGpsData - save and write starting position", e); } finally { //writer.Flush(); } WriteOptionsInfo(); } // for maps, fill x/y values realtive to the starting point ResetMapPosition(); } TimeSpan run_time = DateTime.UtcNow - StartTimeUtc; int total_sec = (int)run_time.TotalSeconds; if (ContinueAfterPause) if (configNoLogPassiveTime) passiveTimeSeconds = total_sec - CurrentTimeSec; else passiveTimeSeconds = 0; total_sec -= passiveTimeSeconds; // Safety check 1: make sure elapsed time is not negative if (total_sec < 0) total_sec = 0; // Safety check 2: make sure new time is increasing if (total_sec < OldT) { OldT = total_sec; } if (ContinueAfterPause) { //force speed 0, so that Pause time calculates to Stoppage time (log v=0 into gcc for reload!) CurrentSpeed = 0.0; ContinueAfterPause = false; if (state == State.logging) { while (total_sec - CurrentTimeSec > 64800) //handle 16 bit limit in log file { CurrentTimeSec += 64800; WriteRecord(CurrentX, CurrentY); //write current records with time increments < 16 bit (18 hours) } } } CurrentTimeSec = total_sec; // compute Stoppage time if (CurrentSpeed < 1.0) { CurrentStoppageTimeSec += CurrentTimeSec - OldT; if (moving && beepOnStartStop) Utils.MessageBeep(Utils.BeepType.IconAsterisk); moving = false; } else { if (!moving && beepOnStartStop) Utils.MessageBeep(Utils.BeepType.Ok); moving = true; } OldT = CurrentTimeSec; // Update max speed (in kmh) if (CurrentSpeed > MaxSpeed) { MaxSpeed = CurrentSpeed; } // compute distance TSummary.Distance += DeltaDistance; Odo += DeltaDistance; ReferenceXDist = CurrentX; ReferenceYDist = CurrentY; // compute elevation gain and min max if (positionAltitudeValid) { if (StartAlt == Int16.MinValue) StartAlt = CurrentAlt; if (CurrentAlt > ReferenceAlt) { TSummary.AltitudeGain += CurrentAlt - ReferenceAlt; ReferenceAlt = CurrentAlt; } else if (CurrentAlt < ReferenceAlt - AltThreshold) { ReferenceAlt = CurrentAlt; } if (CurrentAlt > TSummary.AltitudeMax) TSummary.AltitudeMax = CurrentAlt; if (CurrentAlt < TSummary.AltitudeMin) TSummary.AltitudeMin = CurrentAlt; } // if exclude stop time is activated, do not log stop time in file, and // do not include stop time in live logging. // Bugfix: even if not moving, the first call must be logged, otherwise // the start position is logged more than once (because PlotCount=0), which leads // to an corrupt log file. //if (checkExStopTime.Checked == false || moving == true || PlotCount == 0) //KB: removed this feature because of incorrect average speed and trip time when track loaded back. //{ //Also distance calculation differs slightly and incorrect speed graph. //to make it work: first and last point with v=0 must be logged (last cached and written right before point with v!=0). No distance accumulation when v=0 (slight change in behaviour). if (state == State.logging) { if (oHeartBeat != null) WriteHeartRateRecord(); // write heart rate before normal record because of LoadGcc() WriteRecord(CurrentX, CurrentY); // write battery info every 1 min - and flush data WriteBatteryInfo(); AddPlotData((float)CurrentLat, (float)CurrentLong, (Int16)CurrentAlt, CurrentTimeSec, (Int16)(CurrentSpeed * 10.0), (Int32)TSummary.Distance, (Int16)getHeartRate()); CurrentPlotIndex = PlotCount - 1; DoLiveLogging(); } DoLapStats(); //} }// Logging || tmp vars else if (PlotCount < PlotDataSize) //for graph current point { if (PlotCount > 0) { PlotD[PlotCount] = PlotD[PlotCount - 1]; PlotT[PlotCount] = PlotT[PlotCount - 1]; } else { PlotD[PlotCount] = (Int32)TSummary.Distance; //never executed PlotT[PlotCount] = CurrentTimeSec; } PlotZ[PlotCount] = (Int16)CurrentAlt; PlotS[PlotCount] = (Int16)(CurrentSpeed * 10.0); PlotH[PlotCount] = (Int16)getHeartRate(); CurrentPlotIndex = PlotCount; } else CurrentPlotIndex = PlotCount - 1; } break; case GpsInvalidButTrust: GpsDataState = GpsOk; goto case GpsOk; }// switch } LastPointUtc = position.Time; // save last time } //if position.TLL valid else { GpsSearchCount++; switch (GpsDataState) { case GpsNotOk: case GpsDrop: break; case GpsOk: GpsDataState = GpsInvalidButTrust; break; default: if (checkBeepOnFix.Checked && GpsDataState >= GpsAvg) { Utils.MessageBeep(Utils.BeepType.IconExclamation); } GpsDataState = GpsNotOk; break; } } CurrentStatusString = "GPS"; if(comboGpsPoll.SelectedIndex >= IndexSuspendMode) //always on CurrentStatusString += "(" + PollGpsTimeSec[comboGpsPoll.SelectedIndex] + "s)"; //start/stop (suspend) mode CurrentStatusString += ": "; if (state == State.logging) CurrentStatusString += "Logging "; else if (state == State.paused) CurrentStatusString += "Paused "; else CurrentStatusString += "On "; CurrentStatusString += (comboDropFirst.SelectedIndex > 0) ? ("d" + FirstSampleDropCount + " ") : ""; CurrentStatusString += (position.TimeValid ? "T" : "t") + (position.LatitudeValid ? "L" : "l") + (position.LongitudeValid ? "L" : "l") + (positionAltitudeValid ? "A" : "a") + (position.HeadingValid ? "H" : "h") + (position.SpeedValid ? "S" : "s") + " "; CurrentStatusString += GpsSearchCount; } else //position == null { CurrentStatusString = "no data from GPS"; } //debugStr = "bad samples: " + (numsamples - goodsamples) + "/" + numsamples; //debug }
public GpsPosition GetPosition() { GpsPosition gpsPosition = null; if (Opened) { if (useGccDll) { short hour; short min; short sec; short msec; double latitude; double longitude; double hdop; double altitude; double geoid_sep; int max_snr; int num_sat; double speed; double heading; short day; short month; short year; int status = GccReadGps(out hour, out min, out sec, out msec, out latitude, out longitude, out num_sat, out hdop, out altitude, out geoid_sep, out max_snr, out speed, out heading, out day, out month, out year); /* defines from GccGPS * READ_NO_ERRORS 0x01 * READ_HAS_DATA 0x02 * READ_HAS_GPGSV 0x04 * READ_HAS_GPGGA 0x08 * READ_HAS_GPRMC 0x10 */ // no data read from GPS port if ((status & 0x01) == 0) { return(gpsPosition); } // has some data - create GpsPosition structure if ((status & 0x02) != 0) { gpsPosition = new GpsPosition(); // we set version 2 for our stuff. Use some of the fields in the structure for our use gpsPosition.dwVersion = 2; gpsPosition.dwValidFields = 0; // GPGSV is OK if ((status & 0x04) != 0) { // use flVerticalDilutionOfPrecision field for SNR (do not want to fill SNR for all sats!) gpsPosition.flVerticalDilutionOfPrecision = (float)max_snr; gpsPosition.dwValidFields |= 0x00000400; if (num_sat != -1) { gpsPosition.dwSatellitesInView = num_sat % 100; gpsPosition.dwSatelliteCount = num_sat / 100; gpsPosition.dwValidFields |= 0x00002800; } } // GPGGA is OK if ((status & 0x08) != 0) { /* time and position only from GPRMC because of possible date and time unconsistency * // use dwFlags to fill today time (do not want to fill stUTCTime!) * if( hour != -1) * { * // encode in 3 lower bytes * gpsPosition.dwFlags = 0; * gpsPosition.dwFlags |= (sec & 0xFF); * gpsPosition.dwFlags |= ((min & 0xFF) << 8); * gpsPosition.dwFlags |= ((hour & 0xFF) << 16); * * gpsPosition.dwValidFields |= 0x00000001; * } * * if( (latitude != Int16.MinValue) && (longitude != Int16.MinValue) ) * { * gpsPosition.dblLatitude = latitude; * gpsPosition.dblLongitude = longitude; * gpsPosition.dwValidFields |= 0x00000002; * gpsPosition.dwValidFields |= 0x00000004; * } */ if (hdop != Int16.MinValue) { gpsPosition.flHorizontalDilutionOfPrecision = (float)hdop; gpsPosition.dwValidFields |= 0x00000200; } if (altitude != Int16.MinValue) { gpsPosition.flAltitudeWRTSeaLevel = (float)altitude; gpsPosition.dwValidFields |= 0x00000040; } if (altitude != Int16.MinValue && geoid_sep != Int16.MinValue) { gpsPosition.flAltitudeWRTEllipsoid = (float)(altitude + geoid_sep); gpsPosition.dwValidFields |= 0x00000080; } } // GPRMC is OK if ((status & 0x10) != 0) { if (hour != -1 && day != -1) { gpsPosition.stUTCTime.millisecond = msec; gpsPosition.stUTCTime.second = sec; gpsPosition.stUTCTime.minute = min; gpsPosition.stUTCTime.hour = hour; gpsPosition.stUTCTime.day = day; gpsPosition.stUTCTime.month = month; gpsPosition.stUTCTime.year = (short)(year + 2000); gpsPosition.dwValidFields |= 0x00000001; } if ((latitude != Int16.MinValue) && (longitude != Int16.MinValue)) { gpsPosition.dblLatitude = latitude; gpsPosition.dblLongitude = longitude; gpsPosition.dwValidFields |= 0x00000002; gpsPosition.dwValidFields |= 0x00000004; } if (speed != -1.0) { gpsPosition.flSpeed = (float)speed; gpsPosition.dwValidFields |= 0x00000008; } if (heading != -1.0) { gpsPosition.flHeading = (float)heading; gpsPosition.dwValidFields |= 0x00000010; } } } } else { // allocate the necessary memory on the native side. We have a class (GpsPosition) that // has the same memory layout as its native counterpart IntPtr ptr = Utils.LocalAlloc(Marshal.SizeOf(typeof(GpsPosition))); // fill in the required fields gpsPosition = new GpsPosition(); gpsPosition.dwVersion = 1; gpsPosition.dwSize = Marshal.SizeOf(typeof(GpsPosition)); // Marshal our data to the native pointer we allocated. Marshal.StructureToPtr(gpsPosition, ptr, false); // call native method passing in our native buffer int result = GPSGetPosition(gpsHandle, ptr, 500000, 0); if (result == 0) { // native call succeeded, marshal native data to our managed data gpsPosition = (GpsPosition)Marshal.PtrToStructure(ptr, typeof(GpsPosition)); } // free our native memory Utils.LocalFree(ptr); } } return(gpsPosition); }