private PointLatLngAlt GetDronePosition(Drone drone, int a, double distance = 0)
{
var basepos = GetBasePosition();
if (basepos == null || basepos.Lat == 0 || basepos.Lng == 0)
{
return(drone.Location);
}
double sin = 0;
if (zprogress.ContainsKey(drone))
{
sin = Math.Sin(zprogress[drone]);
zprogress[drone] += ZSpeed;
}
else
{
zprogress[drone] = 0;
}
// 14 - 4 = 10
var delta = MaxOffset - MinOffset;
var mid = MinOffset + delta / 2;
// (-1-1)/2 = -0.5 - 0.5 > * delta =
var sinno = (sin / 2.0 * (delta));
if (a == 0)
{
return(basepos.newpos(basepos.Heading + 90, mid + distance + sinno));
}
if (a == 1)
{
return(basepos.newpos(basepos.Heading - 90, mid + distance + sinno));
}
return(drone.Location);
}
public void UpdatePositions()
{
// get current positions and velocitys
foreach (var drone in Drones)
{
if (drone.Location == null)
{
drone.Location = new PointLatLngAlt();
}
drone.Location.Lat = drone.MavState.cs.lat;
drone.Location.Lng = drone.MavState.cs.lng;
drone.Location.Alt = drone.MavState.cs.alt;
if (drone.Velocity == null)
{
drone.Velocity = new Vector3();
}
drone.Velocity.x = drone.MavState.cs.vx;
drone.Velocity.y = drone.MavState.cs.vy;
drone.Velocity.z = drone.MavState.cs.vz;
}
switch (CurrentMode)
{
case Mode.idle:
// request positon at 10hz
foreach (var drone in Drones)
{
var MAV = drone.MavState;
MAV.parent.requestDatastream(MAVLink.MAV_DATA_STREAM.POSITION, 10, MAV.sysid, MAV.compid);
MAV.cs.rateposition = 10;
drone.takeoffdone = false;
}
CurrentMode = Mode.takeoff;
break;
case Mode.takeoff:
int g = Drones.Count;
int aa = -1;
foreach (var drone in Drones)
{
aa++;
g--;
var MAV = drone.MavState;
try
{
// guided mode
if (!MAV.cs.mode.ToLower().Equals("guided"))
{
MAV.parent.setMode(MAV.sysid, MAV.compid, "GUIDED");
}
// arm
if (!MAV.cs.armed)
{
if (!MAV.parent.doARM(MAV.sysid, MAV.compid, true))
{
return;
}
}
}
catch (Exception ex)
{
log.Error(ex);
Loading.ShowLoading("Communication with one of the drones is failing\n" + ex);
return;
}
// get base to drone dist and bearing
var basepos = GetBasePosition();
var dronepos = drone.Location;
if (basepos == null)
{
return;
}
var dist = basepos.GetDistance(dronepos);
var bearing = basepos.GetBearing(dronepos);
// set drone target position
drone.TargetLocation = basepos.newpos(bearing, dist);
drone.TargetLocation.Alt = TakeOffAlt + g * 2;
try
{
// takeoff
if (MAV.cs.alt < drone.TargetLocation.Alt - 0.5)
{
if (MAV.parent.doCommand(MAV.sysid, MAV.compid, MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0,
0, (float)drone.TargetLocation.Alt))
{
drone.takeoffdone = true;
}
}
}
catch (Exception ex)
{
log.Error(ex);
Loading.ShowLoading("Communication with one of the drones is failing\n" + ex);
return;
}
drone.MavState.GuidedMode.x = (int)(drone.TargetLocation.Lat * 1e7);
drone.MavState.GuidedMode.y = (int)(drone.TargetLocation.Lng * 1e7);
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
// wait for takeoff
if (MAV.cs.alt < drone.TargetLocation.Alt - 0.5)
{
Thread.Sleep(100);
// check we are still armed
if (!MAV.cs.armed)
{
return;
}
drone.TargetVelocity = GetBaseVelocity();
//drone.SendPositionVelocity(drone.TargetLocation, drone.TargetVelocity);
drone.MavState.GuidedMode.x = (int)((float)drone.TargetLocation.Lat * 1e7);
drone.MavState.GuidedMode.y = (int)((float)drone.TargetLocation.Lng * 1e7);
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
// alt target must be reached
return;
}
// we should only get here once takeoff alt has been archived by this drone.
drone.TargetLocation = GetDronePosition(drone, aa);
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
drone.MavState.GuidedMode.x = (int)((float)drone.TargetLocation.Lat * 1e7);
drone.MavState.GuidedMode.y = (int)((float)drone.TargetLocation.Lng * 1e7);
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
}
CurrentMode = Mode.alongside;
break;
case Mode.alongside:
int a = 0;
foreach (var drone in Drones)
{
// set drone target position
drone.TargetLocation = GetDronePosition(drone, a);
drone.TargetLocation.Alt = TakeOffAlt;
a++;
drone.TargetVelocity = GetBaseVelocity();
// position control
drone.SendPositionVelocity(drone.TargetLocation, drone.TargetVelocity);
// apply yaw
if (GetBasePosition()?.Heading != null)
{
drone.SendYaw(GetBasePosition().Heading);
}
drone.MavState.GuidedMode.x = (int)((float)drone.TargetLocation.Lat * 1e7);
drone.MavState.GuidedMode.y = (int)((float)drone.TargetLocation.Lng * 1e7);
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
}
break;
case Mode.z:
int d = 0;
foreach (var drone in Drones)
{
// set drone target position
drone.TargetLocation = GetDronePosition(drone, d);
drone.TargetLocation.Alt = TakeOffAlt;
d++;
drone.TargetVelocity = GetBaseVelocity();
// position control
drone.SendPositionVelocity(drone.TargetLocation, drone.TargetVelocity);
// apply yaw
if (GetBasePosition()?.Heading != null)
{
drone.SendYaw(GetBasePosition().Heading);
}
drone.MavState.GuidedMode.x = (int)((float)drone.TargetLocation.Lat * 1e7);
drone.MavState.GuidedMode.y = (int)((float)drone.TargetLocation.Lng * 1e7);
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
}
break;
case Mode.LandAlt:
var e = 0;
foreach (var drone in Drones)
{
drone.TargetLocation = GetDronePosition(drone, e);
drone.TargetLocation.Alt = TakeOffAlt + e * 2;
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
Thread.Sleep(200);
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
e++;
}
// check status
foreach (var drone in Drones)
{
// wait for alt hit
while (Math.Abs(drone.MavState.cs.alt - drone.TargetLocation.Alt) > 0.5)
{
if (!drone.MavState.cs.armed)
{
break;
}
Thread.Sleep(200);
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
}
Thread.Sleep(200);
}
CurrentMode = Mode.Land;
break;
case Mode.Land:
Drone landing = null;
foreach (var drone in Drones)
{
if (drone.MavState.cs.armed)
{
landing = drone;
var basePosition = GetBasePosition();
var basevelocity = GetBaseVelocity();
drone.SendYaw(basePosition.Heading);
drone.TargetLocation = basePosition;
drone.TargetVelocity = basevelocity;
if (drone.Location.GetDistance(drone.TargetLocation) < 2)
{
drone.TargetLocation.Alt = drone.Location.Alt - 2;
}
else
{
// same alt - 0.4
drone.TargetLocation.Alt = drone.Location.Alt - 0.4;
}
drone.SendPositionVelocity(drone.TargetLocation, drone.TargetVelocity);
// one drone at a time
break;
}
}
int f = 0;
foreach (var drone in Drones)
{
if (drone == landing)
{
continue;
}
// set drone target position
drone.TargetLocation = GetDronePosition(drone, f);
f++;
// position control
drone.SendPositionVelocity(drone.TargetLocation, drone.TargetVelocity);
drone.MavState.GuidedMode.x = (int)((float)drone.TargetLocation.Lat * 1e7);
drone.MavState.GuidedMode.y = (int)((float)drone.TargetLocation.Lng * 1e7);
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
}
break;
}
}
