public static PatrolElement[] Read(string filename)
{
try
{
var fstream = File.Open(filename, FileMode.Open);
var list = null as PatrolElement[];
using (var reader = new BinaryReader(fstream))
{
var count = reader.ReadInt32();
list = new PatrolElement[count];
for (var i = 0; i < count; i++)
{
var elapsed_time = reader.ReadInt32();
var Pcmd = new Pcmd(reader.ReadInt32(), reader.ReadInt32(), reader.ReadInt32(), reader.ReadInt32(), reader.ReadInt32());
list[i] = new PatrolElement(elapsed_time, Pcmd);
}
}
fstream.Close();
return(list);
}
catch (Exception e)
{
return(null);
}
}
public void Stop()
{
if (this.IsEnabled() == false)
{
return;
}
this.Write(new Pcmd());
using (var writer = new BinaryWriter(this._fstream))
{
writer.Write(this.PatrolElementQueue.Count);
foreach (var element in this.PatrolElementQueue)
{
writer.Write(element.ToBytes());
}
}
this.PatrolElementQueue.Clear();
this._last_Pcmd = null;
this._stopwatch.Reset();
this._fstream.Close();
this._fstream = null;
}
public bool Write(Pcmd Pcmd)
{
if (this.IsEnabled() == false)
{
return(false);
}
if (this._last_Pcmd != null && this._last_Pcmd.Equals(Pcmd))
{
return(false);
}
if (this._last_Pcmd != null)
{
this._stopwatch.Stop();
var element = new PatrolElement((int)this._stopwatch.ElapsedMilliseconds, this._last_Pcmd);
this.PatrolElementQueue.Enqueue(element);
}
this._last_Pcmd = new Pcmd(Pcmd);
this._stopwatch.Reset();
this._stopwatch.Start();
return(true);
}
public Pcmd Bebop2_OnRequestPcmd(Bebop2 bebop)
{
this._mutex.WaitOne();
var pcmd = new Pcmd(this._pcmd);
this._mutex.ReleaseMutex();
return(pcmd);
}
public void updatePcmdUI(Pcmd pcmd)
{
if (this.Root == null)
{
return;
}
try
{
this.bunifuImageButton5.Invoke(new MethodInvoker(delegate()
{
this.bunifuImageButton5.BackColor = pcmd.pitch > 0 ? Color.Salmon : Color.DarkGray;
}));
this.bunifuImageButton6.Invoke(new MethodInvoker(delegate()
{
this.bunifuImageButton6.BackColor = pcmd.pitch < 0 ? Color.Salmon : Color.DarkGray;
}));
this.bunifuImageButton7.Invoke(new MethodInvoker(delegate()
{
this.bunifuImageButton7.BackColor = pcmd.roll < 0 ? Color.Salmon : Color.DarkGray;
}));
this.bunifuImageButton8.Invoke(new MethodInvoker(delegate()
{
this.bunifuImageButton8.BackColor = pcmd.roll > 0 ? Color.Salmon : Color.DarkGray;
}));
this.bunifuImageButton1.Invoke(new MethodInvoker(delegate()
{
this.bunifuImageButton1.BackColor = pcmd.gaz > 0 ? Color.Salmon : Color.DarkGray;
}));
this.bunifuImageButton2.Invoke(new MethodInvoker(delegate()
{
this.bunifuImageButton2.BackColor = pcmd.gaz < 0 ? Color.Salmon : Color.DarkGray;
}));
this.bunifuImageButton3.Invoke(new MethodInvoker(delegate()
{
this.bunifuImageButton3.BackColor = pcmd.yaw > 0 ? Color.Salmon : Color.DarkGray;
}));
this.bunifuImageButton4.Invoke(new MethodInvoker(delegate()
{
this.bunifuImageButton4.BackColor = pcmd.yaw < 0 ? Color.Salmon : Color.DarkGray;
}));
}
catch (Exception)
{ }
}
private void runConsumer(BlockingCollection <iCommand> queue, bool heartbeat)
{
LOGGER.Info("Creating a specific command queue consumer...");
new Thread(() =>
{
try
{
using (var sumoSocket = new UdpClient())
{
while (sendCommands)
{
try
{
iCommand command = queue.Take();
if (command != null)
{
int cnt = getNextSequenceNumber(command);
byte[] packet = command.getBytes(cnt);
sumoSocket.Send(packet, packet.Length, SumoRemote);
LOGGER.Info(String.Format("Sending command: {0}", command));
Thread.Sleep(command.waitingTime());
}
else // send "null" PCmd (0,0) = ("go by 0 speed") when queue is empty
{
if (heartbeat)
{
Pcmd nullCommand = Pcmd.pcmd(0, 0, maxWaitingTime);
byte[] nullMovePacket = nullCommand.getBytes(getNextSequenceNumber(nullCommand));
//LOGGER.Debug("empty queue, sending null command '{}' with packet {}", nullCommand, convertAndCutPacket(nullMovePacket, false));
sumoSocket.Send(nullMovePacket, nullMovePacket.Length, SumoRemote);
}
Thread.Sleep(maxWaitingTime);
//MILLISECONDS.sleep(maxWaitingTime);
}
}
catch (Exception e)
{
throw new CommandException("Got interrupted while taking a command", e);
}
}
}
}
catch (IOException)
{
LOGGER.Warn("Error occurred while sending packets to the drone.");
}
}).Start();
}
/*
* public DroneController send(Command command)
* {
* this.droneConnection.sendCommand(command);
* return this;
* }
*/
public DroneController pcmd(int speed, int degree)
{
this.droneConnection.sendCommand(Pcmd.pcmd(speed, degree));
return(this);
}
public PatrolElement(int elapsedTime, Pcmd command)
{
this.ElapsedTime = elapsedTime;
this.Command = new Pcmd(command);
}
private void PatrolReader_OnChanged(Pcmd pcmd)
{
this._pcmd = new Pcmd(pcmd);
this.defaultView.sideExpandedBar.droneTab.updatePcmdUI(this._pcmd);
}
