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); }