public void UpdatePositions()
{
if (airmaster == null || groundmaster == null)
{
return;
}
// 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 = Math.Cos(drone.MavState.cs.groundcourse * deg2rad) * drone.MavState.cs.groundspeed;
drone.Velocity.y = Math.Sin(drone.MavState.cs.groundcourse * deg2rad) * drone.MavState.cs.groundspeed;
drone.Velocity.z = drone.MavState.cs.verticalspeed;
// set default target as ground reference
drone.TargetVelocity = GroundMasterDrone.Velocity;
}
// generate a new list including the airmaster first
List <Drone> newlist = new List <Drone>();
newlist.Add(AirMasterDrone);
// get list to remove airmaster and groundmaster
List <Drone> newlist2 = new List <Drone>();
newlist2.AddRange(Drones);
newlist2.Remove(AirMasterDrone);
newlist2.Remove(GroundMasterDrone);
// add modified list back
newlist.AddRange(newlist2);
// collision check
foreach (var drone1 in newlist)
{
// ground drone is not part of the check
if (drone1 == GroundMasterDrone)
{
continue;
}
if (!drone1.MavState.cs.armed)
{
continue;
}
foreach (var drone2 in newlist)
{
if (drone1 == drone2)
{
continue;
}
if (drone2 == GroundMasterDrone)
{
continue;
}
if (!drone2.MavState.cs.armed)
{
continue;
}
// check how close they are based on a 1 second projection
if (drone1.ProjectedLocation.GetDistance(drone2.ProjectedLocation) < Seperation / 2)
{
// check if they are heading the same direction
if (((Math.Abs(drone1.Heading - drone2.Heading) + 360) % 360) < 45 && drone1.MavState.cs.groundspeed > 0.5)
{
// they are heading within 45 degrees of each other
// return here to let them settle themselfs, as the target position will be correct
// ie the ground refrence is moving faster than the drones can maintain
Console.WriteLine("1 drone, to close");
return;
}
// check if the are heading are at each other
if (((Math.Abs(drone1.Heading - drone2.Heading) + 360) % 360) > 135)
{
// stop the drones
drone1.SendPositionVelocity(drone1.Location, Vector3.Zero);
drone2.SendPositionVelocity(drone2.Location, Vector3.Zero);
Console.WriteLine("2 stopping drone, to close and heading towards each other");
return;
}
}
}
}
// convert wp path to 0.1m path increments
var path = Path.GeneratePath(AirMasterDrone.MavState);
if (path.Count == 0)
{
return;
}
// update the graph to show location along path
if (pathcount != path.Count)
{
path_to_fly.Clear();
double inc = 0;
path.ForEach(i =>
{
path_to_fly.Add(inc, i.Alt);
inc += 0.1;
});
}
pathcount = path.Count;
foreach (var drone in Drones)
{
var locs = GetLocations(path, drone.Location, 0, 0);
if (locs.Count == 0)
{
continue;
}
drone.PathIndex = path.IndexOf(locs[0]);
}
switch (CurrentMode)
{
case Mode.idle:
CurrentMode = Mode.takeoff;
// request positon at 5hz
foreach (var drone in Drones)
{
var MAV = drone.MavState;
MAV.parent.requestDatastream(MAVLink.MAV_DATA_STREAM.POSITION, 5, MAV.sysid, MAV.compid);
MAV.cs.rateposition = 5;
if (drone != GroundMasterDrone)
{
try
{
// get param
MAV.parent.GetParam(MAV.sysid, MAV.compid, "RTL_ALT");
// set param
MAV.parent.setParam(MAV.sysid, MAV.compid, "RTL_ALT", 0); // cms - rtl at current alt
}
catch
{
}
try
{
// get param
MAV.parent.GetParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL");
// set param to default 100cm/s
MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", 100);
}
catch
{
}
}
}
break;
case Mode.takeoff:
// newposition index
int a = 0;
// calc new positions based on lead and seperation
var lead = Drones.Count * Seperation;
// get locations based on position of the airmaster with a negative lead
var newpositions = GetLocations(path, AirMasterDrone.MavState.cs.HomeLocation, lead, Seperation);
if (newpositions.Count == 0)
{
return;
}
// from here airmaster will be first, all other drones will be in order they will fly in (could sort based on sysid?)
foreach (var drone in newlist)
{
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.ToString());
return;
}
// set drone target position
drone.TargetLocation = newpositions[a];
// setup seperation (0=0,1=1,2=2,3=0)
drone.TargetLocation.Alt += Takeoff_Land_alt_sep * (a % 3);
float takeoffalt = (float)drone.TargetLocation.Alt;
try
{
// takeoff
if (MAV.cs.alt < (takeoffalt - 0.5))
{
if (MAV.parent.doCommand(MAV.sysid, MAV.compid, MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0,
0, takeoffalt))
{
return;
}
}
}
catch (Exception ex)
{
log.Error(ex);
Loading.ShowLoading("Communication with one of the drones is failing\n" + ex.ToString());
return;
}
drone.MavState.GuidedMode.x = 0;
drone.MavState.GuidedMode.y = 0;
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
// wait for takeoff
if (MAV.cs.alt < (takeoffalt - 0.5))
{
System.Threading.Thread.Sleep(100);
// check we are still armed
if (!MAV.cs.armed)
{
return;
}
a++;
// move on to next drone
continue;
}
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
drone.MavState.GuidedMode.x = (float)drone.TargetLocation.Lat;
drone.MavState.GuidedMode.y = (float)drone.TargetLocation.Lng;
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
// check how far off target we are
if (drone.TargetLocation.GetDistance(drone.Location) > Seperation)
{
// only return if this is the third drone without seperation
if (a % 3 == 2)
{
//if we are off target, we have already sent the command to this drone,
//but skip the one behind it untill this one is within the seperation range
return;
}
}
a++;
}
// wait for all to get within seperation distance
foreach (var drone in newlist)
{
// check how far off target we are
if (drone.TargetLocation.GetDistance(drone.Location) > Seperation)
{
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
//if we are off target, we have already sent the command to this drone,
//but skip the one behind it untill this one is within the seperation range
return;
}
}
CurrentMode = Mode.flytouser;
break;
case Mode.flytouser:
// get locations based on position of the airmaster
var newpositions2 = GetLocations(path, AirMasterDrone.Location, Seperation, Seperation);
if (newpositions2.Count == 0)
{
// check if we are within 5m of the end of our flightplan
if (path.Last().GetDistance(AirMasterDrone.Location) < Seperation)
{
CurrentMode = Mode.RTH;
}
return;
}
int b = 0;
foreach (var drone in newlist)
{
if (newpositions2.Count == b)
{
break;
}
// set drone target position
drone.TargetLocation = newpositions2[b];
if (AltInterleave)
{
drone.TargetLocation.Alt += Takeoff_Land_alt_sep * (b % 2);
}
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
drone.MavState.GuidedMode.x = (float)drone.TargetLocation.Lat;
drone.MavState.GuidedMode.y = (float)drone.TargetLocation.Lng;
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
b++;
}
// this is the same as used in the next step, to prevent a jump
var newpositionsfollowuser = GetLocations(path, GroundMasterDrone.Location, Lead, Seperation);
// check how far off target we are
if (newpositionsfollowuser.Count > 0 && AirMasterDrone.Location.GetDistance(newpositionsfollowuser.First()) < Seperation)
{
// update speed as we are changing to a high dynamic mode
foreach (var drone in newlist)
{
if (drone == GroundMasterDrone)
{
continue;
}
var MAV = drone.MavState;
// update to faster speed
MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", (float)WPNAV_ACCEL * 100.0f);
}
//if we are off target, we have already sent the command to this drone,
//but skip the one behind it untill this one is within the seperation range
CurrentMode = Mode.followuser;
return;
}
break;
case Mode.followuser:
// calc new positions based on lead and seperation
List <PointLatLngAlt> newpositions3 = new List <PointLatLngAlt>();
if (V)
{
newpositions3 = GetLocationsV(path, GroundMasterDrone.Location, Lead, Seperation);
}
else
{
newpositions3 = GetLocations(path, GroundMasterDrone.Location, Lead, Seperation);
}
if (newpositions3.Count == 0)
{
return;
}
// check if the user is offpath
var wps = new List <PointLatLngAlt>();
AirMasterDrone.MavState.wps.Values.ForEach(i => { wps.Add(new PointLatLngAlt((Locationwp)i)); });
if (GetOffPathDistance(wps, GroundMasterDrone.Location) > OffPathTrigger)
{
CurrentMode = Mode.RTH;
}
int c = 0;
// send position and velocity
foreach (var drone in newlist)
{
if (drone.MavState == groundmaster)
{
continue;
}
if (c > (newpositions3.Count - 1))
{
break;
}
drone.TargetLocation = newpositions3[c];
if (AltInterleave)
{
drone.TargetLocation.Alt += Takeoff_Land_alt_sep * (c % 2);
}
// spline control
drone.SendPositionVelocity(drone.TargetLocation, drone.TargetVelocity / 3);
drone.MavState.GuidedMode.x = (float)drone.TargetLocation.Lat;
drone.MavState.GuidedMode.y = (float)drone.TargetLocation.Lng;
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
// vel only
//drone.SendVelocity(drone.TargetVelocity);
// check how far off target we are
if (drone.TargetLocation.GetDistance(drone.Location) > Seperation * 2)
{
//if we are off target, we have already sent the command to this drone,
//but skip the one behind it untill this one is within the seperation range
//break;
}
c++;
}
break;
case Mode.RTH:
// get locations based on position of the airmaster
var newpositions4 = GetLocations(path, AirMasterDrone.Location, Seperation, Seperation);
if (newpositions4.Count == 0)
{
if (AirMasterDrone.Location.GetDistance(path.Last()) < Seperation)
{
CurrentMode = Mode.LandAlt;
}
return;
}
int d = 0;
foreach (var drone in newlist)
{
if (d > (newpositions4.Count - 1))
{
break;
}
try
{
var MAV = drone.MavState;
// set param to default 100cm/s
MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", 100);
}
catch
{
}
// set drone target position
drone.TargetLocation = newpositions4[d];
// used in next step
double prevalt = drone.TargetLocation.Alt;
if (AltInterleave)
{
drone.TargetLocation.Alt += Takeoff_Land_alt_sep * (d % 2);
}
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
drone.MavState.GuidedMode.x = (float)drone.TargetLocation.Lat;
drone.MavState.GuidedMode.y = (float)drone.TargetLocation.Lng;
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
// used for next step
drone.TargetLocation.Alt = prevalt;
d++;
}
break;
case Mode.LandAlt:
int e = 0;
foreach (var drone in newlist)
{
drone.TargetLocation.Alt += Takeoff_Land_alt_sep * e;
try
{
var MAV = drone.MavState;
// set param to default 100cm/s
MAV.parent.setParam(MAV.sysid, MAV.compid, "WPNAV_ACCEL", 100);
}
catch
{
}
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
System.Threading.Thread.Sleep(200);
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
e++;
}
// check status
foreach (var drone in newlist)
{
// wait for alt hit
while (drone.MavState.cs.alt < (drone.TargetLocation.Alt - 0.5))
{
if (!drone.MavState.cs.armed)
{
break;
}
System.Threading.Thread.Sleep(200);
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
}
System.Threading.Thread.Sleep(200);
log.Info(drone.MavState.sysid + " " + drone.MavState.cs.alt + " at alt " + drone.TargetLocation.Alt);
// set mode rtl
drone.MavState.parent.setMode(drone.MavState.sysid, drone.MavState.compid, "RTL");
}
CurrentMode = Mode.Land;
break;
case Mode.Land:
Drone closest = new Drone()
{
Location = PointLatLngAlt.Zero
};
var lastpnt = path.Last();
foreach (var drone in newlist)
{
if (!drone.MavState.cs.armed)
{
continue;
}
// low flying filter, move onto next drone
if (drone.MavState.cs.alt < lastpnt.Alt - 1)
{
continue;
}
if (AirMasterDrone.MavState.cs.HomeLocation.GetDistance(drone.Location) < AirMasterDrone.MavState.cs.HomeLocation.GetDistance(closest.Location))
{
closest = drone;
}
}
if (closest.MavState != null && !closest.MavState.cs.mode.ToLower().Equals("rtl"))
{
closest.MavState.parent.setMode(closest.MavState.sysid, closest.MavState.compid, "RTL");
}
break;
}
}
public void UpdatePositions()
{
if (airmaster == null || groundmaster == null)
return;
// 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 = Math.Cos(drone.MavState.cs.groundcourse*deg2rad)*drone.MavState.cs.groundspeed;
drone.Velocity.y = Math.Sin(drone.MavState.cs.groundcourse*deg2rad)*drone.MavState.cs.groundspeed;
drone.Velocity.z = drone.MavState.cs.verticalspeed;
// set default target as ground reference
drone.TargetVelocity = GroundMasterDrone.Velocity;
}
// generate a new list including the airmaster first
List<Drone> newlist = new List<Drone>();
newlist.Add(AirMasterDrone);
// get list to remove airmaster and groundmaster
List<Drone> newlist2 = new List<Drone>();
newlist2.AddRange(Drones);
newlist2.Remove(AirMasterDrone);
newlist2.Remove(GroundMasterDrone);
// add modified list back
newlist.AddRange(newlist2);
// collision check
foreach (var drone1 in newlist)
{
// ground drone is not part of the check
if (drone1 == GroundMasterDrone)
continue;
if (!drone1.MavState.cs.armed)
continue;
foreach (var drone2 in newlist)
{
if(drone1 == drone2)
continue;
if (drone2 == GroundMasterDrone)
continue;
if(!drone2.MavState.cs.armed)
continue;
// check how close they are based on a 1 second projection
if (drone1.ProjectedLocation.GetDistance(drone2.ProjectedLocation) < Seperation/2)
{
// check if they are heading the same direction
if (((Math.Abs(drone1.Heading - drone2.Heading)+360)%360) < 45 && drone1.MavState.cs.groundspeed > 0.5)
{
// they are heading within 45 degrees of each other
// return here to let them settle themselfs, as the target position will be correct
// ie the ground refrence is moving faster than the drones can maintain
Console.WriteLine("1 drone, to close");
return;
}
// check if the are heading are at each other
if (((Math.Abs(drone1.Heading - drone2.Heading)+360)%360) > 135 )
{
// stop the drones
drone1.SendPositionVelocity(drone1.Location, Vector3.Zero);
drone2.SendPositionVelocity(drone2.Location, Vector3.Zero);
Console.WriteLine("2 stopping drone, to close and heading towards each other");
return;
}
}
}
}
// convert wp path to 0.1m path increments
var path = Path.GeneratePath(AirMasterDrone.MavState);
if (path.Count == 0)
return;
// update the graph to show location along path
if (pathcount != path.Count)
{
path_to_fly.Clear();
double inc = 0;
path.ForEach(i =>
{
path_to_fly.Add(inc, i.Alt);
inc += 0.1;
});
}
pathcount = path.Count;
foreach (var drone in Drones)
{
var locs = GetLocations(path, drone.Location, 0, 0);
if (locs.Count == 0)
continue;
drone.PathIndex = path.IndexOf(locs[0]);
}
switch (CurrentMode)
{
case Mode.idle:
CurrentMode = Mode.takeoff;
// request positon at 5hz
foreach (var drone in Drones)
{
var MAV = drone.MavState;
MAV.parent.requestDatastream(MAVLink.MAV_DATA_STREAM.POSITION, 5, MAV.sysid, MAV.compid);
MAV.cs.rateposition = 5;
if (drone != GroundMasterDrone)
{
try
{
// get param
MAV.parent.GetParam(MAV.sysid, MAV.compid, "RTL_ALT");
// set param
MAV.parent.setParam(MAV.sysid, MAV.compid, "RTL_ALT", 0); // cms - rtl at current alt
}
catch
{
}
}
}
break;
case Mode.takeoff:
// newposition index
int a = 0;
// calc new positions based on lead and seperation
var lead = Drones.Count * Seperation;
// get locations based on position of the airmaster with a negative lead
var newpositions = GetLocations(path, AirMasterDrone.MavState.cs.HomeLocation, lead, Seperation);
if (newpositions.Count == 0)
return;
// from here airmaster will be first, all other drones will be in order they will fly in (could sort based on sysid?)
foreach (var drone in newlist)
{
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.ToString());
return;
}
// set drone target position
drone.TargetLocation = newpositions[a];
// setup seperation (0=0,1=1,2=2,3=0)
drone.TargetLocation.Alt += Takeoff_Land_alt_sep * (a % 3);
float takeoffalt = (float)drone.TargetLocation.Alt;
try
{
// takeoff
if (MAV.cs.alt < (takeoffalt - 0.5))
if (MAV.parent.doCommand(MAV.sysid, MAV.compid, MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0,
0, takeoffalt))
return;
}
catch (Exception ex)
{
log.Error(ex);
Loading.ShowLoading("Communication with one of the drones is failing\n" + ex.ToString());
return;
}
drone.MavState.GuidedMode.x = 0;
drone.MavState.GuidedMode.y = 0;
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
// wait for takeoff
if (MAV.cs.alt < (takeoffalt - 0.5))
{
System.Threading.Thread.Sleep(100);
// check we are still armed
if (!MAV.cs.armed)
return;
a++;
// move on to next drone
continue;
}
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
drone.MavState.GuidedMode.x = (float)drone.TargetLocation.Lat;
drone.MavState.GuidedMode.y = (float)drone.TargetLocation.Lng;
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
// check how far off target we are
if (drone.TargetLocation.GetDistance(drone.Location) > Seperation)
{
// only return if this is the third drone without seperation
if (a%3 == 2)
{
//if we are off target, we have already sent the command to this drone,
//but skip the one behind it untill this one is within the seperation range
return;
}
}
a++;
}
// wait for all to get within seperation distance
foreach (var drone in newlist)
{
// check how far off target we are
if (drone.TargetLocation.GetDistance(drone.Location) > Seperation)
{
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
//if we are off target, we have already sent the command to this drone,
//but skip the one behind it untill this one is within the seperation range
return;
}
}
CurrentMode = Mode.flytouser;
break;
case Mode.flytouser:
// get locations based on position of the airmaster
var newpositions2 = GetLocations(path, AirMasterDrone.Location, Seperation, Seperation);
if (newpositions2.Count == 0)
{
// check if we are within 5m of the end of our flightplan
if (path.Last().GetDistance(AirMasterDrone.Location) < Seperation)
CurrentMode = Mode.RTH;
return;
}
int b = 0;
foreach (var drone in newlist)
{
if (newpositions2.Count == b)
break;
// set drone target position
drone.TargetLocation = newpositions2[b];
if (AltInterleave)
drone.TargetLocation.Alt += Takeoff_Land_alt_sep * (b % 2);
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
drone.MavState.GuidedMode.x = (float)drone.TargetLocation.Lat;
drone.MavState.GuidedMode.y = (float)drone.TargetLocation.Lng;
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
b++;
}
// this is the same as used in the next step, to prevent a jump
var newpositionsfollowuser = GetLocations(path, GroundMasterDrone.Location, Lead, Seperation);
// check how far off target we are
if (newpositionsfollowuser.Count > 0 && AirMasterDrone.Location.GetDistance(newpositionsfollowuser.First()) < Seperation)
{
//if we are off target, we have already sent the command to this drone,
//but skip the one behind it untill this one is within the seperation range
CurrentMode = Mode.followuser;
return;
}
break;
case Mode.followuser:
// calc new positions based on lead and seperation
List<PointLatLngAlt> newpositions3 = new List<PointLatLngAlt>();
if (V)
{
newpositions3 = GetLocationsV(path, GroundMasterDrone.Location, Lead, Seperation);
}
else
{
newpositions3 = GetLocations(path, GroundMasterDrone.Location, Lead, Seperation);
}
if (newpositions3.Count == 0)
return;
// check if the user is offpath
var wps = new List<PointLatLngAlt>();
AirMasterDrone.MavState.wps.Values.ForEach(i => { wps.Add(new PointLatLngAlt((Locationwp)i)); });
if (GetOffPathDistance(wps, GroundMasterDrone.Location) > OffPathTrigger)
{
CurrentMode = Mode.RTH;
}
int c = 0;
// send position and velocity
foreach (var drone in newlist)
{
if (drone.MavState == groundmaster)
continue;
if (c > (newpositions3.Count - 1))
break;
drone.TargetLocation = newpositions3[c];
if (AltInterleave)
drone.TargetLocation.Alt += Takeoff_Land_alt_sep * (c % 2);
// spline control
drone.SendPositionVelocity(drone.TargetLocation, drone.TargetVelocity / 3);
drone.MavState.GuidedMode.x = (float)drone.TargetLocation.Lat;
drone.MavState.GuidedMode.y = (float)drone.TargetLocation.Lng;
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
// vel only
//drone.SendVelocity(drone.TargetVelocity);
// check how far off target we are
if (drone.TargetLocation.GetDistance(drone.Location) > Seperation * 2)
{
//if we are off target, we have already sent the command to this drone,
//but skip the one behind it untill this one is within the seperation range
//break;
}
c++;
}
break;
case Mode.RTH:
// get locations based on position of the airmaster
var newpositions4 = GetLocations(path, AirMasterDrone.Location, Seperation, Seperation);
if (newpositions4.Count == 0)
{
if (AirMasterDrone.Location.GetDistance(path.Last()) < Seperation)
CurrentMode = Mode.LandAlt;
return;
}
int d = 0;
foreach (var drone in newlist)
{
if (d > (newpositions4.Count - 1))
break;
// set drone target position
drone.TargetLocation = newpositions4[d];
// used in next step
double prevalt = drone.TargetLocation.Alt;
if (AltInterleave)
drone.TargetLocation.Alt += Takeoff_Land_alt_sep * (d % 2);
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
drone.MavState.GuidedMode.x = (float)drone.TargetLocation.Lat;
drone.MavState.GuidedMode.y = (float)drone.TargetLocation.Lng;
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
// used for next step
drone.TargetLocation.Alt = prevalt;
d++;
}
break;
case Mode.LandAlt:
int e = 0;
foreach (var drone in newlist)
{
drone.TargetLocation.Alt += Takeoff_Land_alt_sep * e;
// position control
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
drone.MavState.GuidedMode.z = (float)drone.TargetLocation.Alt;
System.Threading.Thread.Sleep(200);
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
e++;
}
// check status
foreach (var drone in newlist)
{
// wait for alt hit
while (drone.MavState.cs.alt < (drone.TargetLocation.Alt-0.5))
{
if (!drone.MavState.cs.armed)
break;
System.Threading.Thread.Sleep(200);
drone.SendPositionVelocity(drone.TargetLocation, Vector3.Zero);
}
System.Threading.Thread.Sleep(200);
log.Info(drone.MavState.sysid + " " + drone.MavState.cs.alt + " at alt " + drone.TargetLocation.Alt);
// set mode rtl
drone.MavState.parent.setMode(drone.MavState.sysid, drone.MavState.compid, "RTL");
}
CurrentMode = Mode.Land;
break;
case Mode.Land:
Drone closest = new Drone() {Location = PointLatLngAlt.Zero};
var lastpnt = path.Last();
foreach (var drone in newlist)
{
if(!drone.MavState.cs.armed)
continue;
// low flying filter, move onto next drone
if (drone.MavState.cs.alt < lastpnt.Alt-1)
continue;
if (AirMasterDrone.MavState.cs.HomeLocation.GetDistance(drone.Location) < AirMasterDrone.MavState.cs.HomeLocation.GetDistance(closest.Location))
{
closest = drone;
}
}
if(closest.MavState!=null && !closest.MavState.cs.mode.ToLower().Equals("rtl"))
closest.MavState.parent.setMode(closest.MavState.sysid, closest.MavState.compid, "RTL");
break;
}
}