private PointLatLngAlt GetUserPosition(Drone drone)
{
// project on current heading 1 seconds ahead - testing
var newpos = drone.Location.newpos(drone.Heading, drone.speed);
drone.TargetVelocity = new Vector3(Math.Cos(drone.Heading * Math.PI / 180.0),
Math.Sin(drone.Heading * Math.PI / 180.0), 0).normalized() * drone.speed;
// can still move around, but alt limited
if (newpos.Alt > FenceMaxAlt)
{
newpos.Alt = FenceMaxAlt;
}
// cant move around, as alt is the issue
if (newpos.Alt < FenceMinAlt + 0.5)
{
newpos = drone.Location;
newpos.Alt = FenceMinAlt + 0.5;
}
// close the loop if needed
if (!Fence.Last().Equals(Fence.First()))
{
Fence.Add(Fence.First());
}
if (!PointInPolygon(newpos, Fence))
{
var bear = drone.Location.GetBearing(Fence.First());
newpos = drone.Location.newpos(bear, 0.2);
drone.TargetVelocity = Vector3.Zero;
}
Console.WriteLine("{0} {1} {2}", drone.MavState.sysid, newpos, drone.TargetVelocity.ToString());
return(newpos);
}
private PointLatLngAlt GetUserPosition(Drone drone)
{
PointLatLngAlt newpos;
double yawtarget = drone.MavState.cs.yaw;
var idx = Drones.IndexOf(drone);
if (idx < Controller.Joysticks.Count && Controller.Joysticks[idx] != null)
{
Controller.Joysticks[idx].Poll();
var state = Controller.Joysticks[idx].CurrentJoystickState();
var x = map(state.X, ushort.MinValue, ushort.MaxValue, short.MinValue, short.MaxValue);
var y = map(state.Y, ushort.MinValue, ushort.MaxValue, short.MinValue, short.MaxValue);
var z = map(state.Z, ushort.MinValue, ushort.MaxValue, -1, 1);
var yaw = map(state.Rz, ushort.MinValue, ushort.MaxValue, short.MinValue, short.MaxValue);
// matrix with our current copter yaw
var Matrix = new Matrix3();
Matrix.from_euler(0, 0, drone.MavState.cs.yaw * MathHelper.deg2rad);
// z deadzone
if (Math.Abs(z) < 0.1)
{
z = 0;
}
// rotated vector based on heading.
var vector = new Vector3(x, y, 0);
vector = Matrix * vector;
var vectorbase = new Vector3(short.MaxValue, short.MaxValue, 0);
var lengthscale = vector.length() / vectorbase.length();
var newvector = vector.normalized() * drone.speed;
var direction = Math.Atan2(newvector.x, -newvector.y) * (180 / Math.PI);
// yaw deadzone
if (Math.Abs(x) > 5000)
{
yawtarget = direction;
}
newpos = drone.Location.newpos(yawtarget, drone.speed * lengthscale);
newpos.Alt += z;
drone.TargetVelocity = Vector3.Zero;
//new Vector3(Math.Cos(yawtarget * MathHelper.deg2rad) *
//drone.speed * lengthscale, Math.Sin(yawtarget * MathHelper.deg2rad) *
//drone.speed * lengthscale, 0);
//drone.TargetVelocity = newvector;
//Console.WriteLine("{0} {1} {2} {3} {4} {5}", vector.ToString(), yaw, lengthscale, newvector.ToString(), direction, newpos);
}
else
{
return(PointLatLngAlt.Zero);
}
// can still move around, but alt limited
if (newpos.Alt > FenceMaxAlt)
{
newpos.Alt = FenceMaxAlt;
}
// cant move around, as alt is the issue
if (newpos.Alt < FenceMinAlt + 0.5)
{
newpos.Alt = FenceMinAlt + 0.5;
}
// close the loop if needed
if (!Fence.Last().Equals(Fence.First()))
{
Fence.Add(Fence.First());
}
if (!PointInPolygon(newpos, Fence))
{
var bear = drone.Location.GetBearing(Centroid(Fence));
newpos = drone.Location.newpos(bear, 0.2);
drone.TargetVelocity = Vector3.Zero;
}
//Console.WriteLine("{0} {1} {2}",drone.MavState.sysid, newpos, drone.TargetVelocity.ToString());
return(newpos);
}