private void updateLocationLabel(Locationwp plla)
{
this.BeginInvoke((MethodInvoker) delegate
{
LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt + " " + gotolocation.Tag;
}
);
}
private void updateLocationLabel(Locationwp plla)
{
if (!Instance.IsDisposed)
{
Instance.BeginInvoke((MethodInvoker) delegate
{
Instance.LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt + " " + gotolocation.Tag;
}
);
}
}
void mainloop()
{
DateTime nextsend = DateTime.Now;
StreamWriter sw = new StreamWriter(File.OpenWrite("followmeraw.txt"));
threadrun = true;
while (threadrun)
{
try
{
string line = comPort.ReadLine();
sw.WriteLine(line);
//string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
if (line.StartsWith("$GPGGA") || line.StartsWith("$GNGGA")) //
{
string[] items = line.Trim().Split(',', '*');
if (items[15] != GetChecksum(line.Trim()))
{
Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
continue;
}
if (items[6] == "0")
{
Console.WriteLine("No Fix");
continue;
}
gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);
if (items[3] == "S")
{
gotolocation.Lat *= -1;
}
gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);
if (items[5] == "W")
{
gotolocation.Lng *= -1;
}
gotolocation.Alt = intalt;
gotolocation.Tag = "Sats " + items[7] + " hdop " + items[8];
}
if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 &&
gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
{
nextsend = DateTime.Now.AddMilliseconds(1000 / updaterate);
Console.WriteLine("Sending follow wp " + DateTime.Now.ToString("h:MM:ss") + " " +
gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt);
lastgotolocation = new PointLatLngAlt(gotolocation);
Locationwp gotohere = new Locationwp();
gotohere.id = (ushort)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float)(gotolocation.Alt);
gotohere.lat = (gotolocation.Lat);
gotohere.lng = (gotolocation.Lng);
try
{
updateLocationLabel(gotohere);
}
catch
{
}
if (MainV2.comPort.BaseStream.IsOpen && MainV2.comPort.giveComport == false)
{
try
{
MainV2.comPort.giveComport = true;
MainV2.comPort.setGuidedModeWP(gotohere, false);
MainV2.comPort.giveComport = false;
}
catch
{
MainV2.comPort.giveComport = false;
}
}
}
}
catch
{
System.Threading.Thread.Sleep((int)(1000 / updaterate));
}
}
sw.Close();
}
void mainloop()
{
DateTime nextsend = DateTime.Now;
DateTime nextrallypntupdate = DateTime.Now;
StreamWriter sw = new StreamWriter(Settings.GetUserDataDirectory() + File.OpenWrite("MovingBase.txt"));
threadrun = true;
while (threadrun)
{
try
{
string line = comPort.ReadLine();
sw.WriteLine(line);
//string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
if (line.StartsWith("$GPGGA") || line.StartsWith("$GNGGA")) //
{
string[] items = line.Trim().Split(',', '*');
if (items[items.Length - 1] != GetChecksum(line.Trim()))
{
Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
continue;
}
if (items[6] == "0")
{
Console.WriteLine("No Fix");
continue;
}
gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);
if (items[3] == "S")
{
gotolocation.Lat *= -1;
}
gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0;
gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);
if (items[5] == "W")
{
gotolocation.Lng *= -1;
}
gotolocation.Alt = double.Parse(items[9], CultureInfo.InvariantCulture);
gotolocation.Tag = "Sats " + items[7] + " hdop " + items[8];
}
if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
{
nextsend = DateTime.Now.AddMilliseconds(1000 / updaterate);
Console.WriteLine("new home wp " + DateTime.Now.ToString("h:MM:ss") + " " + gotolocation.Lat +
" " + gotolocation.Lng + " " + gotolocation.Alt);
lastgotolocation = new PointLatLngAlt(gotolocation);
Locationwp gotohere = new Locationwp();
gotohere.id = (ushort)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = (float)(gotolocation.Alt);
gotohere.lat = (gotolocation.Lat);
gotohere.lng = (gotolocation.Lng);
if (chk_relalt.Checked)
{
gotohere.alt = gotohere.alt - (float)MainV2.comPort.MAV.cs.HomeAlt;
}
try
{
updateLocationLabel(gotohere);
}
catch
{
}
MainV2.comPort.MAV.cs.MovingBase = gotolocation;
// plane only
if (updaterallypnt && DateTime.Now > nextrallypntupdate)
{
nextrallypntupdate = DateTime.Now.AddSeconds(5);
try
{
MainV2.comPort.setParam("RALLY_TOTAL", 1);
MainV2.comPort.setRallyPoint(0,
new PointLatLngAlt(gotolocation)
{
Alt =
gotolocation.Alt + double.Parse(Settings.Instance["TXT_DefaultAlt"].ToString())
},
0, 0, 0, (byte)(float)MainV2.comPort.MAV.param["RALLY_TOTAL"]);
MainV2.comPort.setParam("RALLY_TOTAL", 1);
}
catch (Exception ex)
{
Console.WriteLine(ex.ToString());
}
}
}
}
catch
{
System.Threading.Thread.Sleep((int)(1000 / updaterate));
}
}
sw.Close();
}
