public void Schedule(int index = 0, int delay = 0)
{
System.Threading.Timer timer = new System.Threading.Timer(
delegate(object state)
{
if (this.commandSchedule.Count <= index)
{
return;
}
int scheduleTime = 0;
if (commandSchedule[index].commandType == DroneCommandType.DRONE_FLYING)
{
scheduleTime = 5000;
}
else if (commandSchedule[index].commandType == DroneCommandType.DRONE_PILOTING)
{
scheduleTime = commandSchedule[index].sustainTime;
}
byte[] packetBytes = CommandPacketManager.SerializeCommand(this.commandSchedule[index]);
byte temp = packetBytes[packetBytes.Length - 1];
packetBytes[packetBytes.Length - 1] = packetBytes[packetBytes.Length - 2];
packetBytes[packetBytes.Length - 2] = temp;
AddACKCheckEvent(delegate()
{
mainForm.UpdateLog("schedule" + index.ToString() + " success");
Schedule(++this.scheduleIndex, scheduleTime + BebopDrone.SCHEDULE_WAIT_TERM);
},
2000,
"Schedule time out.");
server.WriteData(id, packetBytes);
}, null, delay, System.Threading.Timeout.Infinite);
}
// Device have to return ACK if device connect to drone
public void CheckConnection()
{
CommandPacket packet =
CommandPacketManager.CommandPack(DroneCommandType.ACK,
DroneFlyingMode.NONE);
byte[] packetBytes = CommandPacketManager.SerializeCommand(packet);
server.WriteData(id, packetBytes);
}
// Device have to return ACK if device command to drone successfully
public void Land()
{
CommandPacket packet =
CommandPacketManager.CommandPack(DroneCommandType.DRONE_FLYING,
DroneFlyingMode.LAND);
byte[] packetBytes = CommandPacketManager.SerializeCommand(packet);
AddACKCheckEvent(delegate() { mainForm.UpdateLog("Land success"); },
2000,
"Land time out.");
server.WriteData(id, packetBytes);
}
// Device have to return ACK if drone move done successfully
public void PCMD_Move(sbyte roll, sbyte pitch, sbyte yaw, sbyte gaz, short time)
{
// Move
CommandPacket packet =
CommandPacketManager.CommandPack(DroneCommandType.DRONE_PILOTING,
DroneFlyingMode.NONE,
roll, pitch, yaw, gaz, time);
byte[] packetBytes = CommandPacketManager.SerializeCommand(packet);
byte temp = packetBytes[packetBytes.Length - 1];
packetBytes[packetBytes.Length - 1] = packetBytes[packetBytes.Length - 2];
packetBytes[packetBytes.Length - 2] = temp;
AddACKCheckEvent(delegate() { mainForm.UpdateLog("PCMD move Success"); },
2000,
"PCMD move time out.");
server.WriteData(id, packetBytes);
}
public CommandModifyForm(MainForm mainForm, DataGridViewRow dataRow)
{
InitializeComponent();
this.mainForm = mainForm;
this.rollPitchGraphic = this.rollPitchBox.CreateGraphics();
this.gazYawGraphic = this.gazYawBox.CreateGraphics();
this.redPen = new Pen(Color.Red, 2f);
this.bluePen = new Pen(Color.Blue, 2f);
this.rollPitchMouseDown = false;
this.rollPitchPoint = Point.Empty;
this.gazYawMouseDown = false;
this.gazYawPoint = Point.Empty;
this.isOK = false;
this.rollPitchBox.Paint += rollPitchBox_Paint;
this.gazYawBox.Paint += gazYawBox_Paint;
if (dataRow.Cells[GridViewConsts.COMMAND_TYPE].Value.Equals(CommandComboBoxConsts.TAKE_OFF))
{
this.packet = CommandPacketManager.CommandPack(DroneCommandType.DRONE_FLYING, DroneFlyingMode.TAKE_OFF);
this.commandComboBox.SelectedIndex = 0;
}
else if (dataRow.Cells[GridViewConsts.COMMAND_TYPE].Value.Equals(CommandComboBoxConsts.LAND))
{
this.packet = CommandPacketManager.CommandPack(DroneCommandType.DRONE_FLYING, DroneFlyingMode.TAKE_OFF);
this.commandComboBox.SelectedIndex = 1;
}
else if (dataRow.Cells[GridViewConsts.COMMAND_TYPE].Value.Equals(CommandComboBoxConsts.PILOTING))
{
sbyte roll = 0, pitch = 0, yaw = 0, gaz = 0;
short sustainTime = 0;
bool parseResult =
sbyte.TryParse((string)dataRow.Cells[GridViewConsts.ROLL].Value, out roll) &&
sbyte.TryParse((string)dataRow.Cells[GridViewConsts.PITCH].Value, out pitch) &&
sbyte.TryParse((string)dataRow.Cells[GridViewConsts.YAW].Value, out yaw) &&
sbyte.TryParse((string)dataRow.Cells[GridViewConsts.GAZ].Value, out gaz) &&
short.TryParse((string)dataRow.Cells[GridViewConsts.TIME].Value, out sustainTime);
if (!parseResult)
{
mainForm.ShowMessageBox("선택한 행의 데이터가 잘못되었습니다.");
this.Close();
return;
}
this.packet = CommandPacketManager.CommandPack(DroneCommandType.DRONE_PILOTING,
DroneFlyingMode.NONE,
roll,
pitch,
yaw,
gaz,
sustainTime);
this.rollPitchPoint = new Point((int)((((float)roll + 100) / 200) * this.rollPitchBox.Size.Width),
(int)(((-(float)pitch + 100) / 200) * this.rollPitchBox.Size.Height));
this.gazYawPoint = new Point((int)((((float)yaw + 100) / 200) * this.gazYawBox.Size.Width),
(int)(((-(float)gaz + 100) / 200) * this.gazYawBox.Size.Height));
this.rollPitchBox.Invalidate();
this.gazYawBox.Invalidate();
this.rollTextBox.Text = roll.ToString();
this.pitchTextBox.Text = pitch.ToString();
this.yawTextBox.Text = yaw.ToString();
this.gazTextBox.Text = gaz.ToString();
this.timeTextBox.Text = sustainTime.ToString();
}
}
