public PolarCoordinate(float r, double theta, Azimuth origin, PolarCoordinateOrientation orientation)
{
_R = r;
_Theta = new Angle(theta);
_Origin = origin;
_Orientation = orientation;
}
public override bool MouseMove(MouseEventArgs e)
{
if (Step == 1)
{
RotateALL(baseX, baseY, (float)(-angle));
var dx = Primitive.ResultX - baseX;
var dy = Primitive.ResultY - baseY;
angle = Azimuth.Create(dx, dy);
RotateALL(baseX, baseY, (float)angle);
return(true);
}
return(false);
}
/// <summary>
/// Randomizes the emulation by changing speed and direction
/// </summary>
/// <remarks>GPS coordinate emulation can be randomized by any number of factors, depending on the emulator type used.
/// Any emulation can have it's direction and speed randomized within specified tolerances. By default, speed is
/// limited to between 0 (low) and 5 (high) meters/second and bearing changes are limited to +/- 45 degrees
/// (a 90 degree arc) from North (0) degrees.</remarks>
public virtual void Randomize()
{
// Flag it so the emulation will use random values
_isRandom = true;
// Randomize the speed within range
_speed = Speed.FromMetersPerSecond((_seed.NextDouble() * (_speedHigh - _speedLow)) + _speedLow);
// Randomize the bearing within the arc.
_bearing = new Azimuth(_seed.NextDouble() * _bearingArc + (_bearingStart - (_bearingArc * .5))).Normalize();
// Reset the bearing origin
_bearingStart = _bearing.DecimalDegrees;
}
/// <summary>
/// Overrides OnSentanceChanged for the GPVTGSentence
/// </summary>
protected override void OnSentenceChanged()
{
// First, process the basic info for the sentence
base.OnSentenceChanged();
// Cache the sentence words
string[] words = Words;
int wordCount = words.Length;
/*
* $GPVTG
*
* Track Made Good and Ground Speed.
*
* eg1. $GPVTG, 360.0, T, 348.7, M, 000.0, N, 000.0, K*43
* eg2. $GPVTG, 054.7, T, 034.4, M, 005.5, N, 010.2, K
*
* 054.7, T True track made good
* 034.4, M Magnetic track made good
* 005.5, N Ground speed, knots
* 010.2, K Ground speed, Kilometers per hour
*
* eg3. $GPVTG, t, T, ,, s.ss, N, s.ss, K*hh
* 0 = Track made good
* 1 = Fixed text 'T' indicates that track made good is relative to true north
* 2 = not used
* 3 = not used
* 4 = Speed over ground in knots
* 5 = Fixed text 'N' indicates that speed over ground in in knots
* 6 = Speed over ground in kilometers/hour
* 7 = Fixed text 'K' indicates that speed over ground is in kilometers/hour
*
*
*
*/
#region Heading
if (wordCount >= 1 && words[0].Length != 0)
{
_heading = Azimuth.Parse(words[0], NmeaCultureInfo);
}
else
{
_heading = Azimuth.Invalid;
}
#endregion Heading
}
public void Azimuth_1_30_addDeflection_Pos2_15_shouldYieldNewAzimuth_3_45()
{
Azimuth anAzimuth = new Azimuth();
anAzimuth.setFromDegreesMinutesSeconds(1, 30, 0);
Deflection aDefl = new Deflection();
aDefl.setFromDegreesMinutesSeconds(2, 15, 0);
Double expected = 3.75;
Azimuth newAz = anAzimuth + aDefl;
Double actual = newAz.getAsDegreesDouble();
Assert.AreEqual(expected: expected, actual: actual, delta: delta);
}
/// <summary>
/// Randomizes the emulation by changing speed and direction
/// </summary>
/// <param name="seed"> The randomizer to use. </param>
/// <param name="speedLow"> The minimum travel speed. </param>
/// <param name="speedHigh"> The maximum travel speed. </param>
/// <param name="bearingStart"> The initial direction of travel. </param>
/// <param name="bearingArc"> The arc in which random directional changes will occur. </param>
/// <remarks>
/// GPS coordinate emulation can be randomized by any number of factors, depending on the emulator type used.
/// Any emulation can have it's direction and speed randomized within specified tolerances. By default, speed is
/// limited to between 0 (low) and 5 (high) meters/second and bearing changes are limited to +/- 45 degrees
/// (a 90 degree arc) from North (0) degrees.
/// </remarks>
public void Randomize(Random seed, Speed speedLow, Speed speedHigh, Azimuth bearingStart, Azimuth bearingArc)
{
// Flag it so the emulation will use random values
_isRandom = true;
// Get the randomizer parameters
_seed = seed;
// Get the speed variance
_speedLow = speedLow.ToMetersPerSecond().Value;
_speedHigh = speedHigh.ToMetersPerSecond().Value;
// Get the normalized arc values
_bearingStart = bearingStart.Normalize().DecimalDegrees;
_bearingArc = bearingArc.Normalize().DecimalDegrees;
}
public Arc(Point centerPt, Double incomingDir_, Double outgoingDir_, Double radius)
{
Azimuth incomingDir = Azimuth.ctorAzimuthFromDegree(incomingDir_);
Azimuth outgoingDir = Azimuth.ctorAzimuthFromDegree(outgoingDir_);
int modifier = (outgoingDir - incomingDir) > 0 ? 1 : -1;
Azimuth BegRadiusDirection = incomingDir +
new Deflection(Math.PI / 2.0, -1 * modifier);
BeginRadiusVector = new Vector(
direction: BegRadiusDirection,
length: radius);
Point startPt = centerPt + BeginRadiusVector;
Deflection def = outgoingDir - incomingDir;
populateThis(startPt, incomingDir, def, radius);
}
/// <summary>
///
/// </summary>
/// <param name="style"></param>
/// <returns></returns>
public string ToString(Angle.DataStyle style)
{
string temp = string.Empty;
if (!double.IsNaN(Range))
{
temp += "R:" + Range.ToString("# ###.###");
}
if (Azimuth != null)
{
temp += ", A:" + Azimuth.ToString(style) + ", E:" + Elevation.ToString(style);
}
return(temp);
}
public void setUpTestingModel_20140422()
{
setUpFeaturesForTestingModel();
allGrahics = new List <Graphic>();
this.AddGraphic(new LineSegment(1.0, 8.5, 13.2, 0.8));
this.AddGraphic(new LineSegment(1.0, 2.5, 13.2, 9.5));
this.AddGraphic(new LineSegment(0.0, 0.0, 0.5, 0.25));
this.AddGraphic(new LineSegment(2.0, 1.0, 1.0, 1.0));
this.AddGraphic(new LineSegment(0.0, 0.0, 1.5, 0.0));
this.AddGraphic(new LineSegment(0.1, 0.0, -1.3, -1.3));
this.AddGraphic(new LineSegment(0.1, 0.0, -1.3, -0.7));
this.AddGraphic(new LineSegment(0.1, 0.0, 1.0, -1.0));
this.AddGraphic(new LineSegment(0.1, 0.0, 1.3, -0.7));
this.AddGraphic(new LineSegment(0.1, 0.0, 1.6, -1.6));
this.AddGraphic(new Text("1.6, -0.6", new Point(1.6, -0.6), 0.25));
this.AddGraphic(new Text("-2, +0.9", new Point(-2, 0.9), 0.45));
var rotexPt = new Point(-1.1, -0.6);
//var rotText = new Text("Rotated 1", rotexPt, 0.3);
//rotText.Rotation = Angle.radiansFromDegree(1);
//this.AddGraphic(rotText);
//var rotText2 = new Text("Rotated 5", rotexPt, 0.3);
//rotText2.Rotation = Angle.radiansFromDegree(5);
//this.AddGraphic(rotText2);
var rotText3 = new Text("Rotated 340", rotexPt, 0.3);
rotText3.Rotation = Angle.radiansFromDegree(340);
rotText3.Feature = this.FeatureList.Children["GreenDot"];
this.AddGraphic(rotText3);
var anArc = new Arc(new Point(-1.4, 1.5),
Azimuth.ctorAzimuthFromDegree(20.0),
Deflection.ctorDeflectionFromAngle(310.0, 1), 0.25);
anArc.Feature = this.FeatureList.Children["BlueDashed"];
this.AddGraphic(anArc);
allGrahics[0].Feature = this.FeatureList.Children["GreenDot"];
allGrahics[1].Feature = this.FeatureList.Children["RedThick"];
allGrahics[2].Feature = this.FeatureList.Children["BlueDashed"];
}
public void PgrmfSentenceFromString()
{
Latitude l = new Latitude(1, 0.5, LatitudeHemisphere.North);
Longitude l2 = new Longitude(1, 23.0, LongitudeHemisphere.West);
Position p = new Position(l, l2);
DateTime dt = new DateTime(2003, 12, 4, 21, 59, 48);
Azimuth a = new Azimuth(62.0);
Speed s = new Speed(0, SpeedUnit.KilometersPerHour);
PgrmfSentence sentence =
new PgrmfSentence("$PGRMF,223,424801,041203,215948,13,0100.5000,N,00123.0000,W,A,2,0,62,2,1*35");
Assert.AreEqual("$PGRMF,223,424801,041203,215948,13,0100.5000,N,00123.0000,W,A,2,0,62,2,1*35",
sentence.Sentence);
Assert.AreEqual(dt, sentence.UtcDateTime);
Assert.AreEqual(p, sentence.Position);
Assert.AreEqual(FixMode.Automatic, sentence.FixMode);
Assert.AreEqual(FixMethod.Fix3D, sentence.FixMethod);
Assert.AreEqual(s, sentence.Speed);
Assert.AreEqual(a, sentence.Bearing);
Assert.AreEqual(new DilutionOfPrecision(2), sentence.PositionDilutionOfPrecision);
/*
* 2.2.3
* GPS Fix Data Sentence (PGRMF)
* $PGRMF,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>,<13>,<14>,<15>*hh<CR><LF>
* <0> GPS week number (0 - 1023)
* <1> GPS seconds (0 - 604799)
* <2> UTC date of position fix, ddmmyy format
* <3> UTC time of position fix, hhmmss format
* <4> GPS leap second count
* <5> Latitude, ddmm.mmmm format (leading zeros will be transmitted)
* <6> Latitude hemisphere, N or S
* <7> Longitude, dddmm.mmmm format (leading zeros will be transmitted)
* <8> Longitude hemisphere, E or W
* <9> Mode, M = manual, A = automatic
* <10> Fix type, 0 = no fix, 1 = 2D fix, 2 = 3D fix
* <11> Speed over ground, 0 to 1051 kilometers/hour
* <12> Course over ground, 0 to 359 degrees, true
* <13> Position dilution of precision, 0 to 9 (rounded to nearest integer value)
* <14> Time dilution of precision, 0 to 9 (rounded to nearest integer value)
* hh <CR><LF>
*
*/
}
private void SetPrimitiveData(double x, double y)
{
if (Step == 0)
{
baseX = x;
baseY = y;
Step++;
}
else
{
RotateALL(baseX, baseY, (float)(-angle));
var dx = x - baseX;
var dy = y - baseY;
angle = Azimuth.Create(dx, dy);
RotateALL(baseX, baseY, (float)angle);
Stop();
}
}
public void Azimuth_setFromXY()
{
Tuple <Double, Double, Double>[] testCases =
{
new Tuple <Double, Double, Double>(10, 2, 78.690067526),
new Tuple <Double, Double, Double>(10, -2, 101.309932474),
new Tuple <Double, Double, Double>(-10, 2, 281.309932474),
new Tuple <Double, Double, Double>(-10, -2, 258.690067526)
};
foreach (var testCase in testCases)
{
Azimuth anAzimuth = new Azimuth();
anAzimuth.setFromXY(testCase.Item1, testCase.Item2);
Double actualDegrees = anAzimuth.getAsDegreesDouble();
Assert.AreEqual(expected: testCase.Item3, actual: actualDegrees, delta: delta);
}
}
public void Azimuth_Subtraction_Az340MinusAz268_equalsDeflectionPos72()
{
Azimuth incomingDir = Azimuth.ctorAzimuthFromDegree(268);
var str = incomingDir.ToString();
Assert.AreEqual(268.0, incomingDir.getAsDegreesDouble(), 0.0000001);
Azimuth outgoingDir = Azimuth.ctorAzimuthFromDegree(340);
Assert.AreEqual(340.0, outgoingDir.getAsDegreesDouble(), 0.0000001);
Deflection def = outgoingDir - incomingDir;
Assert.AreEqual
(expected: 72.0
, actual: def.getAsDegreesDouble()
, delta: 0.00001);
Assert.AreEqual
(expected: 1
, actual: def.deflectionDirection);
}
/// <summary>
/// Randomize
/// </summary>
/// <param name="seed">The seed.</param>
/// <param name="speedLow">The speed low.</param>
/// <param name="speedHigh">The speed high.</param>
/// <param name="bearingStart">The bearing start.</param>
/// <param name="bearingArc">The bearing arc.</param>
/// <param name="minHDOP">The min HDOP.</param>
/// <param name="maxHDOP">The max HDOP.</param>
/// <param name="minVDOP">The min VDOP.</param>
/// <param name="maxVDOP">The max VDOP.</param>
public void Randomize(
Random seed,
Speed speedLow,
Speed speedHigh,
Azimuth bearingStart,
Azimuth bearingArc,
DilutionOfPrecision minHDOP,
DilutionOfPrecision maxHDOP,
DilutionOfPrecision minVDOP,
DilutionOfPrecision maxVDOP)
{
Randomize(seed, speedLow, speedHigh, bearingStart, bearingArc);
_minHdop = minHDOP.Value;
_maxHdop = maxHDOP.Value;
_minVdop = minVDOP.Value;
_maxVdop = maxVDOP.Value;
SetRandom(true);
}
/// <summary>
/// Called when [sentence changed].
/// </summary>
/// <remarks></remarks>
protected override void OnSentenceChanged()
{
// Parse the basic sentence information
base.OnSentenceChanged();
// Cache the words
string[] words = Words;
int wordCount = words.Length;
// Do we have enough words to make a heading?
if (wordCount >= 1 && words[0].Length != 0)
{
#region Heading
double heading = 0.0;
double.TryParse(words[0], out heading);
_heading = new Azimuth(heading);
#endregion
}
}
protected Emulator(string name)
{
_Name = name;
// Create new buffers for reading and writing
_ReadBuffer = new List <byte>(_DefaultReadBufferSize);
_WriteBuffer = new List <byte>(_DefaultWriteBufferSize);
// Initialize simulated values
_ReadDataAvailableWaitHandle = new ManualResetEvent(false);
_WriteDataAvailableWaitHandle = new ManualResetEvent(false);
_EmulationIntervalWaitHandle = new ManualResetEvent(false);
// Default timeouts for reading and writing
_ReadTimeout = _DefaultReadTimeout;
_WriteTimeout = _DefaultWriteTimeout;
// Simulated values
_seed = new Random();
_UtcDateTime = DateTime.UtcNow;
_CurrentPosition = Positioning.Position.Empty;
_Altitude = Distance.FromFeet(1000);
_Route = new List <Position>();
_Satellites = new List <Satellite>();
_FixQuality = FixQuality.GpsFix;
_FixMode = FixMode.Automatic;
_FixMethod = FixMethod.Fix3D;
_FixStatus = FixStatus.Fix;
_HorizontalDop = DilutionOfPrecision.Good;
_VerticalDop = DilutionOfPrecision.Good;
_MeanDop = DilutionOfPrecision.Good;
_Speed = Speed.FromStatuteMilesPerHour(20);
_speedLow = Speed.FromKilometersPerSecond(10).Value;
_speedHigh = Speed.FromKilometersPerSecond(25).Value;
_Bearing = Azimuth.Southwest;
_bearingStart = _seed.NextDouble() * 360;
_bearingArc = 10;
}
private void UpdateAzimuthFromTo(AzimuthTypes fromType, AzimuthTypes toType)
{
try
{
double angle = Azimuth.GetValueOrDefault();
if (fromType == AzimuthTypes.Degrees && toType == AzimuthTypes.Mils)
{
angle *= 17.777777778;
}
else if (fromType == AzimuthTypes.Mils && toType == AzimuthTypes.Degrees)
{
angle *= 0.05625;
}
Azimuth = angle;
}
catch (Exception ex)
{
Console.WriteLine(ex);
}
}
public string GenerateReport()
{
Recalculate();
if (String.IsNullOrEmpty(TreeStatus))
{
return(string.Empty);
}
var azimuth = (Azimuth > 0) ? "Azimuth:" + Azimuth.ToString() : String.Empty;
return(String.Format("Tree was {0} (DBH:{1}, slope:{2}%, slope distance:{3:F2}', limiting distance:{4:F2}' to {5} of tree, {6}:{7}) {8}\r\n",
TreeStatus,
DBH,
SlopePCT,
SlopeDistance,
LimitingDistance,
MeasureTo.ToString(),
(IsVariableRadius) ? "BAF" : "FPS",
BAForFPSize,
azimuth));
}
public void GprmcSentenceFromObjects()
{
Latitude l = new Latitude(51, 33.82, LatitudeHemisphere.North);
Longitude l2 = new Longitude(0, 42.24, LongitudeHemisphere.West);
Position p = new Position(l, l2);
DateTime date = new DateTime(2004, 06, 13, 22, 05, 16, DateTimeKind.Utc);
Speed s = new Speed(173.8, SpeedUnit.Knots);
Azimuth a = new Azimuth(231.8);
Longitude mv = new Longitude(4.2, LongitudeHemisphere.West);
GprmcSentence sentence = new GprmcSentence(date, true, p, s, a, mv);
Assert.AreEqual("$GPRMC,220516.000,A,5133.8200,N,00042.2400,W,173.8,231.8,130604,4.2,W*67", sentence.Sentence);
Assert.AreEqual(FixStatus.Fix, sentence.FixStatus);
Assert.AreEqual(l, sentence.Position.Latitude);
Assert.AreEqual(l2, sentence.Position.Longitude);
Assert.AreEqual(s, sentence.Speed);
Assert.AreEqual(a, sentence.Bearing);
Assert.AreEqual(mv, sentence.MagneticVariation);
Assert.AreEqual(date, sentence.UtcDateTime);
}
private void UpdateAzimuthFromTo(AzimuthTypes fromType, AzimuthTypes toType)
{
try
{
double angle = Azimuth.GetValueOrDefault();
if (fromType == AzimuthTypes.Degrees && toType == AzimuthTypes.Mils)
{
angle *= ValueConverterConstant.DegreeToMils;
}
else if (fromType == AzimuthTypes.Degrees && toType == AzimuthTypes.Gradians)
{
angle *= ValueConverterConstant.DegreeToGradian;
}
else if (fromType == AzimuthTypes.Mils && toType == AzimuthTypes.Degrees)
{
angle *= ValueConverterConstant.MilsToDegree;
}
else if (fromType == AzimuthTypes.Mils && toType == AzimuthTypes.Gradians)
{
angle *= ValueConverterConstant.MilsToGradian;
}
else if (fromType == AzimuthTypes.Gradians && toType == AzimuthTypes.Degrees)
{
angle *= ValueConverterConstant.GradianToDegree;
}
else if (fromType == AzimuthTypes.Gradians && toType == AzimuthTypes.Mils)
{
angle *= ValueConverterConstant.GradianToMils;
}
Azimuth = angle;
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine(ex);
}
}
/// <summary>
/// Returns the current instance adjusted to the specified orientation and
/// origin.
/// </summary>
public PolarCoordinate ToOrientation(Azimuth origin, PolarCoordinateOrientation orientation)
{
if (_Orientation.Equals(orientation) && _Origin.Equals(origin))
{
return(this);
}
// Make a copy of the angle
double NewAngle = Theta.DecimalDegrees;
// Has the CW/CCW orientation changed?
if (Orientation != orientation)
{
// Yes. Subtract the angle from 360
NewAngle = 360 - NewAngle;
}
if (Origin != origin)
{
// Add the offset to the angle and normalize
NewAngle -= 360 - origin.DecimalDegrees - Origin.DecimalDegrees;
}
// And return the new coordinate
return(new PolarCoordinate(_R, new Angle(NewAngle), origin, orientation));
}
/// <summary>
/// Creates a GphdtSentence from the specified parameters
/// </summary>
/// <param name="heading">The heading.</param>
/// <remarks></remarks>
public GphdtSentence(Azimuth heading)
{
// Build a sentence
StringBuilder builder = new StringBuilder(128);
#region Append the command word
// Append the command word
builder.Append("$GPHDT");
#endregion Append the command word
// Append a comma
builder.Append(',');
#region Append the Heading
builder.Append(heading.DecimalDegrees);
#endregion Append the Heading
// Append a comma
builder.Append(',');
#region Append the TRUE
builder.Append("T");
#endregion Append the TRUE
// Set this object's sentence
SetSentence(builder.ToString());
// Finally, append the checksum
AppendChecksum();
}
/// <summary>
/// Creates a GphdtSentence from the specified parameters
/// </summary>
/// <param name="heading">The heading.</param>
/// <remarks></remarks>
public GphdtSentence(Azimuth heading)
{
// Build a sentence
StringBuilder builder = new StringBuilder(128);
#region Append the command word
// Append the command word
builder.Append("$GPHDT");
#endregion Append the command word
// Append a comma
builder.Append(',');
#region Append the Heading
builder.Append(heading.DecimalDegrees);
#endregion Append the Heading
// Append a comma
builder.Append(',');
#region Append the TRUE
builder.Append("T");
#endregion Append the TRUE
// Set this object's sentence
SetSentence(builder.ToString());
// Finally, append the checksum
AppendChecksum();
}
private void SetPrimitiveData(double x, double y)
{
if (Step == 0)
{
if (Primitive.CurrentSelectedPrimitives.Count < 1)
{
if (Select())
{
Step++;
}
}
baseX = Primitive.ResultX;
baseY = Primitive.ResultY;
}
else
{
RotateAt(baseX, baseY, (float)(-angle));
var dx = x - baseX;
var dy = y - baseY;
angle = Azimuth.Create(dx, dy);
RotateAt(baseX, baseY, (float)angle);
Stop();
}
}
/// <summary>
/// Returns the current instance adjusted to the specified orientation and
/// origin.
/// </summary>
public PolarCoordinate ToOrientation(Azimuth origin, PolarCoordinateOrientation orientation)
{
if (_Orientation.Equals(orientation) && _Origin.Equals(origin))
return this;
// Make a copy of the angle
double NewAngle = Theta.DecimalDegrees;
// Has the CW/CCW orientation changed?
if (Orientation != orientation)
// Yes. Subtract the angle from 360
NewAngle = 360 - NewAngle;
if (Origin != origin)
{
// Add the offset to the angle and normalize
NewAngle -= 360 - origin.DecimalDegrees - Origin.DecimalDegrees;
}
// And return the new coordinate
return new PolarCoordinate(_R, new Angle(NewAngle), origin, orientation);
}
/// <summary>
/// Overrides OnSentanceChanged for the GPVTGSentence
/// </summary>
protected override void OnSentenceChanged()
{
// First, process the basic info for the sentence
base.OnSentenceChanged();
// Cache the sentence words
string[] words = Words;
int wordCount = words.Length;
/*
* $GPVTG
Track Made Good and Ground Speed.
eg1. $GPVTG, 360.0, T, 348.7, M, 000.0, N, 000.0, K*43
eg2. $GPVTG, 054.7, T, 034.4, M, 005.5, N, 010.2, K
054.7, T True track made good
034.4, M Magnetic track made good
005.5, N Ground speed, knots
010.2, K Ground speed, Kilometers per hour
eg3. $GPVTG, t, T, ,, s.ss, N, s.ss, K*hh
0 = Track made good
1 = Fixed text 'T' indicates that track made good is relative to true north
2 = not used
3 = not used
4 = Speed over ground in knots
5 = Fixed text 'N' indicates that speed over ground in in knots
6 = Speed over ground in kilometers/hour
7 = Fixed text 'K' indicates that speed over ground is in kilometers/hour
*
*
*/
#region Heading
if (wordCount >= 1 && words[0].Length != 0)
_heading = Azimuth.Parse(words[0], NmeaCultureInfo);
else
_heading = Azimuth.Invalid;
#endregion Heading
}
/// <summary>
/// Adds a new observation and applies the filter.
/// </summary>
/// <param name="gpsPosition">The new observation to add to the filter.</param>
/// <param name="deviceError">A DeviceError, which does not currently affect position averaging.</param>
/// <param name="horizontalDOP">A horizontal dilution of position, which does not currently affect position averaging.</param>
/// <param name="verticalDOP">A vertical dilution of positoin which does not currently affect position averaging.</param>
/// <param name="bearing">A directional bearing, which does not currently affect position averaging.</param>
/// <param name="speed">A speed, which does not currently affect position averaging.</param>
/// <returns></returns>
/// <remarks>This method updates the FilteredLocation property without consideration for SampleCount.</remarks>
public override Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
_samples.Add(gpsPosition);
_sampleTimes.Add(DateTime.Now);
int count = _samples.Count;
int timeCount = _sampleTimes.Count;
int maxCount = 0;
// Only average the number of samples specified in the constructor
while (count > _sampleCount)
{
_samples.RemoveAt(0);
count--;
maxCount++;
}
// Only 2 times are needed, oldest and most recent.
// Try to remove as many as were removed from the sample collection.
while (timeCount > 2 && maxCount > 0)
{
_sampleTimes.RemoveAt(0);
timeCount--;
}
Filter();
return(_filteredPositon);
}
/// <summary>
/// Updates the state.
/// </summary>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="bearing">The bearing.</param>
/// <param name="speed">The speed.</param>
/// <param name="z">The z.</param>
public void UpdateState(Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed, Position3D z)
{
if (_x.IsInvalid)
{
Initialize(z);
return;
}
// More insanity
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Covariance values are invalid. Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_deviceError = deviceError.Value;
_horizontalDOP = horizontalDOP.Value;
_verticalDOP = verticalDOP.Value;
double hCovariance = _deviceError * _horizontalDOP;
double vCovariance = _deviceError * _verticalDOP;
// Setup the observation covariance (measurement error)
_r = new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance);
#region Process Noise Estimation
// Get the translation of the last correction
CartesianPoint subX = _x.ToPosition3D(_ellipsoid)
.TranslateTo(bearing, speed.ToDistance(_delay), _ellipsoid)
.ToCartesianPoint();
// Get the vector of the translation and the last observation
//CartesianPoint w = (subX - this.z);
CartesianPoint w =
new CartesianPoint(
Distance.FromMeters(subX.X.Value - _z.X.Value), // Values are in meters
Distance.FromMeters(subX.Y.Value - _z.Y.Value), // Values are in meters
Distance.FromMeters(subX.Z.Value - _z.Z.Value)); // Values are in meters
// Setup the noise covariance (process error)
_q = new SquareMatrix3D(
Math.Abs(w.X.Value), 0, 0,
0, Math.Abs(w.Y.Value), 0,
0, 0, Math.Abs(w.Z.Value));
#endregion Process Noise Estimation
// Update the observation state
_z = z.ToCartesianPoint(_ellipsoid);
#region State vector prediction and covariance
//s.x = s.A*s.x + s.B*s.u;
//this.x = this.A * this.x + this.B * this.u;
CartesianPoint ax = _a.TransformVector(_x);
CartesianPoint bu = _b.TransformVector(_u);
_x =
new CartesianPoint(
Distance.FromMeters(ax.X.Value + bu.X.Value),
Distance.FromMeters(ax.Y.Value + bu.Y.Value),
Distance.FromMeters(ax.Z.Value + bu.Z.Value));
//s.P = s.A * s.P * s.A' + s.Q;
_p = _a * _p * SquareMatrix3D.Transpose(_a) + _q;
#endregion State vector prediction and covariance
#region Kalman gain factor
//K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);
SquareMatrix3D ht = SquareMatrix3D.Transpose(_h);
SquareMatrix3D k = _p * ht * SquareMatrix3D.Invert(_h * _p * ht + _r);
#endregion Kalman gain factor
#region Observational correction
//s.x = s.x + K*(s.z-s.H*s.x);
//this.x = this.x + K * (this.z - this.H * this.x);
CartesianPoint hx = _h.TransformVector(_x);
CartesianPoint zHx = new CartesianPoint(
Distance.FromMeters(_z.X.Value - hx.X.Value),
Distance.FromMeters(_z.Y.Value - hx.Y.Value),
Distance.FromMeters(_z.Z.Value - hx.Z.Value));
CartesianPoint kzHx = k.TransformVector(zHx);
_x =
new CartesianPoint(
Distance.FromMeters(_x.X.Value + kzHx.X.Value),
Distance.FromMeters(_x.Y.Value + kzHx.Y.Value),
Distance.FromMeters(_x.Z.Value + kzHx.Z.Value));
//s.P = s.P - K*s.H*s.P;
_p = _p - k * _h * _p;
#endregion Observational correction
// Bump the state count
_interval++;
// Calculate the average error for the system stste.
_errorState = (_errorState + Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2))) * .5f;
// Calculate the interval between samples
DateTime now = DateTime.Now;
_delay = now.Subtract(_lastObservation);
_lastObservation = now;
}
protected override void OnSentenceChanged()
{
// First, process the basic info for the sentence
base.OnSentenceChanged();
// Cache the sentence words
string[] words = base.Words;
int wordCount = words.Length;
/*
* $GPRMC
Recommended minimum specific GPS/Transit data
eg1. $GPRMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130998,011.3,E*62
eg2. $GPRMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68
225446 Time of fix 22:54:46 UTC
A Navigation receiver warning A = OK, V = warning
4916.45,N Latitude 49 deg. 16.45 min North
12311.12,W Longitude 123 deg. 11.12 min West
000.5 Speed over ground, Knots
054.7 Course Made Good, True
191194 Date of fix 19 November 1994
020.3,E Magnetic variation 20.3 deg East
*68 mandatory checksum
eg3. $GPRMC,220516,A,5133.82,N,00042.24,W,173.8,231.8,130694,004.2,W*70
1 2 3 4 5 6 7 8 9 10 11 12
1 220516 Time Stamp
2 A validity - A-ok, V-invalid
3 5133.82 current Latitude
4 N North/South
5 00042.24 current Longitude
6 W East/West
7 173.8 Speed in knots
8 231.8 True course
9 130694 Date Stamp
10 004.2 Variation
11 W East/West
12 *70 checksum
eg4. $GPRMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,ddmmyy,x.x,a*hh
1 = UTC of position fix
2 = Data status (V=navigation receiver warning)
3 = Latitude of fix
4 = N or S
5 = Longitude of fix
6 = E or W
7 = Speed over ground in knots
8 = Track made good in degrees True
9 = UT date
10 = Magnetic variation degrees (Easterly var. subtracts from true course)
11 = E or W
12 = Checksum
*/
// Do we have enough words to parse the fix status?
if (wordCount >= 2 && words[1].Length != 0)
{
#region Fix status
// Get the fix flag
_FixStatus = words[1].Equals("A", StringComparison.OrdinalIgnoreCase) ? FixStatus.Fix : FixStatus.NoFix;
#endregion
}
else
{
// The fix status is invalid
_FixStatus = FixStatus.Unknown;
}
// Do we have enough words to parse the UTC date/time?
if (wordCount >= 9 && words[0].Length != 0 && words[8].Length != 0)
{
#region Parse the UTC time
string utcTimeWord = words[0];
int utcHours = int.Parse(utcTimeWord.Substring(0, 2), NmeaCultureInfo); // AA
int utcMinutes = int.Parse(utcTimeWord.Substring(2, 2), NmeaCultureInfo); // BB
int utcSeconds = int.Parse(utcTimeWord.Substring(4, 2), NmeaCultureInfo); // CC
int utcMilliseconds = 0;
if(utcTimeWord.Length > 6)
utcMilliseconds = Convert.ToInt32(float.Parse(utcTimeWord.Substring(6), NmeaCultureInfo) * 1000, NmeaCultureInfo); // DDDD
#endregion
#region Parse the UTC date
string utcDateWord = words[8];
int utcDay = int.Parse(utcDateWord.Substring(0, 2), NmeaCultureInfo);
int utcMonth = int.Parse(utcDateWord.Substring(2, 2), NmeaCultureInfo);
int utcYear = int.Parse(utcDateWord.Substring(4, 2), NmeaCultureInfo) + 2000;
#endregion
#region Build a UTC date/time
_UtcDateTime = new DateTime(utcYear, utcMonth, utcDay, utcHours, utcMinutes, utcSeconds, utcMilliseconds, DateTimeKind.Utc);
#endregion
}
else
{
// The UTC date/time is invalid
_UtcDateTime = DateTime.MinValue;
}
// Do we have enough data to parse the location?
if (wordCount >= 6 && words[2].Length != 0 && words[3].Length != 0 && words[4].Length != 0 && words[5].Length != 0)
{
#region Parse the latitude
string latitudeWord = words[2];
int latitudeHours = int.Parse(latitudeWord.Substring(0, 2), NmeaCultureInfo);
double latitudeDecimalMinutes = double.Parse(latitudeWord.Substring(2), NmeaCultureInfo);
LatitudeHemisphere latitudeHemisphere =
words[3].Equals("N", StringComparison.Ordinal) ? LatitudeHemisphere.North : LatitudeHemisphere.South;
#endregion
#region Parse the longitude
string longitudeWord = words[4];
int longitudeHours = int.Parse(longitudeWord.Substring(0, 3), NmeaCultureInfo);
double longitudeDecimalMinutes = double.Parse(longitudeWord.Substring(3), NmeaCultureInfo);
LongitudeHemisphere longitudeHemisphere =
words[5].Equals("E", StringComparison.Ordinal) ? LongitudeHemisphere.East : LongitudeHemisphere.West;
#endregion
#region Build a Position from the latitude and longitude
_Position = new Position(
new Latitude(latitudeHours, latitudeDecimalMinutes, latitudeHemisphere),
new Longitude(longitudeHours, longitudeDecimalMinutes, longitudeHemisphere));
#endregion
}
else
{
_Position = Position.Invalid;
}
// Do we have enough info to process speed?
if (wordCount >= 7 && words[6].Length != 0)
{
#region Speed
// The speed is the sixth word, expressed in knots
_Speed = new Speed(double.Parse(words[6], NmeaCultureInfo), SpeedUnit.Knots);
#endregion
}
else
{
// The speed is invalid
_Speed = Speed.Invalid;
}
// do we have enough info to process the bearing?
if (wordCount >= 8 && words[7].Length != 0)
{
#region Bearing
// The bearing is the seventh word
_Bearing = new Azimuth(double.Parse(words[7], NmeaCultureInfo));
#endregion
}
else
{
// The bearing is invalid
_Bearing = Azimuth.Invalid;
}
// Do we have enough info for magnetic variation?
if (wordCount >= 10 && words[9].Length != 0)
{
// Parse the value
_MagneticVariation = new Longitude(double.Parse(words[9], NmeaCultureInfo));
}
else
{
// The magnetic variation is invalid
_MagneticVariation = Longitude.Invalid;
}
}
/// <summary>
/// Creates a new instance containing the specified Azimuth object.
/// </summary>
public AzimuthEventArgs(Azimuth angle)
{
_Azimuth = angle;
}
/// <inheritdocs/>
protected override void OnPaintOffScreen(PaintEventArgs e)
{
PolarGraphics f = CreatePolarGraphics(e.Graphics);
// What bearing are we drawing?
#if PocketPC
Azimuth BearingToRender = _Bearing;
#else
Azimuth bearingToRender = new Azimuth(_valueInterpolator[_interpolationIndex]);
#endif
// Cache drawing options in order to prevent race conditions during
// drawing!
double minorInterval = _minorTickInterval.DecimalDegrees;
double majorInterval = _majorTickInterval.DecimalDegrees;
double directionInterval = _directionLabelInterval.DecimalDegrees;
double angleInterval = _angleLabelInterval.DecimalDegrees;
// Draw tick marks
if (minorInterval > 0)
{
for (double angle = 0; angle < 360; angle += minorInterval)
{
// And draw a line
f.DrawLine(_minorTickPen, new PolarCoordinate(98, angle), new PolarCoordinate(100, angle));
}
}
// Draw tick marks
if (majorInterval > 0)
{
for (double angle = 0; angle < 360; angle += majorInterval)
{
// And draw a line
f.DrawLine(_majorTickPen, new PolarCoordinate(95, angle), new PolarCoordinate(100, angle));
}
}
if (directionInterval > 0)
{
for (double angle = 0; angle < 360; angle += directionInterval)
{
// And draw a line
f.DrawLine(_directionTickPen, new PolarCoordinate(92, angle), new PolarCoordinate(100, angle));
}
}
if (angleInterval > 0)
{
for (double angle = 0; angle < 360; angle += angleInterval)
{
// Get the coordinate of the line's start
PolarCoordinate start = new PolarCoordinate(60, angle, Azimuth.North, PolarCoordinateOrientation.Clockwise);
#if PocketPC
f.DrawCenteredString(((Angle)angle).ToString(_AngleLabelFormat, CultureInfo.CurrentCulture), _AngleLabelFont, _AngleLabelBrush, start);
#else
f.DrawRotatedString(((Angle)angle).ToString(_angleLabelFormat, CultureInfo.CurrentCulture), _angleLabelFont, _angleLabelBrush, start);
#endif
}
}
if (directionInterval > 0)
{
for (double angle = 0; angle < 360; angle += directionInterval)
{
// Get the coordinate of the line's start
PolarCoordinate start = new PolarCoordinate(80, angle, Azimuth.North, PolarCoordinateOrientation.Clockwise);
#if PocketPC
f.DrawCenteredString(((Azimuth)angle).ToString("c", CultureInfo.CurrentCulture), _DirectionLabelFont, _DirectionLabelBrush, start);
#else
f.DrawRotatedString(((Azimuth)angle).ToString("c", CultureInfo.CurrentCulture), _directionLabelFont, _directionLabelBrush, start);
#endif
}
}
// Draw an ellipse at the center
f.DrawEllipse(_centerPen, PolarCoordinate.Empty, 10);
// Now draw the needle shadow
PolarCoordinate[] needleNorth = _needlePointsNorth.Clone() as PolarCoordinate[];
PolarCoordinate[] needleSouth = _needlePointsSouth.Clone() as PolarCoordinate[];
// Adjust the needle to the current bearing
if (needleNorth != null)
{
for (int index = 0; index < needleNorth.Length; index++)
{
needleNorth[index] = needleNorth[index].Rotate(bearingToRender.DecimalDegrees);
if (needleSouth != null)
{
needleSouth[index] = needleSouth[index].Rotate(bearingToRender.DecimalDegrees);
}
}
}
#if !PocketPC
// Now draw a shadow
f.Graphics.TranslateTransform(_pNeedleShadowSize.Width, _pNeedleShadowSize.Height, MatrixOrder.Append);
f.FillPolygon(_pNeedleShadowBrush, needleNorth);
f.FillPolygon(_pNeedleShadowBrush, needleSouth);
f.Graphics.ResetTransform();
#endif
f.FillPolygon(_pNorthNeedleBrush, needleNorth);
f.DrawPolygon(_pNorthNeedlePen, needleNorth);
f.FillPolygon(_pSouthNeedleBrush, needleSouth);
f.DrawPolygon(_pSouthNeedlePen, needleSouth);
}
public void Randomize(
Random seed,
Speed speedLow,
Speed speedHigh,
Azimuth bearingStart,
Azimuth bearingArc,
DilutionOfPrecision minHDOP,
DilutionOfPrecision maxHDOP,
DilutionOfPrecision minVDOP,
DilutionOfPrecision maxVDOP)
{
base.Randomize(seed, speedLow, speedHigh, bearingStart, bearingArc);
_minHDOP = minHDOP.Value;
_maxHDOP = maxHDOP.Value;
_minVDOP = minVDOP.Value;
_maxVDOP = maxVDOP.Value;
SetRandom(true);
}
/// <summary>
/// Does the initialize.
/// </summary>
private void DoInitialize()
{
_altitude = Distance.Invalid;
_altitudeAboveEllipsoid = _altitude;
_bearing = Azimuth.Invalid;
_heading = Azimuth.Invalid;
_fixStatus = FixStatus.Unknown;
_horizontalDop = DilutionOfPrecision.Maximum;
_magneticVariation = Longitude.Invalid;
_position = Position.Invalid;
_speed = Speed.Invalid;
_meanDop = DilutionOfPrecision.Invalid;
_verticalDop = DilutionOfPrecision.Invalid;
if (_satellites != null)
_satellites.Clear();
}
/// <summary>
/// Randomizes the emulation by changing speed and direction
/// </summary>
/// <remarks>
/// GPS coordinate emulation can be randomized by any number of factors, depending on the emulator type used.
/// Any emulation can have it's direction and speed randomized within specified tolerances. By default, speed is
/// limited to between 0 (low) and 5 (high) meters/second and bearing changes are limited to +/- 45 degrees
/// (a 90 degree arc) from North (0) degrees.
/// </remarks>
public virtual void Randomize()
{
// Flag it so the emulation will use random values
_isRandom = true;
// Randomize the speed within range
_Speed = Speed.FromMetersPerSecond((_seed.NextDouble() * (_speedHigh - _speedLow)) + _speedLow);
// Randomize the bearing within the arc.
_Bearing = new Azimuth(_seed.NextDouble() * _bearingArc + (_bearingStart - (_bearingArc * .5))).Normalize();
// Reset the bearing origin
_bearingStart = _Bearing.DecimalDegrees;
}
/// <summary>
/// Updates the current direction of heading.
/// </summary>
/// <param name="value">The value.</param>
protected virtual void SetHeading(Azimuth value)
{
// If the new value is invalid, ignore it
if (value.IsInvalid)
return;
// Notify of the receipt
if (HeadingReceived != null)
HeadingReceived(this, new AzimuthEventArgs(_heading));
// Change the devices class
Devices.Heading = _heading;
// If the value hasn't changed, skiiiip
if (_heading.Equals(value))
return;
// Yes. Set the new value
_heading = value;
// Notify of the change
if (HeadingChanged != null)
HeadingChanged(this, new AzimuthEventArgs(_heading));
}
protected Emulator(string name)
{
_Name = name;
// Create new buffers for reading and writing
_ReadBuffer = new List<byte>(_DefaultReadBufferSize);
_WriteBuffer = new List<byte>(_DefaultWriteBufferSize);
// Initialize simulated values
_ReadDataAvailableWaitHandle = new ManualResetEvent(false);
_WriteDataAvailableWaitHandle = new ManualResetEvent(false);
_EmulationIntervalWaitHandle = new ManualResetEvent(false);
// Default timeouts for reading and writing
_ReadTimeout = _DefaultReadTimeout;
_WriteTimeout = _DefaultWriteTimeout;
// Simulated values
_seed = new Random();
_UtcDateTime = DateTime.UtcNow;
_CurrentPosition = GeoFramework.Position.Empty;
_Altitude = Distance.FromFeet(1000);
_Route = new List<Position>();
_Satellites = new List<Satellite>();
_FixQuality = FixQuality.GpsFix;
_FixMode = FixMode.Automatic;
_FixMethod = FixMethod.Fix3D;
_FixStatus = FixStatus.Fix;
_HorizontalDop = DilutionOfPrecision.Good;
_VerticalDop = DilutionOfPrecision.Good;
_MeanDop = DilutionOfPrecision.Good;
_Speed = Speed.FromStatuteMilesPerHour(20);
_speedLow = Speed.FromKilometersPerSecond(10).Value;
_speedHigh = Speed.FromKilometersPerSecond(25).Value;
_Bearing = Azimuth.Southwest;
_bearingStart = _seed.NextDouble() * 360;
_bearingArc = 10;
}
/// <summary>
/// Adds a new observation and applies the filter.
/// </summary>
/// <param name="gpsPosition"> The new observation to add to the filter. </param>
/// <param name="deviceError"> Does not currently affect position averaging. </param>
/// <param name="horizontalDOP"> Does not currently affect position averaging. </param>
/// <param name="verticalDOP"> Does not currently affect position averaging. </param>
/// <param name="bearing"> Does not currently affect position averaging. </param>
/// <param name="speed"> Does not currently affect position averaging. </param>
/// <remarks>
/// This method updates the FilteredLocation property without consideration for SampleCount.
/// </remarks>
public override Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
this._samples.Add(gpsPosition);
this._sampleTimes.Add(DateTime.Now);
int count = this._samples.Count;
int timeCount = this._sampleTimes.Count;
int maxCount = 0;
// Only average the number of samples specified in the constructor
while (count > _sampleCount)
{
this._samples.RemoveAt(0);
count--;
maxCount++;
}
// Only 2 times are needed, oldest and most recent.
// Try to remove as many as were removed from the sample collection.
while (timeCount > 2 && maxCount > 0)
{
this._sampleTimes.RemoveAt(0);
timeCount--;
}
Filter();
return _filteredPositon;
}
/// <summary>
/// Creates a new instance by converting the specified string.
/// </summary>
/// <param name="value">A <strong>String</strong> describing a polar coordinate in any culture.</param>
/// <param name="culture">A <strong>CultureInfo</strong> object describing how to parse the specified string.</param>
public PolarCoordinate(string value, CultureInfo culture)
{
// Split the two values based on the list separator
string[] Values = value.Split(culture.TextInfo.ListSeparator.ToCharArray());
// The first value is R, the second is Theta
_R = float.Parse(Values[0], culture);
_Theta = Angle.Parse(Values[1], culture);
_Origin = Azimuth.North;
_Orientation = PolarCoordinateOrientation.Clockwise;
}
/// <summary>
/// Overrides OnSentanceChanged for the GPVTGSentence
/// </summary>
protected override void OnSentenceChanged()
{
// First, process the basic info for the sentence
base.OnSentenceChanged();
// Cache the sentence words
string[] words = base.Words;
int wordCount = words.Length;
/*
* $GPVTG
Track Made Good and Ground Speed.
eg1. $GPVTG,360.0,T,348.7,M,000.0,N,000.0,K*43
eg2. $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K
054.7,T True track made good
034.4,M Magnetic track made good
005.5,N Ground speed, knots
010.2,K Ground speed, Kilometers per hour
eg3. $GPVTG,t,T,,,s.ss,N,s.ss,K*hh
1 = Track made good
2 = Fixed text 'T' indicates that track made good is relative to true north
3 = not used
4 = not used
5 = Speed over ground in knots
6 = Fixed text 'N' indicates that speed over ground in in knots
7 = Speed over ground in kilometers/hour
8 = Fixed text 'K' indicates that speed over ground is in kilometers/hour
*
*
*/
#region Bearing
if (wordCount >= 1 && words[0].Length != 0)
_Bearing = Azimuth.Parse(words[0], NmeaCultureInfo);
else
_Bearing = Azimuth.Invalid;
#endregion
#region Magnetic Variation
if (wordCount >= 3 && words[2].Length != 0)
_MagneticVariation = new Longitude(double.Parse(words[2], NmeaCultureInfo) - _Bearing.DecimalDegrees);
else
_MagneticVariation = Longitude.Invalid;
#endregion
#region Speed
/* Speed is reported as both knots and KM/H. We can parse either of the values.
* First, try to parse knots. If that fails, parse KM/h.
*/
if (wordCount > 6 && words[5].Length != 0)
{
_Speed = new Speed(
// Parse the numeric portion
double.Parse(words[5], NmeaCultureInfo),
// Use knots
SpeedUnit.Knots);
}
else if (wordCount > 8 && words[7].Length != 0)
{
_Speed = new Speed(
// Parse the numeric portion
double.Parse(words[7], NmeaCultureInfo),
// Use knots
SpeedUnit.KilometersPerHour);
}
else
{
// Invalid speed
_Speed = Speed.Invalid;
}
#endregion
}
internal PolarGraphics(Graphics g, Rectangle visibleClipBounds,
Angle rotation, Azimuth origin, PolarCoordinateOrientation orientation,
float centerR, float maximumR)
/// <summary>
/// Called when [sentence changed].
/// </summary>
/// <remarks></remarks>
protected override void OnSentenceChanged()
{
// Parse the basic sentence information
base.OnSentenceChanged();
// Cache the words
string[] words = Words;
int wordCount = words.Length;
// Do we have enough words to make a heading?
if (wordCount >= 1 && words[0].Length != 0)
{
#region Heading
double heading = 0.0;
double.TryParse(words[0], out heading);
_heading = new Azimuth(heading);
#endregion
}
}
internal PolarGraphics(Graphics g, Angle rotation, Azimuth origin,
PolarCoordinateOrientation orientation, float centerR, float maximumR)
#endif
{
this.g = g;
pRotation = rotation;
pOrigin = origin;
pOrientation = orientation;
pCenterR = centerR;
pMaximumR = maximumR;
#if PocketPC
HalfWidth = (visibleClipBounds.Width * 0.5);
HalfHeight = (visibleClipBounds.Height * 0.5);
// // Create smoother graphics
// gx = new DotSpatial.Positioning.Drawing.GraphicsX(visibleClipBounds.Width, visibleClipBounds.Height);
// gx.Clear(Color.White);
// gx.ResetTransform();
#else
HalfWidth = g.VisibleClipBounds.Width * 0.5;
HalfHeight = g.VisibleClipBounds.Height * 0.5;
#endif
SpanSize = Math.Abs(pMaximumR - pCenterR);
HorizontalScale = HalfWidth / SpanSize;
VerticalScale = HalfHeight / SpanSize;
#if !PocketPC
// Set up the string format
pStringFormat.LineAlignment = StringAlignment.Center;
pStringFormat.Alignment = StringAlignment.Center;
#endif
}
/// <summary>
/// OnSentanceChanged event handler
/// </summary>
protected override void OnSentenceChanged()
{
base.OnSentenceChanged();
// Cache the sentence words
string[] words = Words;
int wordCount = words.Length;
/*
* Garmin produces several embedded GPS systems. They are easy to setup because Garmin provides a nice utility for uploading configuration data to the GPS. You first load the utility to a PC. Connect the PC to the GPS through one of the serial ports. The utility will check each baud rate until it communicates with the GPS.
The common configuration parameters are output sentences from the GPS unit, the communication baud rate with a host, and the required pulse per second.
Each sentence is preceded with a ‘$’ symbol and ends with a line-feed character. At one sentence per second, the following is out put in four seconds:
$PGRMF, 223, 424798, 041203, 215945, 13, 0000.0000, N, 00000.0000, W, A, 2, 0, 62, 2, 1*3B
$PGRMF, 223, 424799, 041203, 215946, 13, 00000.0000, N, 00000.0000, W, A, 2, 0, 62, 2, 1*39
$PGRMF, 223, 424800, 041203, 215947, 13, 00000.0000, N, 00000.0000, W, A, 2, 0, 62, 2, 1*34
$PGRMF, 223, 424801, 041203, 215948, 13, 00000.0000, N, 00000.0000, W, A, 2, 0, 62, 2, 1*35
The sentence is proprietary to the Garmin GPS Global Positioning System and is translated below.
$PGRMF
<1>GPS Week Number(0-1023)
<2>GPS Seconds (0 - 604799)
<3>UTC Date of position fix, ddmmyy format
<4>UTC time of position fix, hhmmss format
<5>GPS leap second count
<6>Latitude, ddmm.mmmm format (leading zeros transmitted)
<7>Latitude hemisphere N or S
<8>Longitude, ddmm.mmmm format (leading zeros transmitted)
<9>Longitude hemisphere N or S
<10>Mode M = Manual, A automatic
<11>Fix type 0 = No Fix, 1 = 2D Fix, 2 = 3D fix
<12>Speed over ground, 0 to 359 degrees true
<13>Course over ground, 0 to 9 (rounded to nearest intvalue)
<14>Time dilution of precision, 0 to 9 (rnded nearest int val)
<15>Time dilution of precision, 0 to 9 (rnded nearest int val)
*hh <CR><LF>
*/
// TODO: Convert GPS week number/seconds to UTC date/time
// Do we have enough words to parse the fix status?
if (wordCount >= 4 && words[2].Length != 0 && words[3].Length != 0)
{
// Build a UTC date/time object.
_utcDateTime = new DateTime(
int.Parse(words[2].Substring(4, 2), NmeaCultureInfo) + 2000, // Year
int.Parse(words[2].Substring(2, 2), NmeaCultureInfo), // Month
int.Parse(words[2].Substring(0, 2), NmeaCultureInfo), // Day
int.Parse(words[3].Substring(0, 2), NmeaCultureInfo), // Hour
int.Parse(words[3].Substring(2, 2), NmeaCultureInfo), // Minute
int.Parse(words[3].Substring(4, 2), NmeaCultureInfo), // Second
DateTimeKind.Utc);
}
#region Position
// Can we parse the latitude and longitude?
if (wordCount >= 8 && words[5].Length != 0 && words[6].Length != 0 && words[7].Length != 0 && words[8].Length != 0)
{
#region Parse the latitude
string latitudeWord = words[5];
int latitudeHours = int.Parse(latitudeWord.Substring(0, 2), NmeaCultureInfo);
double latitudeDecimalMinutes = double.Parse(latitudeWord.Substring(2), NmeaCultureInfo);
LatitudeHemisphere latitudeHemisphere =
words[6].Equals("N", StringComparison.Ordinal) ? LatitudeHemisphere.North : LatitudeHemisphere.South;
#endregion Parse the latitude
#region Parse the longitude
string longitudeWord = words[7];
int longitudeHours = int.Parse(longitudeWord.Substring(0, 3), NmeaCultureInfo);
double longitudeDecimalMinutes = double.Parse(longitudeWord.Substring(3), NmeaCultureInfo);
LongitudeHemisphere longitudeHemisphere =
words[8].Equals("E", StringComparison.Ordinal) ? LongitudeHemisphere.East : LongitudeHemisphere.West;
#endregion Parse the longitude
#region Build a Position from the latitude and longitude
_position = new Position(
new Latitude(latitudeHours, latitudeDecimalMinutes, latitudeHemisphere),
new Longitude(longitudeHours, longitudeDecimalMinutes, longitudeHemisphere));
#endregion Build a Position from the latitude and longitude
}
else
{
_position = Position.Invalid;
}
#endregion Position
#region Fix Mode
if (wordCount >= 9 && words[9].Length != 0)
{
_fixMode = words[9] == "A" ? FixMode.Automatic : FixMode.Manual;
}
else
{
_fixMode = FixMode.Unknown;
}
#endregion Fix Mode
#region Fix Quality
// Do we have enough data for fix quality?
if (wordCount >= 10 && words[10].Length != 0)
{
switch (int.Parse(words[10], NmeaCultureInfo))
{
case 0:
_fixQuality = FixQuality.NoFix;
break;
case 1:
_fixQuality = FixQuality.GpsFix;
break;
case 2:
_fixQuality = FixQuality.DifferentialGpsFix;
break;
case 3:
_fixQuality = FixQuality.PulsePerSecond;
break;
case 4:
_fixQuality = FixQuality.FixedRealTimeKinematic;
break;
case 5:
_fixQuality = FixQuality.FloatRealTimeKinematic;
break;
case 6:
_fixQuality = FixQuality.Estimated;
break;
case 7:
_fixQuality = FixQuality.ManualInput;
break;
case 8:
_fixQuality = FixQuality.Simulated;
break;
default:
_fixQuality = FixQuality.Unknown;
break;
}
}
else
{
// This fix quality is invalid
_fixQuality = FixQuality.Unknown;
}
#endregion Fix Quality
#region Bearing
// Do we have enough data for fix quality?
if (wordCount >= 13 && words[12].Length != 0)
{
_bearing = new Azimuth(words[12], NmeaCultureInfo);
}
else
{
_bearing = Azimuth.Invalid;
}
#endregion Bearing
#region Speed
// Do we have enough data for fix quality?
if (wordCount >= 12 && words[11].Length != 0)
{
_speed = Speed.FromKilometersPerHour(double.Parse(words[11], NmeaCultureInfo));
}
else
{
_speed = Speed.Invalid;
}
#endregion Speed
#region Position Dilution of Precision
// Do we have enough data for fix quality?
if (wordCount >= 13 && words[13].Length != 0)
{
_positionDop = new DilutionOfPrecision(float.Parse(words[13], NmeaCultureInfo));
}
else
{
_positionDop = DilutionOfPrecision.Invalid;
}
#endregion Position Dilution of Precision
}
/// <summary>
/// Overrides OnSentanceChanged for the GPVTGSentence
/// </summary>
protected override void OnSentenceChanged()
{
// First, process the basic info for the sentence
base.OnSentenceChanged();
// Cache the sentence words
string[] words = base.Words;
int wordCount = words.Length;
/*
* $GPVTG
*
* Track Made Good and Ground Speed.
*
* eg1. $GPVTG,360.0,T,348.7,M,000.0,N,000.0,K*43
* eg2. $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K
*
*
* 054.7,T True track made good
* 034.4,M Magnetic track made good
* 005.5,N Ground speed, knots
* 010.2,K Ground speed, Kilometers per hour
*
*
* eg3. $GPVTG,t,T,,,s.ss,N,s.ss,K*hh
* 1 = Track made good
* 2 = Fixed text 'T' indicates that track made good is relative to true north
* 3 = not used
* 4 = not used
* 5 = Speed over ground in knots
* 6 = Fixed text 'N' indicates that speed over ground in in knots
* 7 = Speed over ground in kilometers/hour
* 8 = Fixed text 'K' indicates that speed over ground is in kilometers/hour
*
*
*
*/
#region Bearing
if (wordCount >= 1 && words[0].Length != 0)
{
_Bearing = Azimuth.Parse(words[0], NmeaCultureInfo);
}
else
{
_Bearing = Azimuth.Invalid;
}
#endregion
#region Magnetic Variation
if (wordCount >= 3 && words[2].Length != 0)
{
_MagneticVariation = new Longitude(double.Parse(words[2], NmeaCultureInfo) - _Bearing.DecimalDegrees);
}
else
{
_MagneticVariation = Longitude.Invalid;
}
#endregion
#region Speed
/* Speed is reported as both knots and KM/H. We can parse either of the values.
* First, try to parse knots. If that fails, parse KM/h.
*/
if (wordCount > 6 && words[5].Length != 0)
{
_Speed = new Speed(
// Parse the numeric portion
double.Parse(words[5], NmeaCultureInfo),
// Use knots
SpeedUnit.Knots);
}
else if (wordCount > 8 && words[7].Length != 0)
{
_Speed = new Speed(
// Parse the numeric portion
double.Parse(words[7], NmeaCultureInfo),
// Use knots
SpeedUnit.KilometersPerHour);
}
else
{
// Invalid speed
_Speed = Speed.Invalid;
}
#endregion
}
/// <summary>
/// Return a filtered Position3D from the specified parameters
/// </summary>
/// <param name="gpsPosition">The GPS position.</param>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="bearing">The bearing.</param>
/// <param name="speed">The speed.</param>
/// <returns></returns>
public override Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_currentState.UpdateState(deviceError, horizontalDOP, verticalDOP, bearing, speed, gpsPosition);
return _currentState.CorrectedLocation();
}
/// <summary> /// Return a filtered Position3D from the specified parameters /// </summary> /// <param name="gpsPosition">The GPS position.</param> /// <param name="deviceError">The device error.</param> /// <param name="horizontalDOP">The horizontal DOP.</param> /// <param name="verticalDOP">The vertical DOP.</param> /// <param name="bearing">The bearing.</param> /// <param name="speed">The speed.</param> /// <returns></returns> public abstract Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed);
/// <summary>
/// Returns the position
/// </summary>
/// <param name="gpsPosition">The gps Position</param>
/// <param name="currentDOP">The current dilution of precision</param>
/// <param name="bearing">the directional azimuth</param>
/// <param name="speed">the magnitude of the velocity</param>
/// <returns>A Position sturcture</returns>
public Position Filter(Position gpsPosition, DilutionOfPrecision currentDOP, Azimuth bearing, Speed speed)
{
return Filter(gpsPosition, _currentState.DeviceError, currentDOP, currentDOP, bearing, speed);
}
public void UpdateState(Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed, Position3D z)
{
if (this.x.IsInvalid)
{
Initialize(z);
return;
}
// More insanity
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Covariance values are invalid. Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
this._deviceError = deviceError.Value;
this._horizontalDOP = horizontalDOP.Value;
this._verticalDOP = verticalDOP.Value;
double hCovariance = this._deviceError * this._horizontalDOP;
double vCovariance = this._deviceError * this._verticalDOP;
// Setup the observation covariance (measurement error)
this.R = new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance);
#region Process Noise Estimation
// Get the translation of the last correction
CartesianPoint subX = this.x.ToPosition3D(_ellipsoid)
.TranslateTo(bearing, speed.ToDistance(_delay), _ellipsoid)
.ToCartesianPoint();
// Get the vector of the translation and the last observation
//CartesianPoint w = (subX - this.z);
CartesianPoint w =
new CartesianPoint(
Distance.FromMeters(subX.X.Value - this.z.X.Value), // Values are in meters
Distance.FromMeters(subX.Y.Value - this.z.Y.Value), // Values are in meters
Distance.FromMeters(subX.Z.Value - this.z.Z.Value)); // Values are in meters
// Setup the noise covariance (process error)
this.Q = new SquareMatrix3D(
Math.Abs(w.X.Value), 0, 0,
0, Math.Abs(w.Y.Value), 0,
0, 0, Math.Abs(w.Z.Value));
#endregion
// Update the observation state
this.z = z.ToCartesianPoint(_ellipsoid);
#region State vector prediction and covariance
//s.x = s.A*s.x + s.B*s.u;
//this.x = this.A * this.x + this.B * this.u;
CartesianPoint Ax = this.A.TransformVector(x);
CartesianPoint Bu = this.B.TransformVector(u);
this.x =
new CartesianPoint(
Distance.FromMeters(Ax.X.Value + Bu.X.Value),
Distance.FromMeters(Ax.Y.Value + Bu.Y.Value),
Distance.FromMeters(Ax.Z.Value + Bu.Z.Value));
//s.P = s.A * s.P * s.A' + s.Q;
this.P = this.A * this.P * SquareMatrix3D.Transpose(this.A) + this.Q;
#endregion
#region Kalman gain factor
//K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);
SquareMatrix3D Ht = SquareMatrix3D.Transpose(this.H);
SquareMatrix3D K = this.P * Ht * SquareMatrix3D.Invert(this.H * this.P * Ht + this.R);
#endregion
#region Observational correction
//s.x = s.x + K*(s.z-s.H*s.x);
//this.x = this.x + K * (this.z - this.H * this.x);
CartesianPoint Hx = this.H.TransformVector(x);
CartesianPoint zHx = new CartesianPoint(
Distance.FromMeters(this.z.X.Value - Hx.X.Value),
Distance.FromMeters(this.z.Y.Value - Hx.Y.Value),
Distance.FromMeters(this.z.Z.Value - Hx.Z.Value));
CartesianPoint kzHx = K.TransformVector(zHx);
this.x =
new CartesianPoint(
Distance.FromMeters(this.x.X.Value + kzHx.X.Value),
Distance.FromMeters(this.x.Y.Value + kzHx.Y.Value),
Distance.FromMeters(this.x.Z.Value + kzHx.Z.Value));
//s.P = s.P - K*s.H*s.P;
this.P = this.P - K * this.H * this.P;
#endregion
// Bump the state count
this._interval++;
// Calculate the average error for the system stste.
this._errorState = (this._errorState + Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2))) * .5f;
// Calculate the interval between samples
DateTime now = DateTime.Now;
this._delay = now.Subtract(_lastObservation);
this._lastObservation = now;
}
/// <summary>
/// Creates a GprmcSentence from the specified parameters
/// </summary>
/// <param name="utcDateTime"></param>
/// <param name="isFixAcquired"></param>
/// <param name="position"></param>
/// <param name="speed"></param>
/// <param name="bearing"></param>
/// <param name="magneticVariation"></param>
public GprmcSentence(DateTime utcDateTime, bool isFixAcquired, Position position, Speed speed, Azimuth bearing, Longitude magneticVariation)
{
// Use a string builder to create the sentence text
StringBuilder builder = new StringBuilder(128);
/* GPRMC sentences have the following format:
*
* $GPRMC,040302.663,A,3939.7,N,10506.6,W,0.27,358.86,200804,,*1A
*/
// Append the command word, $GPRMC
builder.Append("$GPRMC");
// Append a comma
builder.Append(',');
#region Append the UTC time
builder.Append(utcDateTime.Hour.ToString("0#", NmeaCultureInfo));
builder.Append(utcDateTime.Minute.ToString("0#", NmeaCultureInfo));
builder.Append(utcDateTime.Second.ToString("0#", NmeaCultureInfo));
builder.Append(".");
builder.Append(utcDateTime.Millisecond.ToString("00#", NmeaCultureInfo));
#endregion
// Append a comma
builder.Append(',');
#region Append the fix status
// Write fix information
if (isFixAcquired)
builder.Append("A");
else
builder.Append("V");
#endregion
// Append a comma
builder.Append(',');
#region Append the position
// Append latitude in the format HHMM.MMMM.
builder.Append(position.Latitude.ToString("HHMM.MMMM,I,", NmeaCultureInfo));
// Append Longitude in the format HHHMM.MMMM.
builder.Append(position.Longitude.ToString("HHHMM.MMMM,I,", NmeaCultureInfo));
#endregion
// Append the speed (in knots)
builder.Append(speed.ToKnots().ToString("v.v", NmeaCultureInfo));
// Append a comma
builder.Append(',');
// Append the bearing
builder.Append(bearing.ToString("d.d", NmeaCultureInfo));
// Append a comma
builder.Append(',');
#region Append the UTC date
// Append UTC date
builder.Append(utcDateTime.Day.ToString("0#", NmeaCultureInfo));
builder.Append(utcDateTime.Month.ToString("0#", NmeaCultureInfo));
// Append the year (year minus 2000)
int year = utcDateTime.Year - 2000;
builder.Append(year.ToString("0#", NmeaCultureInfo));
#endregion
// Append a comma
builder.Append(',');
// Append magnetic variation
builder.Append(magneticVariation.ToString("d.d", NmeaCultureInfo));
// Set this object's sentence
SetSentence(builder.ToString());
// Finally, append the checksum
AppendChecksum();
}
public Position3D Filter(Position3D gpsPosition, DilutionOfPrecision currentDOP, Azimuth bearing, Speed speed)
{
return(Filter(gpsPosition, _currentState.DeviceError, currentDOP, currentDOP, bearing, Speed.AtRest));
}
/// <summary>
/// Adds a new observation and applies the filter.
/// </summary>
/// <param name="gpsPosition">The new observation to add to the filter.</param>
/// <param name="deviceError">A DeviceError, which does not currently affect position averaging.</param>
/// <param name="horizontalDOP">A horizontal dilution of position, which does not currently affect position averaging.</param>
/// <param name="verticalDOP">A vertical dilution of positoin which does not currently affect position averaging.</param>
/// <param name="bearing">A directional bearing, which does not currently affect position averaging.</param>
/// <param name="speed">A speed, which does not currently affect position averaging.</param>
/// <returns></returns>
/// <remarks>This method updates the FilteredLocation property without consideration for SampleCount.</remarks>
public override Position Filter(Position gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
Position3D pos3D = Filter(new Position3D(gpsPosition), deviceError, horizontalDOP, verticalDOP, bearing, speed);
return(new Position(pos3D.Latitude, pos3D.Longitude));
}
public override Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_currentState.UpdateState(deviceError, horizontalDOP, verticalDOP, bearing, speed, gpsPosition);
return(_currentState.CorrectedLocation());
}
/// <summary>
/// Adds a new observation and applies the filter.
/// </summary>
/// <param name="gpsPosition"> The new observation to add to the filter. </param>
/// <param name="deviceError"> Does not currently affect position averaging. </param>
/// <param name="horizontalDOP"> Does not currently affect position averaging. </param>
/// <param name="verticalDOP"> Does not currently affect position averaging. </param>
/// <param name="bearing"> Does not currently affect position averaging. </param>
/// <param name="speed"> Does not currently affect position averaging. </param>
/// <remarks>
/// This method updates the FilteredLocation property without consideration for SampleCount.
/// </remarks>
public override Position Filter(Position gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
Position3D pos3d = Filter(new Position3D(gpsPosition), deviceError, horizontalDOP, verticalDOP, bearing, speed);
return new Position(pos3d.Latitude, pos3d.Longitude);
}
/// <summary>
/// Creates a new instance using the specified radius, angle, origin and winding direction.
/// </summary>
/// <param name="r">A <strong>Double</strong> indicating a radius. Increasing values represent a distance further away from the center of a circle.</param>
/// <param name="theta">An <strong>Angle</strong> representing a direction from the center of a circle.</param>
/// <param name="origin">An <strong>Azimuth</strong> indicating which compass direction is associated with zero degrees. (Typically North.)</param>
/// <param name="orientation">A <strong>PolarCoordinateOrientation</strong> value indicating whether increasing Theta values wind clockwise or counter-clockwise.</param>
/// <remarks>The radius "r," when combined with an angle "theta" will create a coordinate relative to
/// the center of a circle. The BearingOrigin will associate a compass direction with zero degrees (0°), but this value is typically "North".</remarks>
public PolarCoordinate(float r, Angle theta, Azimuth origin, PolarCoordinateOrientation orientation)
: this(r, theta.DecimalDegrees, origin, orientation)
{ }
/// <summary>Occurs when the compass direction associated with 0° has changed.</summary>
protected virtual void OnOriginChanged(Azimuth origin)
{
if (OriginChanged != null)
OriginChanged(this, new AzimuthEventArgs(origin));
}
public AzimuthSubtractionTestingStruct( Double Azimuth1
, Double Azimuth2
, Double ExpectedInternal
, Double ExpectedExternal
)
{
this.Az1 = Azimuth.NewFromDoubleAsDegrees(Azimuth1);
this.Az2 = Azimuth.NewFromDoubleAsDegrees(Azimuth2);
this.ExpectedInternal = ExpectedInternal;
this.ExpectedExternal = ExpectedExternal;
}
protected virtual void OnEmulation()
{
/* The emulator, on its own, will update real-time simulated values such
* as the position, bearing, altitude, and speed.
*/
// How much time has passed since the last emulation?
TimeSpan elapsedTime = DateTime.UtcNow.Subtract(_UtcDateTime);
// Set the new update date
_UtcDateTime = DateTime.UtcNow;
// How much have we moved since the last update?
Distance distanceMoved = _Speed.ToDistance(elapsedTime);
// Do we have a route?
if (_Route.Count == 0)
{
// Randomize the speed and direction if required.
if (_isRandom) Randomize();
// No. Just move in a straight line
_CurrentPosition = _CurrentPosition.TranslateTo(_Bearing, distanceMoved);
}
else
{
// How close are we to the next route point?
Distance toWaypoint = _CurrentPosition.DistanceTo(_CurrentDestination);
// Is the distance to the waypoint small?
if (toWaypoint.ToMeters().Value < _Speed.ToDistance(Interval).ToMeters().Value)
{
// Close enough to "snap" to the waypoint
_CurrentPosition = _CurrentDestination;
// Select the next position in the route as our destination.
_RouteIndex++;
if (_RouteIndex > _Route.Count - 1)
_RouteIndex = 0;
// Set the next destination point
_CurrentDestination = _Route[_RouteIndex];
}
else
{
_CurrentPosition = _CurrentPosition.TranslateTo(_Bearing, distanceMoved);
}
// Get the bearing to the next destination
_Bearing = _CurrentPosition.BearingTo(_CurrentDestination);
}
}
/// <summary> /// Return a filtered Position3D from the specified parameters /// </summary> /// <param name="gpsPosition">The GPS position.</param> /// <param name="deviceError">The device error.</param> /// <param name="horizontalDOP">The horizontal DOP.</param> /// <param name="verticalDOP">The vertical DOP.</param> /// <param name="bearing">The bearing.</param> /// <param name="speed">The speed.</param> /// <returns></returns> public abstract Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed);
/// <summary>
/// Randomizes the emulation by changing speed and direction
/// </summary>
/// <param name="seed"> The randomizer to use. </param>
/// <param name="speedLow"> The minimum travel speed. </param>
/// <param name="speedHigh"> The maximum travel speed. </param>
/// <param name="bearingStart"> The initial direction of travel. </param>
/// <param name="bearingArc"> The arc in which random directional changes will occur. </param>
/// <remarks>
/// GPS coordinate emulation can be randomized by any number of factors, depending on the emulator type used.
/// Any emulation can have it's direction and speed randomized within specified tolerances. By default, speed is
/// limited to between 0 (low) and 5 (high) meters/second and bearing changes are limited to +/- 45 degrees
/// (a 90 degree arc) from North (0) degrees.
/// </remarks>
public void Randomize(Random seed, Speed speedLow, Speed speedHigh, Azimuth bearingStart, Azimuth bearingArc)
{
// Flag it so the emulation will use random values
_isRandom = true;
// Get the randomizer parameters
_seed = seed;
// Get the speed variance
_speedLow = speedLow.ToMetersPerSecond().Value;
_speedHigh = speedHigh.ToMetersPerSecond().Value;
// Get the normalized arc values
_bearingStart = bearingStart.Normalize().DecimalDegrees;
_bearingArc = bearingArc.Normalize().DecimalDegrees;
}
/// <inheritdocs/>
protected override void OnPaintOffScreen(PaintEventArgs e)
{
PolarGraphics f = CreatePolarGraphics(e.Graphics);
// What bearing are we drawing?
#if PocketPC
Azimuth BearingToRender = _Bearing;
#else
Azimuth bearingToRender = new Azimuth(_valueInterpolator[_interpolationIndex]);
#endif
// Cache drawing options in order to prevent race conditions during
// drawing!
double minorInterval = _minorTickInterval.DecimalDegrees;
double majorInterval = _majorTickInterval.DecimalDegrees;
double directionInterval = _directionLabelInterval.DecimalDegrees;
double angleInterval = _angleLabelInterval.DecimalDegrees;
// Draw tick marks
if (minorInterval > 0)
{
for (double angle = 0; angle < 360; angle += minorInterval)
{
// And draw a line
f.DrawLine(_minorTickPen, new PolarCoordinate(98, angle), new PolarCoordinate(100, angle));
}
}
// Draw tick marks
if (majorInterval > 0)
{
for (double angle = 0; angle < 360; angle += majorInterval)
{
// And draw a line
f.DrawLine(_majorTickPen, new PolarCoordinate(95, angle), new PolarCoordinate(100, angle));
}
}
if (directionInterval > 0)
{
for (double angle = 0; angle < 360; angle += directionInterval)
{
// And draw a line
f.DrawLine(_directionTickPen, new PolarCoordinate(92, angle), new PolarCoordinate(100, angle));
}
}
if (angleInterval > 0)
{
for (double angle = 0; angle < 360; angle += angleInterval)
{
// Get the coordinate of the line's start
PolarCoordinate start = new PolarCoordinate(60, angle, Azimuth.North, PolarCoordinateOrientation.Clockwise);
#if PocketPC
f.DrawCenteredString(((Angle)angle).ToString(_AngleLabelFormat, CultureInfo.CurrentCulture), _AngleLabelFont, _AngleLabelBrush, start);
#else
f.DrawRotatedString(((Angle)angle).ToString(_angleLabelFormat, CultureInfo.CurrentCulture), _angleLabelFont, _angleLabelBrush, start);
#endif
}
}
if (directionInterval > 0)
{
for (double angle = 0; angle < 360; angle += directionInterval)
{
// Get the coordinate of the line's start
PolarCoordinate start = new PolarCoordinate(80, angle, Azimuth.North, PolarCoordinateOrientation.Clockwise);
#if PocketPC
f.DrawCenteredString(((Azimuth)angle).ToString("c", CultureInfo.CurrentCulture), _DirectionLabelFont, _DirectionLabelBrush, start);
#else
f.DrawRotatedString(((Azimuth)angle).ToString("c", CultureInfo.CurrentCulture), _directionLabelFont, _directionLabelBrush, start);
#endif
}
}
// Draw an ellipse at the center
f.DrawEllipse(_centerPen, PolarCoordinate.Empty, 10);
// Now draw the needle shadow
PolarCoordinate[] needleNorth = _needlePointsNorth.Clone() as PolarCoordinate[];
PolarCoordinate[] needleSouth = _needlePointsSouth.Clone() as PolarCoordinate[];
// Adjust the needle to the current bearing
if (needleNorth != null)
for (int index = 0; index < needleNorth.Length; index++)
{
needleNorth[index] = needleNorth[index].Rotate(bearingToRender.DecimalDegrees);
if (needleSouth != null) needleSouth[index] = needleSouth[index].Rotate(bearingToRender.DecimalDegrees);
}
#if !PocketPC
// Now draw a shadow
f.Graphics.TranslateTransform(_pNeedleShadowSize.Width, _pNeedleShadowSize.Height, MatrixOrder.Append);
f.FillPolygon(_pNeedleShadowBrush, needleNorth);
f.FillPolygon(_pNeedleShadowBrush, needleSouth);
f.Graphics.ResetTransform();
#endif
f.FillPolygon(_pNorthNeedleBrush, needleNorth);
f.DrawPolygon(_pNorthNeedlePen, needleNorth);
f.FillPolygon(_pSouthNeedleBrush, needleSouth);
f.DrawPolygon(_pSouthNeedlePen, needleSouth);
}
