public byte[] GetPacket(GPSDatum d)
{
gpsDatum = d; // To return in GetPacketString()
Frame3000Packet packet = new Frame3000Packet();
MnpPacketHeader h = new MnpPacketHeader();
h.sync = 0x81ff;
h.frame_id = 3000;
h.data_len = (ushort)(Marshal.SizeOf(packet) - Marshal.SizeOf(h));
h.reserved = 0;
h.header_checksum = 0;
packet.header = h;
packet.lat = d.i32Latitude;
packet.lon = d.i32Longitude;
packet.alt = 0;
packet.speed = 0;
packet.azimuth = 0;
packet.v_speed = 0;
packet.channels_in_sol = 0;
packet.diff_asserts = 0;
return GetBytes(packet);
}
/* $--RMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,xxxx,x.x,a*hh
1) Time (UTC)
2) Status, V = Navigation receiver warning
3) Latitude
4) N or S
5) Longitude
6) E or W
7) Speed over ground, knots
8) Track made good, degrees true
9) Date, ddmmyy
10) Magnetic Variation, degrees
11) E or W
12) Checksum */
//$GNRMC,090056.00,A,5651.2598,N,06035.8873,E,00.000,000.0,100112,,,A*7C
private string getRMSpacket(GPSDatum d)
{
double knot_speed = currentSpeed / 1000 * 3600 / 1.852;
string s = String.Format(CultureInfo.InvariantCulture, "{0:00.000}", knot_speed);
string s2 = getDirection(d);
DateTime n = DateTime.UtcNow;
string time = n.ToString("HHmmss");
string date = n.ToString("ddMMyy");
string val = (GpsValid) ? "A," : "V,";
string p1 = "$GNRMC," + time + ".00," + val + d.nmeaLatitude + "," + d.LatLetter + "," + d.nmeaLongitude + "," + d.LonLetter + "," + s + "," + s2 + "," + date + ",,,A*"; //TODO: not complite NMEA string
p1 = inject_checksum(p1);
//$GNGGA,050631.45,5651.2595,N,06035.8363,E,0,00,0.0,254.4,M,,M,,*68
string p2 = "$GNGGA," + time + ".00," + d.nmeaLatitude + "," + d.LatLetter + "," + d.nmeaLongitude + "," + d.LonLetter + ",1,10,2.0,254.4,M,,M,,*";
p2 = inject_checksum(p2);
//$GNGSA,A,1,,,,,,,,,,,,,,,*00
string p3 = "$GNGSA," + val + "3,01,02,03,04,05,06,07,08,09,10,11,12,2.0,2.0,2.2*";
p3 = inject_checksum(p3);
prevDot = d;
return p1 + p2 + p3;
}
public static double distanse(GPSDatum d1, GPSDatum d2)
{
double lon1p = d1.rLongitude;
double lon2p = d2.rLongitude;
double lat1p = d1.rLatitude;
double lat2p = d2.rLatitude;
double r = r_eq + (r_pl - r_eq) * (lat1p+lat2p)/Math.PI;
double Dist = r * Math.Acos(Math.Sin(lat1p)*Math.Sin(lat2p) + Math.Cos(lat1p)*Math.Cos(lat2p)*Math.Cos(lon1p -lon2p));
return Dist;
}
string getPacketS(GPSDatum d)
{
packet = getRMSpacket(d);
return packet;
}
private string getDirection(GPSDatum d)
{
double angle = 0;
GPSDatum d0 = prevDot;
if (d0 == null) return "000.0";
double deltaLat = d.rLatitude - d0.rLatitude;
double deltaLon = d.rLongitude - d0.rLongitude;
if (deltaLon == 0)
angle = 90 * Math.Sign(deltaLat);
else
{
double c = 90;
if (deltaLon < 0) c = 270;
angle = c - Math.Atan(deltaLat / deltaLon) * (180 / Math.PI);
}
return String.Format(CultureInfo.InvariantCulture, "{0:000.0}", angle);
}
public string Printable(GPSDatum d)
{
return packet;
}
public byte[] getPacket(GPSDatum d)
{
return System.Text.Encoding.ASCII.GetBytes(getPacketS(d));
}
public byte[] GetPacket(GPSDatum d)
{
return getPacket(d);
}
