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