dotMorten/NmeaParser
1
0
Fork 0
mirror of https://github.com/dotMorten/NmeaParser.git synced 2026-02-02 22:04:16 +01:00
Code Issues Actions Packages Projects Releases Wiki Activity

Added a simple deviation map plot

Browse source
This commit is contained in:
Morten Nielsen 2020-07-30 19:57:44 -07:00
parent 41ddca8d0b
commit 1331ad8fcf
4 changed files with 276 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
src/SampleApp.WinDesktop/MainWindow.xaml
View file

@ -45,6 +45,9 @@
<TabItem Header="GNSS Monitor">
<local:GnssMonitorView x:Name="gnssMonitorView" />
</TabItem>
<TabItem Header="Deviation Map">
<local:PointPlotView x:Name="mapplot" />
</TabItem>
<TabItem Header="Map">
<local:View2D x:Name="view2d" />
</TabItem>

16
src/SampleApp.WinDesktop/MainWindow.xaml.cs
View file

@ -55,7 +55,12 @@ namespace SampleApp.WinDesktop
if (currentDevice.IsOpen)
await currentDevice.CloseAsync();
currentDevice.Dispose();
gnssMonitorView.Monitor = null;
if (gnssMonitorView.Monitor != null)
{
gnssMonitorView.Monitor.LocationChanged -= Monitor_LocationChanged;
gnssMonitorView.Monitor = null;
}
mapplot.Clear();
}
output.Text = "";
messages.Clear();
@ -85,7 +90,14 @@ namespace SampleApp.WinDesktop
((NmeaParser.SerialPortDevice)device).Port.BaudRate);
}
await device.OpenAsync();
gnssMonitorView.Monitor = new GnssMonitor(device);
gnssMonitorView.Monitor = new GnssMonitor(device);
gnssMonitorView.Monitor.LocationChanged += Monitor_LocationChanged;
}
private void Monitor_LocationChanged(object sender, EventArgs e)
{
var mon = sender as GnssMonitor;
mapplot.AddLocation(mon.Latitude, mon.Longitude, mon.Altitude);
}
private void device_MessageReceived(object sender, NmeaParser.NmeaMessageReceivedEventArgs args)

58
src/SampleApp.WinDesktop/PointPlotView.xaml Normal file
View file

