298 lines
13 KiB
C#
298 lines
13 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.IO.Ports;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
using KollNet.Lib.EventArguments;
|
|
using KollNet.Lib.Models;
|
|
|
|
namespace KollNet.Lib {
|
|
public static class GpsClient {
|
|
private static SerialPort gpsPort;
|
|
private static Timer timer1;
|
|
private static string carryOver;
|
|
private static bool movingCapable = false;
|
|
private static int roundInt = 6;
|
|
private static bool gpsOffsetSet = false;
|
|
|
|
#region "Public Events"
|
|
internal static void OnSatellitesUpdated(SatellitesUpdatedEventArgs e) { SatellitesUpdated?.Invoke(null, e); }
|
|
public static event EventHandler<SatellitesUpdatedEventArgs> SatellitesUpdated;
|
|
internal static void OnPositionChanged(GpsCoordinatesChangedEventArgs e) {
|
|
PositionChanged?.Invoke(null, e);
|
|
GeoPositionChanged?.Invoke(null, new GeoCoordinatesChangedEventArgs(e.Coordinates));
|
|
}
|
|
public static event EventHandler<GpsCoordinatesChangedEventArgs> PositionChanged;
|
|
public static event EventHandler<GeoCoordinatesChangedEventArgs> GeoPositionChanged;
|
|
internal static void OnConnected() { Connected?.Invoke(null, EventArgs.Empty); }
|
|
public static event EventHandler Connected;
|
|
internal static void OnDisconnected() { Disconnected?.Invoke(null, EventArgs.Empty); }
|
|
public static event EventHandler Disconnected;
|
|
internal static void OnNmeaReceived(NmeaReceivedEventArgs e) {
|
|
Console.WriteLine("NMEA: " + e.NmeaSentence);
|
|
LastPositionTime = DateTime.Now;
|
|
NmeaReceived?.Invoke(null, e);
|
|
}
|
|
public static event EventHandler<NmeaReceivedEventArgs> NmeaReceived;
|
|
#endregion
|
|
|
|
#region "Public Properties"
|
|
public static bool SatelliteStats { get; set; } = true;
|
|
public static bool IsConnected { get; private set; }
|
|
public static DateTime LastPositionTime { get; private set; }
|
|
public static TimeSpan GpsTimeOffset { get; private set; }
|
|
public static string ComPort {
|
|
get {
|
|
return gpsPort?.PortName ?? "";
|
|
} set {
|
|
if (gpsPort?.IsOpen ?? false) {
|
|
throw new Exception("GPS is currently connected and port assignment cannot be changed");
|
|
} else {
|
|
InitGps(value);
|
|
}
|
|
}
|
|
}
|
|
public static int BaudRate {
|
|
get { return gpsPort?.BaudRate ?? 9600; }
|
|
set { if (gpsPort != null) gpsPort.BaudRate = value; }
|
|
}
|
|
#endregion
|
|
|
|
|
|
#region "GPS Connect / Disconnect Methods"
|
|
public static string FindGps(bool autoConfig=true) {
|
|
int[] bauds = { 9600, 4800 };
|
|
|
|
foreach(string comPort in SerialPort.GetPortNames()) {
|
|
foreach(int baud in bauds) {
|
|
if (IsGpsPort(comPort, baud)) {
|
|
if (autoConfig) { ComPort = ComPort; BaudRate = baud; }
|
|
return comPort + ":" + baud;
|
|
}
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
public static bool Connect() {
|
|
if (gpsPort == null) { throw new Exception("GPS Port was not set prior to calling Connect() with no arguments"); }
|
|
|
|
InitGps(ComPort, BaudRate);
|
|
gpsPort.ReadTimeout = 10000;
|
|
gpsPort.Open();
|
|
if (gpsPort.IsOpen) {
|
|
IsConnected = true;
|
|
OnConnected();
|
|
timer1 = new Timer(timer_Elapsed, null, 1000, 1000);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
public static bool Connect(string portName, int baudRate=9600) {
|
|
ComPort = portName;
|
|
BaudRate = baudRate;
|
|
return Connect();
|
|
}
|
|
|
|
public static void Close() {
|
|
if (gpsPort == null) { return; }
|
|
if (gpsPort.IsOpen) { gpsPort.Close(); }
|
|
timer1?.Dispose();
|
|
timer1 = null;
|
|
carryOver = null;
|
|
IsConnected = false;
|
|
movingCapable = false;
|
|
gpsPort.Dispose();
|
|
gpsPort = null;
|
|
|
|
OnDisconnected();
|
|
}
|
|
|
|
public static void Disconnect() {
|
|
Close();
|
|
}
|
|
#endregion
|
|
|
|
#region "Helper Methods"
|
|
private static double DmsToDd2(string val, string nwse) {
|
|
nwse = nwse.ToUpper();
|
|
int chars = val.Substring(0, 1) == "0" ? 3 : 2;
|
|
//int chars = 2;
|
|
uint deg = Convert.ToUInt32(val.Substring(0, chars));
|
|
double min = Convert.ToDouble(val.Substring(chars));
|
|
|
|
return Math.Round(Convert.ToDouble((nwse == "S" || nwse == "W" ? -1 : 1) * (deg + (min * 60) / 3600)), roundInt);
|
|
}
|
|
|
|
private static DateTime? GetTimestamp(string date, string time) {
|
|
try {
|
|
if (date==null) { date = DateTime.UtcNow.ToString("ddMMyy"); }
|
|
DateTime dt = new DateTime(Convert.ToInt32("20" + date.Substring(4, 2)),
|
|
Convert.ToInt32(date.Substring(2, 2)),
|
|
Convert.ToInt32(date.Substring(0, 2)),
|
|
Convert.ToInt32(time.Substring(0, 2)),
|
|
Convert.ToInt32(time.Substring(2, 2)),
|
|
Convert.ToInt32(time.Substring(4, 2)),
|
|
DateTimeKind.Utc);
|
|
return dt;
|
|
} catch (Exception ex) { return null; }
|
|
}
|
|
|
|
private static void SetTimeOffset(string date, string time) {
|
|
if (gpsOffsetSet) { return; }
|
|
DateTime dt = new DateTime(Convert.ToInt32("20" + date.Substring(4, 2)),
|
|
Convert.ToInt32(date.Substring(2, 2)),
|
|
Convert.ToInt32(date.Substring(0, 2)),
|
|
Convert.ToInt32(time.Substring(0, 2)),
|
|
Convert.ToInt32(time.Substring(2, 2)),
|
|
Convert.ToInt32(time.Substring(4, 2)),
|
|
DateTimeKind.Utc);
|
|
GpsTimeOffset = DateTime.UtcNow.Subtract(dt);
|
|
gpsOffsetSet = true;
|
|
}
|
|
|
|
private static SatelliteView SvFromWords(string[] words) {
|
|
try {
|
|
SatelliteView sv = new SatelliteView() {
|
|
PRN = GetIntOrZero(words[0]),
|
|
Elevation = GetIntOrZero(words[1]),
|
|
Azimuth = GetIntOrZero(words[2]),
|
|
SNR = GetIntOrZero(words[3])
|
|
};
|
|
|
|
return sv;
|
|
}catch (Exception ex) {
|
|
Console.WriteLine("Unable to create SatelliteView: " + ex.Message);
|
|
return null;
|
|
}
|
|
}
|
|
|
|
private static int GetIntOrZero(string val) {
|
|
return string.IsNullOrEmpty(val) ? -1 : Convert.ToInt32(val);
|
|
}
|
|
|
|
private static bool IsGpsPort(string comPort, int baudRate) {
|
|
Console.Write("Attempting to locate GPS on " + comPort + ":" + baudRate + "...");
|
|
InitGps(comPort, baudRate);
|
|
gpsPort.ReadTimeout = 3000;
|
|
|
|
try {
|
|
gpsPort.Open();
|
|
Console.Write(" Connected. Listening for NMEA Sentences...");
|
|
} catch (Exception ex) {
|
|
Console.WriteLine(" Error: " + ex.Message);
|
|
return false;
|
|
}
|
|
|
|
string sData = "";
|
|
while (true) {
|
|
string s;
|
|
try {
|
|
s = gpsPort.ReadLine();
|
|
} catch (Exception ex) {
|
|
gpsPort.Close();
|
|
Console.WriteLine(" Error: " + ex.Message);
|
|
return false;
|
|
}
|
|
|
|
if (string.IsNullOrEmpty(s)) { return false; }
|
|
sData += s + Environment.NewLine;
|
|
|
|
if (sData.Length > 1500) {
|
|
|
|
if (sData.Contains("$GPRMC") || sData.Contains("$GPGGA") || sData.Contains("$GPGSA")) {
|
|
Console.WriteLine(" This is a GPS device!");
|
|
gpsPort.Close();
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endregion
|
|
|
|
public static void InitGps(string portName, int baud=9600) {
|
|
gpsPort = new SerialPort(portName, baud);
|
|
}
|
|
|
|
private static void timer_Elapsed(object state) {
|
|
try {
|
|
if (gpsPort != null && gpsPort.IsOpen) {
|
|
string data = gpsPort.ReadExisting();
|
|
string[] sentences = (carryOver + data).Split(new string[] { Environment.NewLine }, StringSplitOptions.None);
|
|
int lastIndex = data.LastIndexOf(Environment.NewLine);
|
|
if (lastIndex < data.Length) { carryOver = data.Substring(lastIndex); }
|
|
|
|
if (sentences.Count() > 1) {
|
|
foreach (string sentence in sentences) {
|
|
if (sentence.Trim() == "") { continue; }
|
|
|
|
//Console.WriteLine("::" + sentence);
|
|
if (sentence.Substring(0,1) != "$") { continue; }
|
|
try {
|
|
string[] words = sentence.Split(',');
|
|
string cmd = words[0].ToUpper();
|
|
|
|
if (cmd == "$GPGGA" && words.Count() >= 14) {
|
|
OnNmeaReceived(new NmeaReceivedEventArgs(sentence));
|
|
if (movingCapable) { continue; }
|
|
// Only capture this data if we haven't received a $GPRMC sentence
|
|
GpsCoordinates gc = new GpsCoordinates() {
|
|
Latitude = DmsToDd2(words[2], words[3]),
|
|
Longitude = DmsToDd2(words[4], words[5]),
|
|
Altitude = Convert.ToDouble(words[9]),
|
|
Timestamp = GetTimestamp(null, words[1])
|
|
};
|
|
OnPositionChanged(new GpsCoordinatesChangedEventArgs(gc));
|
|
} else if (cmd == "$GPRMC" && words.Count() > 12) {
|
|
OnNmeaReceived(new NmeaReceivedEventArgs(sentence));
|
|
if (words[2].ToUpper() != "A") { continue; }
|
|
SetTimeOffset(words[9], words[1]);
|
|
GpsCoordinates gc = new GpsCoordinates() {
|
|
Latitude = DmsToDd2(words[3], words[4]),
|
|
Longitude = DmsToDd2(words[5], words[6]),
|
|
Speed = (Convert.ToDouble(words[7]) * 1.15078),
|
|
Course = Convert.ToDouble(words[8]),
|
|
Timestamp = GetTimestamp(words[9], words[1])
|
|
};
|
|
movingCapable = true;
|
|
OnPositionChanged(new GpsCoordinatesChangedEventArgs(gc));
|
|
} else if (cmd == "$GPGSA" && words.Count() > 17) {
|
|
OnNmeaReceived(new NmeaReceivedEventArgs(sentence));
|
|
|
|
} else if (cmd == "$GPGSV" && words.Count() > 7) {
|
|
OnNmeaReceived(new NmeaReceivedEventArgs(sentence));
|
|
// Sometimes, we may not want to bother processing all of
|
|
// this data, so user can turn this off if not needed
|
|
if (!SatelliteStats) { return; }
|
|
|
|
// Hacky way to detect a new set of Satellite information.
|
|
// We're assuming we always receive the list of GPGSV's in
|
|
// order, so we clear the list of Active Satellites when
|
|
// we receive the first Message
|
|
int gsvId = Convert.ToInt32(words[2]);
|
|
if (gsvId == 1) { ActiveSatellites.Clear(); }
|
|
|
|
for (int i=4; i<words.Length; i+=4) {
|
|
SatelliteView sv = SvFromWords(words.Skip(i-1).Take(4).ToArray());
|
|
if (sv != null) { ActiveSatellites.AddOrUpdate(sv); }
|
|
}
|
|
//Console.WriteLine("Discovered " + satellites.Count + " satellites from a GSV Sentence!");
|
|
|
|
// Equally as hacky, we'll assume when we receive the last
|
|
// expected Message, we'll fire the event for SatellitesUpdated
|
|
if (gsvId == Convert.ToInt32(words[1])) { OnSatellitesUpdated(new SatellitesUpdatedEventArgs(ActiveSatellites.satellites.Values.ToList())); }
|
|
|
|
}
|
|
} catch (Exception ex1) {
|
|
Console.WriteLine("EX1:" + ex1.Message);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} catch (Exception ex) { Console.WriteLine("ERR:" + ex.Message); }
|
|
}
|
|
}
|
|
}
|