internal static async Task<List<LatLon>> LoadWaypoints() { var waypoints = new List<LatLon>(); var config = await "waypoints.txt".ReadStringFromFile(); if (string.IsNullOrEmpty(config)) { Debug.WriteLine("Empty waypoints.txt file");//Write to display insetad return waypoints; } var wps = config.Split('\n'); foreach (var wp in wps) { try { if (string.IsNullOrEmpty(wp)) continue; var newWp = new LatLon(wp); if (newWp.DateTime > DateTime.MinValue) { waypoints.Add(new LatLon(wp)); } else { Debug.WriteLine("Invalid date/time, not loading waypoint");//Write to display insetad } } catch (Exception e) { Debug.WriteLine(e); //Write to display insetad } } return waypoints; }
internal async Task<bool> NavigateToWaypoint(LatLon currentWaypoint, CancellationToken cancelationToken) { var distanceHeading = GpsExtensions.GetDistanceAndHeadingToDestination(Gps.Gps.CurrentLatLon.Lat, Gps.Gps.CurrentLatLon.Lon, currentWaypoint.Lat, currentWaypoint.Lon); var distanceToWaypoint = distanceHeading[0]; var headingToWaypoint = distanceHeading[1]; var travelLengthX = 0D; var travelLengthZ = 0D; var travelRotationY = 0D; var nomGaitSpeed = 50D; travelLengthZ = -50; var turnDirection = "None"; while (distanceToWaypoint > 10) //Inches { if (cancelationToken.IsCancellationRequested) return false; await _display.WriteAsync($"WP D/H {distanceToWaypoint}, {headingToWaypoint}", 1); await _display.WriteAsync($"{turnDirection} {Gps.Gps.CurrentLatLon.Heading}", 2); if (headingToWaypoint + 5 > 359 && Math.Abs(headingToWaypoint - Gps.Gps.CurrentLatLon.Heading) > 1) { var tempHeading = (headingToWaypoint + 5) - 359; if (Gps.Gps.CurrentLatLon.Heading > tempHeading) { turnDirection = "Right"; travelRotationY = -1; } else { turnDirection = "Left"; travelRotationY = 1; } } else if (headingToWaypoint - 5 < 1 && Math.Abs(headingToWaypoint - Gps.Gps.CurrentLatLon.Heading) > 1) { var tempHeading = (headingToWaypoint + 359) - 5; if (Gps.Gps.CurrentLatLon.Heading < tempHeading) { turnDirection = "Right"; travelRotationY = 1; } else { turnDirection = "Left"; travelRotationY = -1; } } else if (Gps.Gps.CurrentLatLon.Heading > headingToWaypoint - 5 && Gps.Gps.CurrentLatLon.Heading < headingToWaypoint + 5) { travelRotationY = 0; turnDirection = "None"; } else if (headingToWaypoint > Gps.Gps.CurrentLatLon.Heading + 20) { if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint > 180) { turnDirection = "Left+"; travelRotationY = -2; } else { turnDirection = "Right+"; travelRotationY = 2; } } else if (headingToWaypoint > Gps.Gps.CurrentLatLon.Heading) { if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint > 180) { turnDirection = "Left"; travelRotationY = -1; } else { turnDirection = "Right"; travelRotationY = 1; } } else if (headingToWaypoint < Gps.Gps.CurrentLatLon.Heading - 20) //If it has a long ways to turn, go fast! { if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint < 180) { turnDirection = "Left+"; travelRotationY = -2; } else { turnDirection = "Right+"; travelRotationY = 2; //Turn towards its right } } else if (headingToWaypoint < Gps.Gps.CurrentLatLon.Heading) { if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint < 180) { turnDirection = "Left"; travelRotationY = -1; } else { turnDirection = "Right"; travelRotationY = 1; } } _ikController.RequestMovement(nomGaitSpeed, travelLengthX, travelLengthZ, travelRotationY); await Task.Delay(50, cancelationToken); distanceHeading = GpsExtensions.GetDistanceAndHeadingToDestination(Gps.Gps.CurrentLatLon.Lat, Gps.Gps.CurrentLatLon.Lon, currentWaypoint.Lat, currentWaypoint.Lon); distanceToWaypoint = distanceHeading[0]; headingToWaypoint = distanceHeading[1]; if (cancelationToken.IsCancellationRequested) return false; } await _display.WriteAsync($"WP D/H {distanceToWaypoint}, {headingToWaypoint}", 1); await _display.WriteAsync($"Heading {Gps.Gps.CurrentLatLon.Heading}", 2); return true; }
internal static LatLon ParseNmea(this string data) { try { var tokens = data.Split(','); var type = tokens[0]; switch (type) { case "GPGGA": //Global Positioning System Fix Data if (tokens.Length < 10) return null; var st = tokens[1]; _dateTime = new DateTime(DateTime.Now.Year, DateTime.Now.Month, DateTime.Now.Day, Convert.ToInt32(st.Substring(0, 2)), Convert.ToInt32(st.Substring(2, 2)), Convert.ToInt32(st.Substring(4, 2)), DateTimeKind.Local); _lat = Latitude2Double(tokens[2], tokens[3]); _lon = Longitude2Double(tokens[4], tokens[5]); int quality; if (int.TryParse(tokens[6], out quality)) _quality = (GpsFixQuality)quality; if (float.TryParse(tokens[9], out _altitude)) _altitude = _altitude * 3.28084f; double.TryParse(tokens[8], out _hdop); break; case "GPGLL": //Global Positioning System Fix Data if (tokens.Length < 8) return null; _lat = Latitude2Double(tokens[1], tokens[2]); _lon = Longitude2Double(tokens[3], tokens[4]); break; case "GPRMC": //Recommended minimum specific GPS/Transit data if (tokens.Length < 9) return null; _lat = Latitude2Double(tokens[3], tokens[4]); _lon = Longitude2Double(tokens[5], tokens[6]); double fps = 0; if (double.TryParse(tokens[7], out fps)) _feetPerSecond = Math.Round(fps * 1.68781, 2); //Convert knots to feet per second or "Speed over ground" double dir = 0; if (double.TryParse(tokens[8], out dir)) _heading = dir; //angle from true north that you are traveling or "Course made good" break; case "GPGSV": //Satellites in View if (tokens.Length < 8) return null; int satellitesInView; if (int.TryParse(tokens[3], out satellitesInView)) _satellitesInView = satellitesInView; int signalToNoiseRatio; if (int.TryParse(tokens[7], out signalToNoiseRatio)) _signalToNoiseRatio = signalToNoiseRatio; break; case "PSTI": if (!tokens[1].Equals("030") || tokens.Length < 15) break; //tokens[12] // Mode indicator //‘N’ = Data not valid //‘A’ = Autonomous mode //‘D’ = Differential mode //‘E’ = Estimated(dead reckoning) mode //‘M’ = Manual input mode //‘S’ = Simulator mode //‘F’ = Float RTK.Satellite syst //em used in RTK mode, floating //integers //‘R’ = Real Time Kinematic. System used in RTK mode with fixed //integers double.TryParse(tokens[13], out _rtkAge); double.TryParse(tokens[14], out _rtkRatio); break; default: return null; } if (Math.Abs(_lat) < .1 || Math.Abs(_lon) < .1) return null; } catch (ArgumentOutOfRangeException) { //No fix yet } catch (IndexOutOfRangeException) { //No fix yet } catch (Exception e) { if (_quality != GpsFixQuality.NoFix) { Debug.WriteLine(e); Debug.WriteLine(data); } } var latLon = new LatLon { Lat = _lat, Lon = _lon, Altitude = _altitude, FeetPerSecond = _feetPerSecond, Quality = _quality, SatellitesInView = _satellitesInView, SignalToNoiseRatio = _signalToNoiseRatio, Heading = _heading, DateTime = _dateTime, RtkAge = _rtkAge, RtkRatio = _rtkRatio, Hdop = _hdop }; //if (_quality > GpsFixQuality.NoFix) // Debug.WriteLine($"Lat, Lon : {_lat}, {_lon}, {_quality}, Heading {_heading}, Alt {_altitude}, Sats {_satellitesInView}, SignalToNoise {_signalToNoiseRatio}"); return latLon; }