SafeDispatch/SipComponent/Simoco/Utils.cs

119 lines
5.3 KiB
C#
Raw Permalink Normal View History

2024-02-22 16:43:59 +00:00
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace SipComponent.Simoco
{
internal class Utils
{
/// <summary>
/// Returns gps information from an array of bytes in short NMEA format
/// </summary>
/// <param name="nmea">array of bytes in short NMEA format</param>
/// <param name="latitude">latitude</param>
/// <param name="longitude">longitude</param>
/// <param name="speed">speed</param>
/// <param name="time">time expressed as UTC</param>
public static void GPSInfo(byte[] nmea, out double latitude, out double longitude, out double speed,
out DateTime time)
{
if (nmea.Length != 10)
throw new ArgumentOutOfRangeException();
DateTime now = DateTime.UtcNow;
// | C NS EW Q speed speed speed speed | speed speed speed ndeg ndeg ndeg ndeg ndeg | ndeg ndeg nmin nmin nmin nmin nmin nmin |
// | byte 0 | byte 1 | byte 2
bool fixValid = false;
if (((nmea[0] >> 4) & 0x01) == 1)
fixValid = true;
if (fixValid)
{
sbyte NS = -1;
if (((nmea[0] >> 6) & 0x01) == 1)
NS = 1;
sbyte EW = -1;
if (((nmea[0] >> 5) & 0x01) == 1)
EW = 1;
byte speed_knots = (byte)((nmea[0] << 3) & 0x7F | (nmea[1] >> 5));
byte latitudeDegrees = (byte)((nmea[1] << 2) & 0x7F | (nmea[2] >> 6));
byte latitudeMinutes = (byte)(nmea[2] & 0x3F);
// | nminf nminf nminf nminf nminf nminf nminf nminf | nminf nminf nminf nminf nminf nminf edeg edeg |
// | byte 3 | byte 4 |
ushort latitudeFractionOfMin = (ushort)((nmea[3] >> 2) * 256 | (nmea[3] << 6) | (nmea[4] >> 2));
//
// | edeg edeg edeg edeg edeg edeg eminmm eminmm | eminmm eminmm eminmm eminmm eminf eminf eminf eminf |
// | byte 5 | byte 6 |
//
byte longitudeDegrees = (byte)((nmea[4] << 6) | (nmea[5] >> 2));
byte longitudeMinutes = (byte)((nmea[5] << 4) & 0x3F | (nmea[6] >> 4));
// | eminf eminf eminf eminf eminf eminf eminf eminf | eminf eminf h h h h h m | m m m m m s s s |
// | byte 7 | byte 8 | byte 9 |
ushort longitudeFractionOfMin = (ushort)(((nmea[6] << 2) & 0x3F | (nmea[7] >> 6) & 0xF3) * 256 |
(nmea[7] << 2) | ((nmea[8] >> 6) & 0xF3));
byte hour = (byte)((nmea[8] >> 1) & 0x1F);
byte minutes = (byte)((nmea[8] << 5) & 0x3F | (nmea[9] >> 3));
byte ten_sec = (byte)(nmea[9] & 0x07);
// viteza in km/h
speed = speed_knots * 1.852;
// latitudine in grade decimale
latitude = (latitudeDegrees + double.Parse(string.Format("{0}{1}{2}",
latitudeMinutes,
System.Globalization.CultureInfo.CurrentCulture.NumberFormat.NumberDecimalSeparator,
latitudeFractionOfMin)) / 60) * NS;
// longitude in grade decimale
longitude = (longitudeDegrees + double.Parse(
string.Format("{0}{1}{2}",
longitudeMinutes,
System.Globalization.CultureInfo.CurrentCulture.NumberFormat.NumberDecimalSeparator,
longitudeFractionOfMin)) / 60) * EW;
// time as unix time
time = new DateTime(now.Year, now.Month, now.Day, hour, minutes, ten_sec * 10);
}
else
{
speed = latitude = longitude = 0;
time = now;
}
//return string.Format(
// "{0}\r\n{1} degrees {2}.{3} minutes {4}\r\n{5} degrees {6}.{7} minutes {8}\r\nSpeed: {9} knots\r\nUTC time: {10:00}:{11:00}:{12:00}",
// fixValid ? "GPS valid fix" : "GPS no fix",
// latitudeDegrees, latitudeMinutes, latitudeFractionOfMin, N ? "North" : "South",
// longitudeDegrees, longitudeMinutes, longitudeFractionOfMin, E ? "East" : "West",
// speed, hour, minutes, ten_sec * 10
// );
}
public static void GetGPSFromStatusReport(byte[] deviceStatusReport, out double latitude, out double longitude)
{
byte[] latitudeBytes = new byte[4];
byte[] longitudeBytes = new byte[4];
Array.Copy(deviceStatusReport, 11, latitudeBytes, 0, 4);
Array.Reverse(latitudeBytes);
Array.Copy(deviceStatusReport, 15, longitudeBytes, 0, 4);
Array.Reverse(longitudeBytes);
latitude = BitConverter.ToInt32(latitudeBytes, 0) / 10000000.0;
longitude = BitConverter.ToInt32(longitudeBytes, 0) / 10000000.0;
}
}
}