@ -0,0 +1,58 @@
<UserControl x:Class="SampleApp.WinDesktop.PointPlotView"
xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"
xmlns:d="http://schemas.microsoft.com/expression/blend/2008"
xmlns:local="clr-namespace:SampleApp.WinDesktop"
mc:Ignorable="d"
d:DesignHeight="450" d:DesignWidth="800">
<Grid SizeChanged="Grid_SizeChanged" Background="Black" >
<Grid Margin="10" HorizontalAlignment="Stretch" VerticalAlignment="Stretch" x:Name="PlotMap" SizeChanged="PlotMap_SizeChanged">
<Grid.ColumnDefinitions>
<ColumnDefinition Width="1*" />
<ColumnDefinition Width="1*" />
<ColumnDefinition Width="1*" />
<ColumnDefinition Width="1*" />
<ColumnDefinition Width="1*" />
<ColumnDefinition Width="1*" />
<ColumnDefinition Width="1*" />
<ColumnDefinition Width="1*" />
<ColumnDefinition Width="1*" />
<ColumnDefinition Width="1*" />
</Grid.ColumnDefinitions>
<Grid.RowDefinitions>
<RowDefinition Height="1*" />
<RowDefinition Height="1*" />
<RowDefinition Height="1*" />
<RowDefinition Height="1*" />
<RowDefinition Height="1*" />
<RowDefinition Height="1*" />
<RowDefinition Height="1*" />
<RowDefinition Height="1*" />
<RowDefinition Height="1*" />
<RowDefinition Height="1*" />
</Grid.RowDefinitions>
<Ellipse HorizontalAlignment="Stretch" VerticalAlignment="Stretch" Stroke="White" StrokeThickness="1" Grid.ColumnSpan="8" Grid.RowSpan="8" Grid.Row="1" Grid.Column="1" StrokeDashArray="2 2" x:Name="OuterRing" />
<Ellipse HorizontalAlignment="Stretch" VerticalAlignment="Stretch" Stroke="White" StrokeThickness="1" Grid.ColumnSpan="6" Grid.RowSpan="6" Grid.Row="2" Grid.Column="2" StrokeDashArray="2 2" />
<Ellipse HorizontalAlignment="Stretch" VerticalAlignment="Stretch" Stroke="White" StrokeThickness="1" Grid.ColumnSpan="4" Grid.RowSpan="4" Grid.Row="3" Grid.Column="3" StrokeDashArray="2 2" />
<Ellipse HorizontalAlignment="Stretch" VerticalAlignment="Stretch" Stroke="White" StrokeThickness="1" Grid.ColumnSpan="2" Grid.RowSpan="2" Grid.Row="4" Grid.Column="4" StrokeDashArray="2 2" />
<Path Data="M0,0 L1,0" HorizontalAlignment="Stretch" VerticalAlignment="Center" Stroke="White" StrokeThickness="1" Grid.RowSpan="8" Grid.ColumnSpan="8" Grid.Row="1" Grid.Column="1" StrokeDashArray="2 2" Stretch="Fill" />
<Path Data="M0,0 L0,1" HorizontalAlignment="Center" VerticalAlignment="Stretch" Stroke="White" StrokeThickness="1" Grid.RowSpan="8" Grid.ColumnSpan="8" Grid.Row="1" Grid.Column="1" StrokeDashArray="2 2" Stretch="Fill" />
<TextBlock x:Name="FirstMeterLabel" Text="5m" Margin="0,15,0,0" FontSize="12" HorizontalAlignment="Center" VerticalAlignment="Center" Foreground="White" Grid.RowSpan="10" Grid.Column="6" Grid.ColumnSpan="2" Background="#55000000" />
<TextBlock x:Name="SecondMeterLabel" Text="10m" Margin="0,15,0,0" FontSize="12" HorizontalAlignment="Center" VerticalAlignment="Center" Foreground="White" Grid.RowSpan="10" Grid.Column="8" Grid.ColumnSpan="2" Background="#55000000" />
<TextBlock Text="N" Margin="10" FontSize="9" HorizontalAlignment="Center" VerticalAlignment="Bottom" Foreground="White" Grid.ColumnSpan="10" />
<TextBlock Text="S" Margin="10" FontSize="9" HorizontalAlignment="Center" VerticalAlignment="Top" Foreground="White" Grid.ColumnSpan="10" Grid.Row="9" />
<TextBlock Text="W" Margin="10" FontSize="9" HorizontalAlignment="Right" VerticalAlignment="Center" Foreground="White" Grid.RowSpan="10" Grid.Column="0" />
<TextBlock Text="E" Margin="10" FontSize="9" HorizontalAlignment="Left" VerticalAlignment="Center" Foreground="White" Grid.RowSpan="10" Grid.Column="9" />
<Canvas Grid.RowSpan="10" Grid.ColumnSpan="10" x:Name="canvas">
<Ellipse Fill="LightGreen" Canvas.Left="10" Width="3" Height="3" Canvas.Top="20" />
</Canvas>
</Grid>
<StackPanel HorizontalAlignment="Left" VerticalAlignment="Bottom" Margin="20">
<Button Content="Clear" FontSize="10" Padding="2" HorizontalAlignment="Stretch" Click="ClearButton_Click"/>
</StackPanel>
<StackPanel HorizontalAlignment="Right" VerticalAlignment="Top" Margin="20">
<TextBlock x:Name="Status" Foreground="White" FontSize="12" />
</StackPanel>
</Grid>
</UserControl>

