From eefd2167358c5c121c61b58f0eab4f85034d5ef8 Mon Sep 17 00:00:00 2001 From: MUSTARDTIGER FPV <122312693+MUSTARDTIGERFPV@users.noreply.github.com> Date: Mon, 29 Jul 2024 18:23:04 -0700 Subject: [PATCH] Refactor ADS-B input support in Mission Planner (#3251) * Fix ADSB: heading, speed & callsign, fix unit conv * Add Squawk codes to SBS-1 decode * Debug-filled functional 1Hz ADS-B * Send less planes faster if less are in radius * Fix ADS-B unit conversions, fix internal API, add velocities * Use ADS-B ThreatLevel from MAV * Clean up old planes from our internal memory * Add ADS-B Collision risk to tooltip * Fix ADS-B radius render errors * Improve collision risk tooltip wording * Pass source to UpdatePlanePosition * Only update threat level when necessary, fix messages * Shorten ADS-B loop, simplify code * Remove log lines * Only show tooltips on hover for ADSB * Fix scaling bugs & add user-agent * Fix merge issue * Improve ADSB HTTP logic, rate limiting, default to adsb.lol --- ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs | 22 +- ExtLibs/Maps/GMapMarkerQuad.cs | 16 +- ExtLibs/Utilities/Download.cs | 17 + ExtLibs/Utilities/PointLatLngAlt.cs | 3 + ExtLibs/Utilities/adsb.cs | 425 +++++++++++------- GCSViews/ConfigurationView/ConfigPlanner.cs | 28 +- GCSViews/FlightData.cs | 29 +- MainV2.cs | 164 +++++-- 8 files changed, 484 insertions(+), 220 deletions(-) diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs index 3a43d133ab..6f021e31ff 100644 --- a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs +++ b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs @@ -5122,15 +5122,21 @@ public async Task readPacketAsync() var id = adsb.ICAO_address.ToString("X5"); if (_UpdateADSBPlanePosition != null) - _UpdateADSBPlanePosition(this, new adsb.PointLatLngAltHdg(adsb.lat / 1e7, + _UpdateADSBPlanePosition(this, new adsb.PointLatLngAltHdg( + adsb.lat / 1e7, adsb.lon / 1e7, - adsb.altitude / 1000.0, adsb.heading * 0.01f, adsb.hor_velocity * 0.01f, id, - DateTime.Now) - { - CallSign = Encoding.UTF8.GetString(adsb.callsign), - Squawk = adsb.squawk, - Raw = adsb - } + adsb.altitude / 1000, + adsb.heading * 0.01f, + adsb.hor_velocity, + id, + DateTime.Now + ) + { + CallSign = Encoding.UTF8.GetString(adsb.callsign), + Squawk = adsb.squawk, + Raw = adsb, + VerticalSpeed = adsb.ver_velocity, + } ); } diff --git a/ExtLibs/Maps/GMapMarkerQuad.cs b/ExtLibs/Maps/GMapMarkerQuad.cs index bb8248de89..8fdcae2589 100644 --- a/ExtLibs/Maps/GMapMarkerQuad.cs +++ b/ExtLibs/Maps/GMapMarkerQuad.cs @@ -198,26 +198,38 @@ public override void OnRender(IGraphics g) double width = (Overlay.Control.MapProvider.Projection.GetDistance(Overlay.Control.FromLocalToLatLng(0, 0), Overlay.Control.FromLocalToLatLng(Overlay.Control.Width, 0)) * 1000.0); + // size of a square meter in pixels on our map double m2pixelwidth = Overlay.Control.Width / width; GPoint loc = new GPoint((int) (LocalPosition.X - (m2pixelwidth * warn * 2)), LocalPosition.Y); + var markerDimension = (int)Math.Abs(loc.X - LocalPosition.X); + if (markerDimension == 0) + { + return; + } + if (m2pixelwidth > 0.001 && warn > 0) g.DrawArc(Pens.Orange, new System.Drawing.Rectangle( LocalPosition.X - Offset.X - (int) (Math.Abs(loc.X - LocalPosition.X) / 2), LocalPosition.Y - Offset.Y - (int) Math.Abs(loc.X - LocalPosition.X) / 2, - (int) Math.Abs(loc.X - LocalPosition.X), (int) Math.Abs(loc.X - LocalPosition.X)), 0, + markerDimension, markerDimension), 0, 360); loc = new GPoint((int) (LocalPosition.X - (m2pixelwidth * danger * 2)), LocalPosition.Y); + markerDimension = (int)Math.Abs(loc.X - LocalPosition.X); + if (markerDimension == 0) + { + return; + } if (m2pixelwidth > 0.001 && danger > 0) g.DrawArc(Pens.Red, new System.Drawing.Rectangle( LocalPosition.X - Offset.X - (int) (Math.Abs(loc.X - LocalPosition.X) / 2), LocalPosition.Y - Offset.Y - (int) Math.Abs(loc.X - LocalPosition.X) / 2, - (int) Math.Abs(loc.X - LocalPosition.X), (int) Math.Abs(loc.X - LocalPosition.X)), 0, + markerDimension, markerDimension), 0, 360); } } diff --git a/ExtLibs/Utilities/Download.cs b/ExtLibs/Utilities/Download.cs index 7ab4a14f1b..825c14f3ef 100644 --- a/ExtLibs/Utilities/Download.cs +++ b/ExtLibs/Utilities/Download.cs @@ -321,6 +321,23 @@ public static async Task GetAsync(string uri) return await Task.Run(() => (content)); } + public struct HTTPResult + { + public string content; + public HttpStatusCode status; + } + + /// + /// Perform a GET request and return the content and status code + /// + public static async Task GetAsyncWithStatus(string uri) + { + var httpClient = new HttpClient(); + var response = await httpClient.GetAsync(uri); + var content = await response.Content.ReadAsStringAsync(); + return await Task.Run(() => (new HTTPResult() { content = content, status = response.StatusCode })); + } + public static event EventHandler RequestModification; public static async Task getFilefromNetAsync(string url, string saveto, Action status = null) diff --git a/ExtLibs/Utilities/PointLatLngAlt.cs b/ExtLibs/Utilities/PointLatLngAlt.cs index 647b3ee5f6..238388938f 100644 --- a/ExtLibs/Utilities/PointLatLngAlt.cs +++ b/ExtLibs/Utilities/PointLatLngAlt.cs @@ -21,6 +21,9 @@ public class PointLatLngAlt: IComparable public static readonly PointLatLngAlt Zero = new PointLatLngAlt(); public double Lat { get; set; } = 0; public double Lng { get; set; } = 0; + /// + /// Altitude in meters + /// public double Alt { get; set; } = 0; public string Tag { get; set; } = ""; public string Tag2 { get; set; } = ""; diff --git a/ExtLibs/Utilities/adsb.cs b/ExtLibs/Utilities/adsb.cs index 9b48d8264c..239abc62e7 100644 --- a/ExtLibs/Utilities/adsb.cs +++ b/ExtLibs/Utilities/adsb.cs @@ -10,7 +10,9 @@ using System.Threading; using Newtonsoft.Json; using Newtonsoft.Json.Linq; - +using static MissionPlanner.Utilities.rtcm3; +using Flurl.Http; +using System.Diagnostics; namespace MissionPlanner.Utilities { @@ -36,15 +38,54 @@ public class adsb static bool run = false; static Thread thisthread; + /// + /// The server to connect to for ADS-B data. If this is set to a URL, the API will be used, otherwise if it's an IP or hostname, a direct connection will be attempted. + /// public static string server = ""; + /// + /// The port to connect to for ADS-B data. If server is a URL, this is ignored. + /// public static int serverport = 0; + /// + /// Radius in nm to request planes from the API + /// + private static int httpRequestRadius = 100; - private static int adsbexchangerange = 100; + /// + /// Multiplication constant to convert knots to cm/s + /// + private const double KNOTS_TO_CMS = 51.444; + /// + /// Multiplication constant to convert feet to meters + /// + private const double FEET_TO_METERS = 1.0d / 3.28d; + /// + /// Multiplication constant to convert feet/mim to cm/s + /// + private const double FTM_TO_CMS = 1.0d / 1.968d; + /// + /// Timeout in milliseconds for ADS-B connections to default endpoints. + /// These are all localhost, so we can be pretty short. + /// + private const int CONNECT_TIMEOUT_MILLISECONDS = 25; + + /// + /// Loop delay in milliseconds for ADS-B connections to HTTP API endpoints. + /// + private const int API_LOOP_DELAY_MILLISECONDS = 1000; + + /// + /// Maximum loop delay in milliseconds for ADS-B connections to HTTP API endpoints. + /// + private const int API_LOOP_DELAY_MAX_MILLISECONDS = 10000; + + /// + /// Application version string for HTTP user agents - set by MainV2.cs + /// + public static string ApplicationVersion { get; set; } public adsb() { - log.Info("adsb ctor"); - thisthread = new Thread(TryConnect); thisthread.Name = "ADSB reader thread"; @@ -77,11 +118,10 @@ void TryConnect() while (run) { - log.Info("adsb connect loop"); - //custom try { - if (!String.IsNullOrEmpty(server)) + // if we have a server string that's not an HTTP(S) URL, try to connect to it + if (!String.IsNullOrEmpty(server) && !server.StartsWith("http", StringComparison.InvariantCultureIgnoreCase)) { using (TcpClient cl = new TcpClient()) { @@ -95,17 +135,112 @@ void TryConnect() } catch (Exception ex) { log.Error(ex); } - // dump1090 sbs + // HTTP API try { - using (TcpClient cl = new TcpClient()) + if (!String.IsNullOrEmpty(server) && server.StartsWith("http", StringComparison.InvariantCultureIgnoreCase) && CurrentPosition != PointLatLngAlt.Zero) { + // ADSB Exchange API format - see https://api.adsb.lol/docs + string url = "{0}/v2/point/{1}/{2}/{3}"; + Download.RequestModification += (u, request) => { + // for future use if necessary: request.Headers.Add("X-API-Auth", "example"); + request.SetHeader("User-Agent", "Mission-Planner/" + ApplicationVersion); + }; + var delay = API_LOOP_DELAY_MILLISECONDS; + + while (true) + { + // Store start time + var timer = Stopwatch.StartNew(); + + string formattedUrl = string.Format(url, server, CurrentPosition.Lat, CurrentPosition.Lng, httpRequestRadius); + var t = Download.GetAsyncWithStatus(formattedUrl); + t.Wait(); + + // Check for long running requests + if (timer.ElapsedMilliseconds > 5000) + { + log.Warn("Long running request: " + formattedUrl + " took " + timer.ElapsedMilliseconds + "ms"); + } + + if (!t.IsFaulted && t.Result.status == System.Net.HttpStatusCode.OK) + { + var result = JsonConvert.DeserializeObject(t.Result.content); - cl.Connect(System.Net.IPAddress.Loopback, 30003); + // Increase range if we don't see many planes + if (result.ac.Count < 10) + httpRequestRadius = Math.Min(httpRequestRadius + 10, 250); - log.Info("Connected loopback:30003"); + foreach (var ac in result.ac) + { + // The API sometimes sets this value to either the string "ground" or a JSON Number. This handles that. + int alt = 0; + if (ac.alt_baro is long intValue) + { + alt = (int)intValue; + } + PointLatLngAltHdg plane = new PointLatLngAltHdg(ac.lat, + ac.lon, + alt * FEET_TO_METERS, + (float)ac.track, + ac.gs * KNOTS_TO_CMS, + ac.hex.Trim().ToUpper(), + DateTime.Now) + { + VerticalSpeed = ac.baro_rate * FTM_TO_CMS, + CallSign = (ac.flight ?? "").Trim().ToUpper(), + Squawk = Convert.ToUInt16(ac.squawk, 16) // Convert the hex value back to a raw uint16 + }; + + UpdatePlanePosition(this, plane); + } + } + // Check HTTP response code for rate limiting + if (!t.IsFaulted && (int)t.Result.status == 429) + { + delay = Math.Min(delay * 2, API_LOOP_DELAY_MAX_MILLISECONDS); + log.Info("Rate limited, increasing delay to " + delay + "ms"); + } + else + { + // Check HTTP response code for bad requests, etc. + if (!t.IsFaulted && (int)t.Result.status != 200) + { + delay = Math.Min(delay * 2, API_LOOP_DELAY_MAX_MILLISECONDS); + log.Warn("HTTP request to " + formattedUrl + " failed: " + t.Result.status + " " + t.Result.content); + } + } - ReadMessage(cl.GetStream()); + // Sleep for the remainder of the loop delay + var elapsed = timer.ElapsedMilliseconds; + if (elapsed < delay) + { + Thread.Sleep(delay - (int)elapsed); + } + } + } + } + catch (Exception e) + { + log.Warn(e); + } + + // dump1090 sbs + try + { + using (TcpClient cl = new TcpClient()) + { + var result = cl.BeginConnect(System.Net.IPAddress.Loopback, 30003, null, null); + result.AsyncWaitHandle.WaitOne(CONNECT_TIMEOUT_MILLISECONDS); + if (cl.Connected) + { + log.Info("Connected loopback:30003"); + ReadMessage(cl.GetStream()); + } + else + { + cl.Close(); + } } } catch (Exception) { } @@ -115,12 +250,17 @@ void TryConnect() { using (TcpClient cl = new TcpClient()) { - - cl.Connect(System.Net.IPAddress.Loopback, 30002); - - log.Info("Connected loopback:30002"); - - ReadMessage(cl.GetStream()); + var result = cl.BeginConnect(System.Net.IPAddress.Loopback, 30002, null, null); + result.AsyncWaitHandle.WaitOne(CONNECT_TIMEOUT_MILLISECONDS); + if (cl.Connected) + { + log.Info("Connected loopback:30002"); + ReadMessage(cl.GetStream()); + } + else + { + cl.Close(); + } } } catch (Exception) { } @@ -131,12 +271,17 @@ void TryConnect() { using (TcpClient cl = new TcpClient()) { - - cl.Connect(System.Net.IPAddress.Loopback, 31004); - - log.Info("Connected loopback:31004"); - - ReadMessage(cl.GetStream()); + var result = cl.BeginConnect(System.Net.IPAddress.Loopback, 31004, null, null); + result.AsyncWaitHandle.WaitOne(CONNECT_TIMEOUT_MILLISECONDS); + if (cl.Connected) + { + log.Info("Connected loopback:31004"); + ReadMessage(cl.GetStream()); + } + else + { + cl.Close(); + } } } catch (Exception) { } @@ -146,12 +291,17 @@ void TryConnect() { using (TcpClient cl = new TcpClient()) { - - cl.Connect(System.Net.IPAddress.Loopback, 31001); - - log.Info("Connected loopback:31001"); - - ReadMessage(cl.GetStream()); + var result = cl.BeginConnect(System.Net.IPAddress.Loopback, 31001, null, null); + result.AsyncWaitHandle.WaitOne(CONNECT_TIMEOUT_MILLISECONDS); + if (cl.Connected) + { + log.Info("Connected loopback:31001"); + ReadMessage(cl.GetStream()); + } + else + { + cl.Close(); + } } } catch (Exception) { } @@ -162,139 +312,88 @@ void TryConnect() { using (TcpClient cl = new TcpClient()) { - - cl.Connect(System.Net.IPAddress.Loopback, 47806); - - log.Info("Connected loopback:47806"); - - ReadMessage(cl.GetStream()); - } - } - catch (Exception) { } - - // adsbexchange - try - { - - if (CurrentPosition != PointLatLngAlt.Zero) - { - string url = - "http://public-api.adsbexchange.com/VirtualRadar/AircraftList.json?lat={0}&lng={1}&fDstL=0&fDstU={2}"; - string path = Settings.GetDataDirectory() + Path.DirectorySeparatorChar + "adsb.json"; - var ans = Download.getFilefromNet(String.Format(url, CurrentPosition.Lat, CurrentPosition.Lng, adsbexchangerange), - path); - - if (ans) + var result = cl.BeginConnect(System.Net.IPAddress.Loopback, 47806, null, null); + result.AsyncWaitHandle.WaitOne(CONNECT_TIMEOUT_MILLISECONDS); + if (cl.Connected) { - var result = JsonConvert.DeserializeObject(File.ReadAllText(path)); - - if (result.acList.Count < 1) - adsbexchangerange = Math.Min(adsbexchangerange + 10, 400); - - foreach (var acList in result.acList) - { - var plane = new MissionPlanner.Utilities.adsb.PointLatLngAltHdg(acList.Lat, acList.Long, - acList.Alt * 0.3048, - (float) acList.Trak, acList.Spd, acList.Icao, DateTime.Now); - - UpdatePlanePosition(null, plane); - } + log.Info("Connected loopback:47806"); + ReadMessage(cl.GetStream()); + } + else + { + cl.Close(); } } } - catch (Exception) - { + catch (Exception) { } - } // cleanup any sockets that might be outstanding. GC.Collect(); - System.Threading.Thread.Sleep(5000); + System.Threading.Thread.Sleep(2500); } log.Info("adsb thread exit"); } - - public class Feed - { - public int id { get; set; } - public string name { get; set; } - public bool polarPlot { get; set; } - } - - public class AcList + public class Ac { - public int Id { get; set; } - public int Rcvr { get; set; } - public bool HasSig { get; set; } - public int Sig { get; set; } - public string Icao { get; set; } - public bool Bad { get; set; } - public string Reg { get; set; } - public string FSeen { get; set; } - public int TSecs { get; set; } - public int CMsgs { get; set; } - public int Alt { get; set; } - public int GAlt { get; set; } - public double InHg { get; set; } - public int AltT { get; set; } - public string Call { get; set; } - public double Lat { get; set; } - public double Long { get; set; } - public string PosTime { get; set; } - public bool Mlat { get; set; } - public bool Tisb { get; set; } - public double Spd { get; set; } - public double Trak { get; set; } - public bool TrkH { get; set; } - public string Type { get; set; } - public string Mdl { get; set; } - public string Man { get; set; } - public string CNum { get; set; } - public string Op { get; set; } - public string OpIcao { get; set; } - public string Sqk { get; set; } - public int Vsi { get; set; } - public int VsiT { get; set; } - public double Dst { get; set; } - public double Brng { get; set; } - public int WTC { get; set; } - public int Species { get; set; } - public string Engines { get; set; } - public int EngType { get; set; } - public int EngMount { get; set; } - public bool Mil { get; set; } - public string Cou { get; set; } - public bool HasPic { get; set; } - public bool Interested { get; set; } - public int FlightsCount { get; set; } - public bool Gnd { get; set; } - public int SpdTyp { get; set; } - public bool CallSus { get; set; } - public int Trt { get; set; } - public string Year { get; set; } - public string From { get; set; } - public string To { get; set; } - public bool? Help { get; set; } + public string hex { get; set; } + public string type { get; set; } + public string r { get; set; } + public string t { get; set; } + public string desc { get; set; } + public string ownOp { get; set; } + public string year { get; set; } + public object alt_baro { get; set; } + public double gs { get; set; } + public double track { get; set; } + public int baro_rate { get; set; } + public string squawk { get; set; } + public string category { get; set; } + public double lat { get; set; } + public double lon { get; set; } + public int nic { get; set; } + public int rc { get; set; } + public double seen_pos { get; set; } + public int version { get; set; } + public int nic_baro { get; set; } + public int nac_p { get; set; } + public int nac_v { get; set; } + public int sil { get; set; } + public string sil_type { get; set; } + public List mlat { get; set; } + public List tisb { get; set; } + public int messages { get; set; } + public double seen { get; set; } + public double rssi { get; set; } + public double dst { get; set; } + public double dir { get; set; } + public string flight { get; set; } + public int? alt_geom { get; set; } + public string emergency { get; set; } + public double? nav_qnh { get; set; } + public int? nav_altitude_mcp { get; set; } + public double? nav_heading { get; set; } + public int? gva { get; set; } + public int? sda { get; set; } + public int? alert { get; set; } + public int? spi { get; set; } + public int? geom_rate { get; set; } } public class RootObject { - public int src { get; set; } - public List feeds { get; set; } - public int srcFeed { get; set; } - public bool showSil { get; set; } - public bool showFlg { get; set; } - public bool showPic { get; set; } - public int flgH { get; set; } - public int flgW { get; set; } - public List acList { get; set; } - public int totalAc { get; set; } - public string lastDv { get; set; } - public int shtTrlSec { get; set; } - public long stm { get; set; } + public List ac { get; set; } + public string msg { get; set; } + public long now { get; set; } + public int total { get; set; } + public long ctime { get; set; } + public int ptime { get; set; } } + /// + /// Our table of planes currently being tracked + /// static Hashtable Planes = new Hashtable(); public class Plane @@ -314,7 +413,9 @@ public class Plane double reflat = -34.988; double reflng = 117.8574; + // Heading in degrees, 0 is north public double heading = 0; + // Horizontal ground speed in cm/s internal int ground_speed; public Plane() @@ -732,7 +833,7 @@ private byte[] ConvertToByteArray(uint crc, bool littleEndian) } } - public static void ReadMessage(Stream st1) + public void ReadMessage(Stream st1) { bool avr = false; bool binary = false; @@ -762,11 +863,11 @@ 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; + plla.Speed = (float)plane.ground_speed * KNOTS_TO_CMS; if (plla.Lat == 0 && plla.Lng == 0) continue; if (UpdatePlanePosition != null && plla != null) - UpdatePlanePosition(null, plla); + UpdatePlanePosition(this, plla); //Console.WriteLine(plane.pllalocal(plane.llaeven)); Console.WriteLine("AVR ADSB: " + plane.ID + " " + plla + " CS: " + plla.CallSign); } @@ -796,6 +897,7 @@ public static void ReadMessage(Stream st1) { // H = HAE altitude = (int)double.Parse(strArray[11].TrimEnd('H','h'), CultureInfo.InvariantCulture);// Integer. Mode C Altitude relative to 1013 mb (29.92" Hg). + altitude = (int)((double)altitude * FEET_TO_METERS); } catch { } @@ -811,11 +913,17 @@ public static void ReadMessage(Stream st1) lon = double.Parse(strArray[15], CultureInfo.InvariantCulture);//Float. Longitude } catch { } + int vspeed = 0; + try + { + vspeed = (int)(int.Parse(strArray[16]) * FTM_TO_CMS); + } + catch { } ushort squawk = 0; try { - squawk = ushort.Parse(strArray[17]); // Squawk transponder code + squawk = Convert.ToUInt16(strArray[17], 16); // Convert the hex value back to a raw uint16 } catch { } @@ -858,12 +966,12 @@ public static void ReadMessage(Stream st1) { int ground_speed = (int)double.Parse(strArray[12], CultureInfo.InvariantCulture);// Integer. Speed over ground. - ((Plane)Planes[hex_ident]).ground_speed = ground_speed;//Integer. Ground track angle. + ((Plane)Planes[hex_ident]).ground_speed = (int)((double)ground_speed * KNOTS_TO_CMS); // knots -> cm/s } catch { } try { - ((Plane)Planes[hex_ident]).heading = (int)double.Parse(strArray[13], CultureInfo.InvariantCulture);//Integer. Ground track angle. + ((Plane)Planes[hex_ident]).heading = (int)double.Parse(strArray[13], CultureInfo.InvariantCulture);//Integer degrees, 0 = N. Ground track angle. } catch { } @@ -930,7 +1038,7 @@ public static void ReadMessage(Stream st1) if (plane.CallSign != null) plla.CallSign = plane.CallSign; plla.Speed = plane.ground_speed; if (UpdatePlanePosition != null && plla != null) - UpdatePlanePosition(null, plla); + UpdatePlanePosition(this, plla); //Console.WriteLine(plane.pllalocal(plane.llaeven)); //Console.WriteLine(plla); } @@ -1133,7 +1241,7 @@ public static Plane ReadMessage(string avrline) 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; + ((Plane)Planes[adsbmess.AA.ToString("X5")]).ground_speed = (int)(_gs * KNOTS_TO_CMS); break; } @@ -1185,6 +1293,9 @@ public PointLatLngAltHdg(double lat, double lng, double alt, float heading, doub this.Speed = speed; } + /// + /// Heading in degrees, 0 = north + /// public float Heading { get; set; } public MAVLink.MAV_COLLISION_THREAT_LEVEL ThreatLevel { get; set; } @@ -1196,9 +1307,17 @@ public PointLatLngAltHdg(double lat, double lng, double alt, float heading, doub public string CallSign { get; set; } = ""; public ushort Squawk { get; set; } - + + /// + /// Horizontal ground speed in cm/s + /// public double Speed { get; set; } + /// + /// Vertical speed in cm/s, positive = ascent + /// + public double VerticalSpeed { get; set; } public object Raw { get; set; } + public object Source { get; set; } } } } diff --git a/GCSViews/ConfigurationView/ConfigPlanner.cs b/GCSViews/ConfigurationView/ConfigPlanner.cs index c0f2f23694..fe7e60e367 100644 --- a/GCSViews/ConfigurationView/ConfigPlanner.cs +++ b/GCSViews/ConfigurationView/ConfigPlanner.cs @@ -908,19 +908,31 @@ private void chk_ADSB_CheckedChanged(object sender, EventArgs e) if (((CheckBox)sender).Checked) { - var server = "127.0.0.1"; + var server = "https://api.adsb.lol/"; // default to adsb.lol if (Settings.Instance["adsbserver"] != null) server = Settings.Instance["adsbserver"]; - if (DialogResult.Cancel == InputBox.Show("Server", "Server IP?", ref server)) + if (DialogResult.Cancel == InputBox.Show("ADSB Server", "Server IP or API base URL (see https://ardupilot.org/planner/docs/common-adsb.html)", ref server)) return; + // Strip ending slash off server + if (server.EndsWith("/")) + server = server.Substring(0, server.Length - 1); Settings.Instance["adsbserver"] = server; - var port = "30003"; - if (Settings.Instance["adsbport"] != null) - port = Settings.Instance["adsbport"]; - if (DialogResult.Cancel == InputBox.Show("Server port", "Server port?", ref port)) - return; - Settings.Instance["adsbport"] = port; + // if server isn't an HTTP(S) URL, assume TCP and ask for a port + if (!server.StartsWith("http", StringComparison.InvariantCultureIgnoreCase)) + { + var port = "30003"; + if (Settings.Instance["adsbport"] != null) + port = Settings.Instance["adsbport"]; + if (DialogResult.Cancel == InputBox.Show("Server port", "Server port?", ref port)) + return; + Settings.Instance["adsbport"] = port; + } + } + else + { + // if we're disabling ADSB, clear the list of planes + MainV2.instance.adsbPlanes.Clear(); } Settings.Instance["enableadsb"] = chk_ADSB.Checked.ToString(); diff --git a/GCSViews/FlightData.cs b/GCSViews/FlightData.cs index c202f76708..f557e542c7 100644 --- a/GCSViews/FlightData.cs +++ b/GCSViews/FlightData.cs @@ -3648,7 +3648,14 @@ private void mainloop() this.BeginInvoke((MethodInvoker) delegate { but_disablejoystick.Visible = true; }); } - adsb.CurrentPosition = MainV2.comPort.MAV.cs.HomeLocation; + if (MainV2.comPort.MAV.cs.Location != PointLatLngAlt.Zero) + { + adsb.CurrentPosition = MainV2.comPort.MAV.cs.Location; + } + else + { + adsb.CurrentPosition = gMapControl1.Position; + } // show proximity screen if (MainV2.comPort.MAV?.Proximity != null && MainV2.comPort.MAV.Proximity.DataAvailable) @@ -4063,11 +4070,18 @@ private void mainloop() return; adsbplane.ToolTipText = "ICAO: " + pllau.Tag + "\n" + - "CallSign: " + pllau.CallSign + "\n" + - "Squawk: " + Convert.ToString(pllau.Squawk) + "\n" + - "Alt: " + (pllau.Alt * CurrentState.multiplieralt).ToString("0") + "\n" + - "Speed: " + pllau.Speed.ToString("0") + "\n" + - "Heading: " + pllau.Heading.ToString("0"); + "Callsign: " + pllau.CallSign + "\n" + + "Squawk: " + pllau.Squawk.ToString("X4") + "\n" + + "Alt: " + (pllau.Alt * CurrentState.multiplieralt).ToString("0") + " " + CurrentState.AltUnit + "\n" + + "Speed: " + (pllau.Speed / 100 /* cm to m */ * CurrentState.multiplierspeed).ToString("0") + " " + CurrentState.SpeedUnit + "\n" + + "VSpeed: " + (pllau.VerticalSpeed / 100 /* cm to m */ * CurrentState.multiplierspeed).ToString("F1") + " " + CurrentState.SpeedUnit + "\n" + + "Heading: " + pllau.Heading.ToString("0") + "°"; + // Add distance + if (MainV2.comPort.MAV.cs.Location.Lat != 0 && MainV2.comPort.MAV.cs.Location.Lng != 0) + adsbplane.ToolTipText += "\n" + "Distance: " + (pllau.GetDistance(MainV2.comPort.MAV.cs.Location) * CurrentState.multiplierdist).ToString("0") + " " + CurrentState.DistanceUnit; + // Add collision threat level + if (pllau.ThreatLevel != MAVLink.MAV_COLLISION_THREAT_LEVEL.NONE) + adsbplane.ToolTipText += "\n" + "Collision risk: " + (pllau.ThreatLevel == MAVLink.MAV_COLLISION_THREAT_LEVEL.LOW ? "Warning" : "Danger"); adsbplane.ToolTipMode = MarkerTooltipMode.OnMouseOver; adsbplane.Position = pllau; adsbplane.heading = pllau.Heading; @@ -4077,9 +4091,6 @@ private void mainloop() { adsbplane.IsVisible = true; - if (pllau.DisplayICAO) - adsbplane.ToolTipMode = MarkerTooltipMode.Always; - switch (pllau.ThreatLevel) { case MAVLink.MAV_COLLISION_THREAT_LEVEL.NONE: diff --git a/MainV2.cs b/MainV2.cs index 4edefffec9..1fe3fc567d 100644 --- a/MainV2.cs +++ b/MainV2.cs @@ -38,6 +38,13 @@ using MissionPlanner.Joystick; using System.Net; using Newtonsoft.Json; +using MissionPlanner; +using Flurl.Util; +using Org.BouncyCastle.Bcpg; +using log4net.Repository.Hierarchy; +using System.Numerics; +using System.Runtime.Remoting.Metadata.W3cXsd2001; +using static MAVLink; using DroneCAN; namespace MissionPlanner @@ -516,6 +523,8 @@ public static bool sitl bool joystickthreadrun = false; + bool adsbThread = false; + Thread httpthread; Thread pluginthread; @@ -524,6 +533,16 @@ public static bool sitl /// private DateTime heatbeatSend = DateTime.UtcNow; + /// + /// track the last ads-b send time + /// + private DateTime adsbSend = DateTime.Now; + /// + /// track the adsb plane index we're round-robin sending + /// starts at -1 because it'll get incremented before sending + /// + private int adsbIndex = -1; + /// /// used to call anything as needed. /// @@ -635,6 +654,7 @@ public MainV2() ShowAirports = true; // setup adsb + Utilities.adsb.ApplicationVersion = System.Windows.Forms.Application.ProductVersion; Utilities.adsb.UpdatePlanePosition += adsb_UpdatePlanePosition; MAVLinkInterface.UpdateADSBPlanePosition += adsb_UpdatePlanePosition; @@ -1162,16 +1182,26 @@ void adsb_UpdatePlanePosition(object sender, MissionPlanner.Utilities.adsb.Point if (MainV2.instance.adsbPlanes.ContainsKey(id)) { + var plane = (adsb.PointLatLngAltHdg)instance.adsbPlanes[id]; + if (plane.Source == null && sender != null) + { + log.DebugFormat("Ignoring MAVLink-sourced ADSB_VEHICLE for locally-known aircraft {0}", adsb.Tag); + return; + } + // update existing - ((adsb.PointLatLngAltHdg) instance.adsbPlanes[id]).Lat = adsb.Lat; - ((adsb.PointLatLngAltHdg) instance.adsbPlanes[id]).Lng = adsb.Lng; - ((adsb.PointLatLngAltHdg) instance.adsbPlanes[id]).Alt = adsb.Alt; - ((adsb.PointLatLngAltHdg) instance.adsbPlanes[id]).Heading = adsb.Heading; - ((adsb.PointLatLngAltHdg) instance.adsbPlanes[id]).Time = DateTime.Now; - ((adsb.PointLatLngAltHdg) instance.adsbPlanes[id]).CallSign = adsb.CallSign; - ((adsb.PointLatLngAltHdg) instance.adsbPlanes[id]).Squawk = adsb.Squawk; - ((adsb.PointLatLngAltHdg) instance.adsbPlanes[id]).Raw = adsb.Raw; - ((adsb.PointLatLngAltHdg) instance.adsbPlanes[id]).Speed = adsb.Speed; + plane.Lat = adsb.Lat; + plane.Lng = adsb.Lng; + plane.Alt = adsb.Alt; + plane.Heading = adsb.Heading; + plane.Time = DateTime.Now; + plane.CallSign = adsb.CallSign; + plane.Squawk = adsb.Squawk; + plane.Raw = adsb.Raw; + plane.Speed = adsb.Speed; + plane.VerticalSpeed = adsb.VerticalSpeed; + plane.Source = sender; + instance.adsbPlanes[id] = plane; } else { @@ -1180,38 +1210,9 @@ void adsb_UpdatePlanePosition(object sender, MissionPlanner.Utilities.adsb.Point new adsb.PointLatLngAltHdg(adsb.Lat, adsb.Lng, adsb.Alt, adsb.Heading, adsb.Speed, id, DateTime.Now) - {CallSign = adsb.CallSign, Squawk = adsb.Squawk, Raw = adsb.Raw}; + {CallSign = adsb.CallSign, Squawk = adsb.Squawk, Raw = adsb.Raw, Source = sender}; } } - - try - { - // dont rebroadcast something that came from the drone - if (sender != null && sender is MAVLinkInterface) - return; - - MAVLink.mavlink_adsb_vehicle_t packet = new MAVLink.mavlink_adsb_vehicle_t(); - - packet.altitude = (int) (adsb.Alt * 1000); - packet.altitude_type = (byte) MAVLink.ADSB_ALTITUDE_TYPE.GEOMETRIC; - packet.callsign = adsb.CallSign.MakeBytes(); - packet.squawk = adsb.Squawk; - packet.emitter_type = (byte) MAVLink.ADSB_EMITTER_TYPE.NO_INFO; - packet.heading = (ushort) (adsb.Heading * 100); - packet.lat = (int) (adsb.Lat * 1e7); - packet.lon = (int) (adsb.Lng * 1e7); - packet.ICAO_address = uint.Parse(adsb.Tag, NumberStyles.HexNumber); - - packet.flags = (ushort) (MAVLink.ADSB_FLAGS.VALID_ALTITUDE | MAVLink.ADSB_FLAGS.VALID_COORDS | - MAVLink.ADSB_FLAGS.VALID_HEADING | MAVLink.ADSB_FLAGS.VALID_CALLSIGN); - - //send to current connected - MainV2.comPort.sendPacket(packet, MainV2.comPort.MAV.sysid, MainV2.comPort.MAV.compid); - } - catch - { - - } } @@ -2181,6 +2182,10 @@ protected override void OnClosing(CancelEventArgs e) serialThread = false; + log.Info("closing adsbthread"); + + adsbThread = false; + log.Info("closing joystickthread"); joystickthreadrun = false; @@ -3172,7 +3177,76 @@ private async void SerialReader() SerialThreadrunner.Set(); } - protected override void OnLoad(EventArgs e) + ManualResetEvent ADSBThreadRunner = new ManualResetEvent(false); + + /// + /// adsb periodic send thread + /// + private async void ADSBRunner() + { + if (adsbThread) + return; + adsbThread = true; + ADSBThreadRunner.Reset(); + while (adsbThread) + { + await Task.Delay(1000).ConfigureAwait(false); // run every 1000 ms + // Clean up old planes + HashSet planesToClean = new HashSet(); + lock(adsblock) + { + MainV2.instance.adsbPlanes.Where(a => a.Value.Time < DateTime.Now.AddSeconds(-30)).ForEach(a => planesToClean.Add(a.Key)); + planesToClean.ForEach(a => MainV2.instance.adsbPlanes.TryRemove(a, out _)); + + } + PointLatLngAlt ourLocation = comPort.MAV.cs.Location; + // Get only close planes, sorted by distance + var relevantPlanes = MainV2.instance.adsbPlanes + .Select(v => new { v, Distance = v.Value.GetDistance(ourLocation) }) + .Where(v => v.Distance <= 10000) + .Where(v => !(v.v.Value.Source is MAVLinkInterface)) + .OrderBy(v => v.Distance) + .Select(v => v.v.Value) + .Take(10) + .ToList(); + adsbIndex = (++adsbIndex % Math.Max(1, Math.Min(relevantPlanes.Count, 10))); + var currentPlane = relevantPlanes.ElementAtOrDefault(adsbIndex); + if (currentPlane == null) + { + continue; + } + MAVLink.mavlink_adsb_vehicle_t packet = new MAVLink.mavlink_adsb_vehicle_t(); + packet.altitude = (int)(currentPlane.Alt * 1000); + packet.altitude_type = (byte)MAVLink.ADSB_ALTITUDE_TYPE.GEOMETRIC; + packet.callsign = currentPlane.CallSign.MakeBytes(); + packet.squawk = currentPlane.Squawk; + packet.emitter_type = (byte)MAVLink.ADSB_EMITTER_TYPE.NO_INFO; + packet.heading = (ushort)(currentPlane.Heading * 100); + packet.lat = (int)(currentPlane.Lat * 1e7); + packet.lon = (int)(currentPlane.Lng * 1e7); + packet.hor_velocity = (ushort)(currentPlane.Speed); + packet.ver_velocity = (short)(currentPlane.VerticalSpeed); + try + { + packet.ICAO_address = uint.Parse(currentPlane.Tag, NumberStyles.HexNumber); + } + catch + { + log.WarnFormat("invalid icao address: {0}", currentPlane.Tag); + packet.ICAO_address = 0; + } + packet.flags = (ushort)(MAVLink.ADSB_FLAGS.VALID_ALTITUDE | MAVLink.ADSB_FLAGS.VALID_COORDS | + MAVLink.ADSB_FLAGS.VALID_VELOCITY | MAVLink.ADSB_FLAGS.VALID_HEADING | MAVLink.ADSB_FLAGS.VALID_CALLSIGN); + + //send to current connected + MainV2.comPort.sendPacket(packet, MainV2.comPort.MAV.sysid, MainV2.comPort.MAV.compid); + + } + + } + + +protected override void OnLoad(EventArgs e) { // check if its defined, and force to show it if not known about if (Settings.Instance["menu_autohide"] == null) @@ -3274,6 +3348,16 @@ protected override void OnLoad(EventArgs e) log.Error(ex); } + log.Info("start adsbsender"); + try + { + ADSBRunner(); + } + catch (NotSupportedException ex) + { + log.Error(ex); + } + log.Info("start plugin thread"); try {