public override bool Loop()
{
if (!Host.cs.armed)
{
return(true);
}
//get actual position
copter_position.Lat = Host.cs.lat;
copter_position.Lng = Host.cs.lng;
copter_position.Alt = Host.cs.altasl;
if ((last_copter_position == copter_position) && (last_copter_position.Alt == copter_position.Alt))
{
return(true);
}
last_copter_position.Lat = copter_position.Lat;
last_copter_position.Lng = copter_position.Lng;
last_copter_position.Alt = copter_position.Alt;
int closest = get_closest_pole();
PointLatLngAlt closest_pole = poles[closest];
//No closest pole, ignore calculations
if (closest == -1)
{
MainV2.instance.BeginInvoke((MethodInvoker)(() =>
{
lDebugInfo.Text = "";
lXtrack.Text = "║ 000 m";
lDist.Text = "══ 000 m";
lAlt.Text = "alt: 000 m";
lPole.Text = "---";
}));
return(true);
}
var lineStart = closest_pole;
var lineEnd = closest_pole;
if (closest == 0)
{
lineEnd = poles[closest + 1];
}
else
{
lineEnd = poles[closest - 1];
}
var lineBear = lineStart.GetBearing(lineEnd);
double angle = copter_position.GetAngle(closest_pole, lineBear);
var lineDist = lineStart.GetDistance2(lineEnd);
var distToLocation = lineStart.GetDistance2(copter_position);
var bearToLocation = lineStart.GetBearing(copter_position);
if (closest > 0)
{
lineBear = lineBear - 180;
}
var angle1 = bearToLocation - lineBear;
if (angle1 < 0)
{
angle1 += 360;
}
var alongline = Math.Cos(angle1 * MathHelper.deg2rad) * distToLocation;
var dXt2 = Math.Sin(angle * MathHelper.deg2rad) * distToLocation;
MainV2.instance.BeginInvoke((MethodInvoker)(() =>
{
//lDebugInfo.Text = closest.ToString() + " " + lineBearing.ToString() + " " + lineBear.ToString() +" " + dXt2.ToString() + " " + alongline.ToString();
lPole.ForeColor = System.Drawing.Color.Gray;
lAlt.ForeColor = System.Drawing.Color.DarkCyan;
lXtrack.Text = String.Format("║ {0} m", dXt2.ToString("F1").Trim());
lDist.Text = String.Format("══ {0} m", alongline.ToString("F1").Trim());
lAlt.Text = String.Format("alt: {0} m", (copter_position.Alt - closest_pole.Alt).ToString("F1").Trim());
lPole.Text = "Pylon:" + closest_pole.Tag.Trim();
}));
return(true);
}
