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++; }