private void BUT_runstep_Click(object sender, EventArgs e)
{
if (step >= workingSequence.Steps.Count)
{
return;
}
// get step layout name
var layoutname = workingSequence.Steps[step];
controller.Start();
// get first homepos
if (startpos == PointLatLngAlt.Zero)
{
startpos = controller.DG.Drones[0].MavState.cs.Location;
}
if (step == 0)
{
controller.DG.Drones.All(a =>
{
if (!a.MavState.cs.mode.ToLower().Equals("guided"))
{
a.MavState.parent.setMode(a.MavState.sysid, a.MavState.compid, "GUIDED");
}
if (a.MavState.cs.armed != true)
{
a.MavState.parent.doARM(a.MavState.sysid, a.MavState.compid, true);
}
a.MavState.parent.doCommand(a.MavState.sysid, a.MavState.compid, MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, 2, false);
return(true);
});
Thread.Sleep(3000);
}
label1.Text = String.Format("{1} : {0}", step, layoutname);
// get the layout
var layout = workingSequence.Layouts.Find(a => a.Id == layoutname);
foreach (var vector3 in layout.Offset)
{
var drone = controller.DG.Drones.Find(a => a.MavState.sysid == vector3.Key);
var newpos = startpos.gps_offset(vector3.Value.x, vector3.Value.y);
newpos.Alt = vector3.Value.z;
if (drone != null)
{
drone.SendPositionVelocity(newpos, Vector3.Zero);
}
}
step++;
}
private void BUT_runstep_Click(object sender, EventArgs e)
{
if (step >= workingSequence.Steps.Count)
{
return;
}
// get step layout name
var layoutname = workingSequence.Steps[step];
controller.Start();
// get first homepos
if (startpos == PointLatLngAlt.Zero)
{
startpos = controller.DG.Drones[0].MavState.cs.Location;
}
label1.Text = String.Format("{1} : {0}", step, layoutname);
// get the layout
var layout = workingSequence.Layouts.Find(a => a.Id == layoutname);
foreach (var vector3 in layout.Offset)
{
var drone = controller.DG.Drones.Find(a => a.MavState.sysid == vector3.Key);
var newpos = startpos.gps_offset(vector3.Value.x, vector3.Value.y);
newpos.Alt = vector3.Value.z;
if (drone != null)
{
drone.SendPositionVelocity(newpos, Vector3.Zero);
}
}
step++;
}
