private void SerialReader()
{
if (serialThread == true)
return;
serialThread = true;
if (BaseStream.PortName == "TCP")
{
// serialThread = false;
// return;
}
int minbytes = 0;
DateTime speechcustomtime = DateTime.Now;
DateTime heatbeatSend = DateTime.Now;
DateTime linkqualitytime = DateTime.Now;
while (serialThread)
{
try
{
Thread.Sleep(5);
// if not connected or busy, sleep and loop
if (BaseStream != null && !BaseStream.IsOpen || MainCrossData.giveComport == true)
{
System.Threading.Thread.Sleep(100);
continue;
}
// make sure we attenuate the link quality if we dont see any valid packets
if ((DateTime.Now - lastvalidpacket).TotalSeconds > 10)
{
MainCrossData.cs.linkqualitygcs = 0;
}
// attenuate the link qualty over time
if ((DateTime.Now - lastvalidpacket).TotalSeconds >= 1)
{
if (linkqualitytime.Second != DateTime.Now.Second)
{
MainCrossData.cs.linkqualitygcs = (ushort)(MainCrossData.cs.linkqualitygcs * 0.8f);
linkqualitytime = DateTime.Now;
}
}
//// send a hb every seconds from gcs to ap
if (heatbeatSend.Second != DateTime.Now.Second)
{
MAVLink.mavlink_heartbeat_t htb = new MAVLink.mavlink_heartbeat_t()
{
type = (byte)MAVLink.MAV_TYPE.GCS,
autopilot = (byte)MAVLink.MAV_AUTOPILOT.ARDUPILOTMEGA,
mavlink_version = 3
};
sendPacket(htb);
heatbeatSend = DateTime.Now;
byte[] data = MavlinkUtil.StructureToByteArray(htb);
PacketReceived(data);
}
// actauly read the packets
while (BaseStream.BytesToRead > minbytes && MainCrossData.giveComport == false)
{
try
{
byte[] packetIn = readPacket();
PacketReceived(packetIn);
}
catch (Exception e)
{
Console.WriteLine(e.ToString());
}
}
}
catch (Exception e)
{
//////log.Error("Serial Reader fail :" + e.ToString());
try
{
Close();
}
catch { }
}
}
}
private void sendOneHBperSecond()
{
if (sendHBThread == true)
return;
sendHBThread = true;
MAVLink.mavlink_heartbeat_t htb = new MAVLink.mavlink_heartbeat_t()
{
type = (byte)MAVLink.MAV_TYPE.QUADROTOR,
autopilot = (byte)MAVLink.MAV_AUTOPILOT.ARDUPILOTMEGA,
mavlink_version = 3
};
byte[] pabyte = new byte[] { 254, 9, 7, 1, 1, 0, 0, 0, 0, 0, 2, 3, 81, 4, 3, 250, 173 };
//mydata.myLink.ReturnByteArray(
// myServer.Write(pabyte, 0, pabyte.Length);
mydata.myLink.sendPacket(htb);
while (sendHBThread)
{
try
{
Thread.Sleep(950);
if (mydata.myLink.BaseStream.IsOpen)
{
mydata.myLink.sendPacket(htb);
// mydata.myLink.BaseStream.Write(pabyte, 0, pabyte.Length);
}
}
catch (Exception e)
{
//log.Error("HB sending fail :" + e.ToString());
try
{
mydata.myLink.Close();
}
catch { }
}
}
}