public static PointLatLngAlt ProjectPoint(MAVLinkInterface comPort)
{
GimbalPoint.comPort = comPort;
comPort.GetMountStatus();
// this should be looking at rc_channel function
yawchannel = (int)(float)comPort.MAV.param["MNT_RC_IN_PAN"];
pitchchannel = (int)(float)comPort.MAV.param["MNT_RC_IN_TILT"];
rollchannel = (int)(float)comPort.MAV.param["MNT_RC_IN_ROLL"];
//if (!comPort.BaseStream.IsOpen)
// return PointLatLngAlt.Zero;
PointLatLngAlt currentlocation = new PointLatLngAlt(comPort.MAV.cs.lat, comPort.MAV.cs.lng);
double yawangle = comPort.MAV.cs.campointc;
double rollangle = comPort.MAV.cs.campointb;
double pitchangle = comPort.MAV.cs.campointa;
//
if ((double)comPort.MAV.param["MNT_TYPE"] == 4)
{
yawangle = comPort.MAVlist[comPort.sysidcurrent, 67].cs.yaw;
rollangle = comPort.MAVlist[comPort.sysidcurrent, 67].cs.roll;
pitchangle = comPort.MAVlist[comPort.sysidcurrent, 67].cs.pitch;
}
if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0)
{
yawangle = ConvertPwmtoAngle(axis.yaw);
//rollangle = ConvertPwmtoAngle(axis.roll);
pitchangle = ConvertPwmtoAngle(axis.pitch) + comPort.MAV.cs.pitch;
pitchangle -= Math.Sin(yawangle * MathHelper.deg2rad) * comPort.MAV.cs.roll;
}
// tan (o/a)
// todo account for ground elevation change.
int distout = 10;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
//dist = Math.Tan((90 + pitchangle) * MathHelper.deg2rad) * (comPort.MAV.cs.alt);
while (distout < 1000)
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt newposdist = currentlocation.newpos(yawangle + comPort.MAV.cs.yaw, distout);
newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;
// get another point 50 infront
PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + comPort.MAV.cs.yaw, distout + 50);
newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;
// get the flat terrain distance out - at 0 alt
double distflat = Math.Tan((90 + pitchangle) * MathHelper.deg2rad) * (comPort.MAV.cs.altasl);
// x is dist from plane, y is alt
var newpoint = FindLineIntersection(start1: new Point(0, comPort.MAV.cs.altasl),
new Point((float)distflat, 0),
new Point((float)distout, (float)newposdist.Alt),
new Point((float)distout + 50, (float)newposdist2.Alt));
if (newpoint.X != 0)
{
newpos = newposdist2;
break;
}
distout += 50;
}
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
//PointLatLngAlt newpos = currentlocation.newpos( yawangle + comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return(newpos);
}
