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 { } } } }