/// <summary> /// Compares two deviation data sets for equality. Minor differences are ignored. /// </summary> /// <param name="other">The other object</param> /// <param name="firstDifference">A short error message where the error is</param> /// <returns>True on equality, false otherwise</returns> public bool Equals(MagneticDeviationCorrection?other, out string firstDifference) { if (other == null) { firstDifference = "Comparing with null"; return(false); } if (!Equals(Identification, other.Identification)) { firstDifference = "Identification is not same"; return(false); } if (_deviationPointsFromCompassReading.Length != other._deviationPointsFromCompassReading.Length) { firstDifference = "Calibration has a different number of compass->magnetic points."; return(false); } if (_deviationPointsToCompassReading.Length != other._deviationPointsToCompassReading.Length) { firstDifference = "Calibration has a different number of magnetic->compass points."; return(false); } for (int i = 0; i < _deviationPointsFromCompassReading.Length; i++) { DeviationPoint left = _deviationPointsFromCompassReading[i]; DeviationPoint right = other._deviationPointsFromCompassReading[i]; if (!left.Equals(right)) { firstDifference = $"Points differ at index {i}, Compass reading {left.CompassReading}"; return(false); } } for (int i = 0; i < _deviationPointsToCompassReading.Length; i++) { DeviationPoint left = _deviationPointsToCompassReading[i]; DeviationPoint right = other._deviationPointsToCompassReading[i]; if (!left.Equals(right)) { firstDifference = $"Points differ at index {i}, Compass reading {left.CompassReading}"; return(false); } } firstDifference = string.Empty; return(true); }
private static void CalculateSmoothing(DeviationPoint[] circle) { for (int i = 0; i < 360; i++) { const int smoothingPoints = 10; // each side double avgDeviation = 0; int usedPoints = 0; for (int k = i - smoothingPoints; k <= i + smoothingPoints; k++) { var ptIn = circle[(k + 360) % 360]; if (ptIn != null) { avgDeviation += ptIn.Deviation; usedPoints++; } } avgDeviation /= usedPoints; if (circle[i] != null) { circle[i].DeviationSmooth = (float)avgDeviation; // The compass reading we get if we apply the smoothed deviation circle[i].CompassReadingSmooth = (float)Angle.FromDegrees(circle[i].MagneticHeading - avgDeviation).Normalize(true).Degrees; } else { float avgReading = i + 0.5f; circle[i] = new DeviationPoint() { CompassReading = (float)avgReading, // Constructed from the result CompassReadingSmooth = (float)avgReading, MagneticHeading = (float)Angle.FromDegrees(avgReading + avgDeviation).Normalize(true).Degrees, Deviation = (float)avgDeviation, DeviationSmooth = (float)avgDeviation }; } } }
/// <summary> /// Tries to calculate a correction from the given recorded file, indicating the timespan where the calibration loops were performed. /// The recorded file should contain a data set where the vessel is turning two slow circles, one with the clock and one against the clock, /// in calm conditions and with no current. /// </summary> /// <param name="fileSet">The recorded nmea files (from a logged session)</param> /// <param name="beginCalibration">The start time of the calibration loops</param> /// <param name="endCalibration">The end time of the calibration loops</param> public void CreateCorrectionTable(string[] fileSet, DateTimeOffset beginCalibration, DateTimeOffset endCalibration) { _interestingSentences.Clear(); _magneticVariation = Angle.Zero; _rawData = new RawData(); var rawCompass = new List <MagneticReading>(); var rawTrack = new List <GnssReading>(); void MessageFilter(NmeaSinkAndSource nmeaSinkAndSource, NmeaSentence nmeaSentence) { if (nmeaSentence.DateTime < beginCalibration || nmeaSentence.DateTime > endCalibration) { return; } if (nmeaSentence is RecommendedMinimumNavigationInformation rmc) { // Track over ground from GPS is useless if not moving if (rmc.Valid && rmc.SpeedOverGround > Speed.FromKnots(0.3)) { _interestingSentences.Add(rmc); if (rmc.MagneticVariationInDegrees.HasValue) { _magneticVariation = rmc.MagneticVariationInDegrees.Value; } float delta = 0; if (rawTrack.Count > 0) { delta = rawTrack[rawTrack.Count - 1].TrackReading - (float)rmc.TrackMadeGoodInDegreesTrue.Degrees; // Need to do it the ugly way here - converting back to an angle is also not really nice while (delta > 180) { delta -= 360; } while (delta < -180) { delta += 360; } } rawTrack.Add(new GnssReading() { TimeStamp = rmc.DateTime.DateTime, TrackReading = (float)rmc.TrackMadeGoodInDegreesTrue.Degrees, DeltaToPrevious = delta }); } } if (nmeaSentence is HeadingMagnetic hdm) { if (hdm.Valid) { _interestingSentences.Add(hdm); float delta = 0; if (rawCompass.Count > 0) { delta = rawCompass[rawCompass.Count - 1].MagneticCompassReading - (float)hdm.Angle.Degrees; while (delta > 180) { delta -= 360; } while (delta < -180) { delta += 360; } } rawCompass.Add(new MagneticReading() { TimeStamp = hdm.DateTime.DateTime, MagneticCompassReading = (float)hdm.Angle.Degrees, DeltaToPrevious = delta }); } } } NmeaLogDataReader reader = new NmeaLogDataReader("Reader", fileSet); reader.OnNewSequence += MessageFilter; reader.StartDecode(); reader.Dispose(); _rawData.Compass = rawCompass.ToArray(); _rawData.Track = rawTrack.ToArray(); DeviationPoint[] circle = new DeviationPoint[360]; // One entry per degree string[] pointsWithProblems = new string[360]; // This will get the average offset, which is assumed to be orientation independent (i.e. if the magnetic compass's forward // direction doesn't properly align with the ship) double averageOffset = 0; for (int i = 0; i < 360; i++) { FindAllTracksWith(i, out var tracks, out var headings); if (tracks.Count > 0 && headings.Count > 0) { Angle averageTrack; // Computed from COG (GPS course) if (!tracks.TryAverageAngle(out averageTrack)) { averageTrack = tracks[0]; // Should be a rare case - just use the first one then } Angle magneticTrack = averageTrack - _magneticVariation; // Now in degrees magnetic magneticTrack = magneticTrack.Normalize(true); // This should be i + 0.5 if the data is good Angle averageHeading; if (!headings.TryAverageAngle(out averageHeading)) { averageHeading = headings[0]; } double deviation = (averageHeading - magneticTrack).Normalize(false).Degrees; var pt = new DeviationPoint() { // First is less "true" than second, so CompassReading + Deviation => MagneticHeading CompassReading = (float)averageHeading.Normalize(true).Degrees, MagneticHeading = (float)magneticTrack.Normalize(true).Degrees, Deviation = (float)-deviation, }; averageOffset += deviation; circle[i] = pt; } } averageOffset /= circle.Count(x => x != null); int numberOfConsecutiveGaps = 0; const int maxConsecutiveGaps = 5; // Evaluate the quality of the result DeviationPoint?previous = null; double maxLocalChange = 0; for (int i = 0; i < 360; i++) { var pt = circle[i]; if (pt == null) { numberOfConsecutiveGaps++; if (numberOfConsecutiveGaps > maxConsecutiveGaps) { throw new InvalidDataException($"Not enough data points. There is not enough data near heading {i} degrees. Total number of points {_interestingSentences.Count}"); } } else { if (Math.Abs(pt.Deviation + averageOffset) > 30) { pointsWithProblems[i] = ($"Your magnetic compass shows deviations of more than 30 degrees. Use a better installation location or buy a new one."); } numberOfConsecutiveGaps = 0; if (previous != null) { if (Math.Abs(previous.Deviation - pt.Deviation) > maxLocalChange) { maxLocalChange = Math.Abs(previous.Deviation - pt.Deviation); pointsWithProblems[i] = $"Big deviation change near heading {i}"; } } previous = pt; } } for (int i = 0; i < 360; i++) { if (pointsWithProblems[i] != null) { circle[i] = null !; } } // Validate again for (int i = 0; i < 360; i++) { var pt = circle[i]; if (pt == null) { numberOfConsecutiveGaps++; if (numberOfConsecutiveGaps > maxConsecutiveGaps) { throw new InvalidDataException($"Not enough data points after cleanup. There is not enough data near heading {i} degrees"); } } else { numberOfConsecutiveGaps = 0; } } CalculateSmoothing(circle); // Now create the inverse of the above map, to get from compass reading back to undisturbed magnetic heading _deviationPointsFromCompassReading = circle; _deviationPointsToCompassReading = Array.Empty <DeviationPoint>(); circle = new DeviationPoint[360]; for (int i = 0; i < 360; i++) { var ptToUse = _deviationPointsFromCompassReading.FirstOrDefault(x => (int)x.MagneticHeading == i); int offs = 1; while (ptToUse == null) { ptToUse = _deviationPointsFromCompassReading.FirstOrDefault(x => (int)x.MagneticHeading == (i + offs) % 360 || (int)x.MagneticHeading == (i + 360 - offs) % 360); offs++; } circle[i] = new DeviationPoint() { CompassReading = ptToUse.CompassReading, CompassReadingSmooth = ptToUse.CompassReadingSmooth, Deviation = ptToUse.Deviation, DeviationSmooth = ptToUse.DeviationSmooth, MagneticHeading = ptToUse.MagneticHeading }; } _deviationPointsToCompassReading = circle; }