diff --git a/ExtLibs/Utilities/adsb.cs b/ExtLibs/Utilities/adsb.cs index 3b184a6c0c..5debb0c450 100644 --- a/ExtLibs/Utilities/adsb.cs +++ b/ExtLibs/Utilities/adsb.cs @@ -11,6 +11,7 @@ using Newtonsoft.Json; using Newtonsoft.Json.Linq; + namespace MissionPlanner.Utilities { public class adsb @@ -760,19 +761,21 @@ public static void ReadMessage(Stream st1) { PointLatLngAltHdg plla = new PointLatLngAltHdg(plane.plla()); plla.Heading = (float)plane.heading; + if (plane.CallSign != null) plla.CallSign = plane.CallSign; + plla.Speed = (float)plane.ground_speed; if (plla.Lat == 0 && plla.Lng == 0) continue; if (UpdatePlanePosition != null && plla != null) UpdatePlanePosition(null, plla); //Console.WriteLine(plane.pllalocal(plane.llaeven)); - Console.WriteLine(plane.ID + " " + plla); + Console.WriteLine("AVR ADSB: " + plane.ID + " " + plla + " CS: " + plla.CallSign); } } } else if ((by == 'M' || by == 'S' || by == 'A' || by == 'I' || by == 'C') && !binary) // msg clk sta air id sel { string line = ((char)by) +ReadLine(st1); - + //Console.WriteLine("ADSB: " + line); if (line.StartsWith("MSG")) { string[] strArray = line.Split(new char[] { ',' }); @@ -1068,6 +1071,9 @@ public static Plane ReadMessage(string avrline) } adsbmess.Ident = builder.ToString(); + + ((Plane)Planes[adsbmess.AA.ToString("X5")]).CallSign = adsbmess.Ident; + //Console.WriteLine("Ident " + builder.ToString()); } else if (adsbmess.DF == 17 && adsbmess.TypeCode == 0x13) // velocity @@ -1105,10 +1111,12 @@ public static Plane ReadMessage(string avrline) nsvel *= -1; double cog = (Math.Atan2(ewvel, nsvel) * (180 / Math.PI)); + double _gs = Math.Sqrt(ewvel * ewvel + nsvel * nsvel) ; - Console.WriteLine("vel " + ewvel + " " + nsvel + " " + cog); + Console.WriteLine("vel " + ewvel + " " + nsvel + " " + cog + " gs " + _gs); ((Plane)Planes[adsbmess.AA.ToString("X5")]).heading = (cog + 360) % 360; + ((Plane)Planes[adsbmess.AA.ToString("X5")]).ground_speed = (int) _gs; break; }