/// <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;
}