201
src/SampleApp.WinDesktop/PointPlotView.xaml.cs Normal file
View file

@ -0,0 +1,201 @@
using Esri.ArcGISRuntime.Location;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Transactions;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
namespace SampleApp.WinDesktop
{
/// <summary>
/// Interaction logic for PointPlotView.xaml
/// </summary>
public partial class PointPlotView : UserControl
{
List<double[]> locations = new List<double[]>();
public PointPlotView()
{
InitializeComponent();
}
private void Grid_SizeChanged(object sender, SizeChangedEventArgs e)
{
PlotMap.Width = PlotMap.Height = Math.Min(e.NewSize.Width, e.NewSize.Height);
}
private void PlotMap_SizeChanged(object sender, SizeChangedEventArgs e)
{
UpdatePlot();
}
public void Clear()
{
locations.Clear();
UpdatePlot();
}
public void AddLocation(double latitude, double longitude, double altitude)
{
Dispatcher.Invoke(() =>
{
locations.Add(new double[] { latitude, longitude, altitude });
UpdatePlot();
});
}
static SolidColorBrush dotFill = new SolidColorBrush(Colors.LightGreen);
private void UpdatePlot()
{
if (canvas.ActualWidth == 0 || canvas.ActualHeight == 0 || !IsVisible)
return;
Status.Text = "";
canvas.Children.Clear();
if (locations.Count == 0)
return;
var latAvr = locations.Select(l => l[0]).Average();
var lonAvr = locations.Select(l => l[1]).Average();
//List<double> lonDistances = new List<double>(locations.Count);
//List<double> latDistances = new List<double>(locations.Count);
List<double> distances = new List<double>(locations.Count);
var locations2 = new List<double[]>();
foreach (var l in locations)
{
var d = Vincenty.GetDistanceVincenty(latAvr, lonAvr, l[0], l[1]);
var dLat = Vincenty.GetDistanceVincenty(latAvr, lonAvr, l[0], lonAvr);
var dLon = Vincenty.GetDistanceVincenty(latAvr, lonAvr, latAvr, l[1]);
distances.Add(d);
//latDistances.Add(dLat);
//lonDistances.Add(dLon);
if (latAvr > l[0]) dLat = -dLat;
if (lonAvr > l[1]) dLon = -dLon;
locations2.Add(new double[] { dLat, dLon });
}
var latMin = locations2.Select(l => l[0]).Min();
var lonMin = locations2.Select(l => l[1]).Min();
var latMax = locations2.Select(l => l[0]).Max();
var lonMax = locations2.Select(l => l[1]).Max();
var latAvr2 = locations2.Select(l => l[0]).Average();
var lonAvr2 = locations2.Select(l => l[1]).Average();
var maxDifLat = Math.Max(latAvr2 - latMin, latMax - latAvr2);
var maxDifLon = Math.Max(lonAvr2 - lonMin, lonMax - lonAvr2);
var maxDif = Math.Max(maxDifLat, maxDifLon);
if (maxDif < .1)
maxDif = .1;
if (maxDif < .25)
maxDif = .25;
else if (maxDif < .5)
maxDif = .5;
else
maxDif = Math.Ceiling(maxDif);
//if(maxDif < 1)
SecondMeterLabel.Text = $"{maxDif}m";
FirstMeterLabel.Text = $"{maxDif/2}m";
double scale = maxDif / Math.Min(OuterRing.ActualWidth, OuterRing.ActualHeight) / .5;
if (scale == 0)
scale = 1;
for (int i = Math.Max(0, locations2.Count - 1000); i < locations2.Count; i++) // Only draw the last 1000 points
{
var l = locations2[i];
var x = canvas.ActualWidth * .5 + (l[1] - lonAvr2) / scale;
var y = canvas.ActualHeight * .5 - (l[0] - latAvr2) / scale;
Ellipse e = new Ellipse() { Width = 3, Height = 3, Fill = dotFill };
if (canvas.Children.Count == locations2.Count - 1)
{
e.Fill = new SolidColorBrush(Colors.Red);
e.Width = 5;
e.Height = 5;
}
Canvas.SetLeft(e, x - e.Width * .5);
Canvas.SetTop(e, y - e.Height * .5);
canvas.Children.Add(e);
}
var stdDevLat = Math.Sqrt(locations2.Sum(d => (d[0] - latAvr2) * (d[0] - latAvr2)) / locations2.Count);
var stdDevLon = Math.Sqrt(locations2.Sum(d => (d[1] - lonAvr2) * (d[1] - lonAvr2)) / locations2.Count);
var zAvr = locations.Select(l => l[2]).Where(l => !double.IsNaN(l)).Average();
var stdDevZ = Math.Sqrt(locations.Select(l => l[2]).Where(l => !double.IsNaN(l)).Sum(d => (d - zAvr) * (d - zAvr)) / locations.Select(l => l[2]).Where(l => !double.IsNaN(l)).Count());
Status.Text = $"Average:\n - Latitude: {latAvr.ToString("0.0000000")}\n - Longitude: {lonAvr.ToString("0.0000000")}\n - Elevation: {zAvr.ToString("0.000")}m\nStandard Deviation:\n - Latitude: {stdDevLat.ToString("0.###")}m\n - Longitude: {stdDevLon.ToString("0.###")}m\n - Horizontal: {distances.Average().ToString("0.###")}m\n - Elevation: {stdDevZ.ToString("0.###")}m";
}
internal static class Vincenty
{
private const double D2R = 0.01745329251994329576923690768489; //Degrees to radians
public static double GetDistanceVincenty(double lat1, double lon1, double lat2, double lon2)
{
return GetDistanceVincenty(lat1, lon1, lat2, lon2, 6378137, 6356752.31424518); //Uses WGS84 values
}
/// <summary>
/// Vincenty's formulae is used in geodesy to calculate the distance
/// between two points on the surface of a spheroid.
/// </summary>
public static double GetDistanceVincenty(double lat1, double lon1, double lat2, double lon2, double semiMajor, double semiMinor)
{
var a = semiMajor;
var b = semiMinor;
var f = (a - b) / a; //flattening
var L = (lon2 - lon1) * D2R;
var U1 = Math.Atan((1 - f) * Math.Tan(lat1 * D2R));
var U2 = Math.Atan((1 - f) * Math.Tan(lat2 * D2R));
var sinU1 = Math.Sin(U1);
var cosU1 = Math.Cos(U1);
var sinU2 = Math.Sin(U2);
var cosU2 = Math.Cos(U2);
double lambda = L;
double lambdaP;
double cosSigma, cosSqAlpha, sinSigma, cos2SigmaM, sigma, sinLambda, cosLambda;
int iterLimit = 100;
do
{
sinLambda = Math.Sin(lambda);
cosLambda = Math.Cos(lambda);
sinSigma = Math.Sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) +
(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
if (sinSigma == 0)
return 0; // co-incident points
cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
sigma = Math.Atan2(sinSigma, cosSigma);
double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
cosSqAlpha = 1 - sinAlpha * sinAlpha;
cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
if (double.IsNaN(cos2SigmaM))
cos2SigmaM = 0; // equatorial line: cosSqAlpha=0 (§6)
double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
lambdaP = lambda;
lambda = L + (1 - C) * f * sinAlpha *
(sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
} while (Math.Abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);
if (iterLimit == 0) return double.NaN; // formula failed to converge
var uSq = cosSqAlpha * (a * a - b * b) / (b * b);
var A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
var B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
var deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
var s = b * A * (sigma - deltaSigma);
s = Math.Round(s, 3); // round to 1mm precision
return s;
}
}
private void ClearButton_Click(object sender, RoutedEventArgs e)
{
Clear();
}
}
}