private void SerialReader()
{
if (serialThread == true)
return;
serialThread = true;
SerialThreadrunner.Reset();
int minbytes = 0;
int altwarningmax = 0;
bool armedstatus = false;
string lastmessagehigh = "";
DateTime speechcustomtime = DateTime.Now;
DateTime speechlowspeedtime = DateTime.Now;
DateTime linkqualitytime = DateTime.Now;
while (serialThread)
{
try
{
Thread.Sleep(1); // was 5
try
{
if (GCSViews.Terminal.comPort is MAVLinkSerialPort)
{
}
else
{
if (GCSViews.Terminal.comPort != null && GCSViews.Terminal.comPort.IsOpen)
continue;
}
}
catch (Exception ex)
{
log.Error(ex);
}
// update connect/disconnect button and info stats
try
{
UpdateConnectIcon();
}
catch (Exception ex)
{
log.Error(ex);
}
// 30 seconds interval speech options
if (speechEnable && speechEngine != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 &&
(MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
if (MainV2.speechEngine.IsReady)
{
if (Settings.Instance.GetBoolean("speechcustomenabled"))
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(""+ Settings.Instance["speechcustom"]));
}
speechcustomtime = DateTime.Now;
}
// speech for battery alerts
//speechbatteryvolt
float warnvolt = Settings.Instance.GetFloat("speechbatteryvolt");
float warnpercent = Settings.Instance.GetFloat("speechbatterypercent");
if (Settings.Instance.GetBoolean("speechbatteryenabled") == true &&
MainV2.comPort.MAV.cs.battery_voltage <= warnvolt &&
MainV2.comPort.MAV.cs.battery_voltage >= 5.0)
{
if (MainV2.speechEngine.IsReady)
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(""+ Settings.Instance["speechbattery"]));
}
}
else if (Settings.Instance.GetBoolean("speechbatteryenabled") == true &&
(MainV2.comPort.MAV.cs.battery_remaining) < warnpercent &&
MainV2.comPort.MAV.cs.battery_voltage >= 5.0 &&
MainV2.comPort.MAV.cs.battery_remaining != 0.0)
{
if (MainV2.speechEngine.IsReady)
{
MainV2.speechEngine.SpeakAsync(
Common.speechConversion("" + Settings.Instance["speechbattery"]));
}
}
}
// speech for airspeed alerts
if (speechEnable && speechEngine != null && (DateTime.Now - speechlowspeedtime).TotalSeconds > 10 &&
(MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
if (Settings.Instance.GetBoolean("speechlowspeedenabled") == true && MainV2.comPort.MAV.cs.armed)
{
float warngroundspeed = Settings.Instance.GetFloat("speechlowgroundspeedtrigger");
float warnairspeed = Settings.Instance.GetFloat("speechlowairspeedtrigger");
if (MainV2.comPort.MAV.cs.airspeed < warnairspeed)
{
if (MainV2.speechEngine.IsReady)
{
MainV2.speechEngine.SpeakAsync(
Common.speechConversion(""+ Settings.Instance["speechlowairspeed"]));
speechlowspeedtime = DateTime.Now;
}
}
else if (MainV2.comPort.MAV.cs.groundspeed < warngroundspeed)
{
if (MainV2.speechEngine.IsReady)
{
MainV2.speechEngine.SpeakAsync(
Common.speechConversion(""+ Settings.Instance["speechlowgroundspeed"]));
speechlowspeedtime = DateTime.Now;
}
}
else
{
speechlowspeedtime = DateTime.Now;
}
}
}
// speech altitude warning - message high warning
if (speechEnable && speechEngine != null &&
(MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
float warnalt = float.MaxValue;
if (Settings.Instance.ContainsKey("speechaltheight"))
{
warnalt = Settings.Instance.GetFloat("speechaltheight");
}
try
{
altwarningmax = (int) Math.Max(MainV2.comPort.MAV.cs.alt, altwarningmax);
if (Settings.Instance.GetBoolean("speechaltenabled") == true && MainV2.comPort.MAV.cs.alt != 0.00 &&
(MainV2.comPort.MAV.cs.alt <= warnalt) && MainV2.comPort.MAV.cs.armed)
{
if (altwarningmax > warnalt)
{
if (MainV2.speechEngine.IsReady)
MainV2.speechEngine.SpeakAsync(
Common.speechConversion(""+Settings.Instance["speechalt"]));
}
}
}
catch
{
} // silent fail
try
{
// say the latest high priority message
if (MainV2.speechEngine.IsReady &&
lastmessagehigh != MainV2.comPort.MAV.cs.messageHigh && MainV2.comPort.MAV.cs.messageHigh != null)
{
if (!MainV2.comPort.MAV.cs.messageHigh.StartsWith("PX4v2 "))
{
MainV2.speechEngine.SpeakAsync(MainV2.comPort.MAV.cs.messageHigh);
lastmessagehigh = MainV2.comPort.MAV.cs.messageHigh;
}
}
}
catch
{
}
}
// not doing anything
if (!MainV2.comPort.logreadmode && !comPort.BaseStream.IsOpen)
{
altwarningmax = 0;
}
// attenuate the link qualty over time
if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds >= 1)
{
if (linkqualitytime.Second != DateTime.Now.Second)
{
MainV2.comPort.MAV.cs.linkqualitygcs = (ushort) (MainV2.comPort.MAV.cs.linkqualitygcs*0.8f);
linkqualitytime = DateTime.Now;
// force redraw if there are no other packets are being read
GCSViews.FlightData.myhud.Invalidate();
}
}
// data loss warning - wait min of 10 seconds, ignore first 30 seconds of connect, repeat at 5 seconds interval
if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds > 10
&& (DateTime.Now - connecttime).TotalSeconds > 30
&& (DateTime.Now - nodatawarning).TotalSeconds > 5
&& (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)
&& MainV2.comPort.MAV.cs.armed)
{
if (speechEnable && speechEngine != null)
{
if (MainV2.speechEngine.IsReady)
{
MainV2.speechEngine.SpeakAsync("WARNING No Data for " +
(int)
(DateTime.Now - MainV2.comPort.MAV.lastvalidpacket)
.TotalSeconds + " Seconds");
nodatawarning = DateTime.Now;
}
}
}
// get home point on armed status change.
if (armedstatus != MainV2.comPort.MAV.cs.armed && comPort.BaseStream.IsOpen)
{
armedstatus = MainV2.comPort.MAV.cs.armed;
// status just changed to armed
if (MainV2.comPort.MAV.cs.armed == true && MainV2.comPort.MAV.aptype != MAVLink.MAV_TYPE.GIMBAL)
{
try
{
//MainV2.comPort.getHomePosition();
MainV2.comPort.MAV.cs.HomeLocation = new PointLatLngAlt(MainV2.comPort.getWP(0));
if (MyView.current != null && MyView.current.Name == "FlightPlanner")
{
// update home if we are on flight data tab
FlightPlanner.updateHome();
}
}
catch
{
// dont hang this loop
this.BeginInvoke(
(MethodInvoker)
delegate
{
CustomMessageBox.Show("Failed to update home location (" +
MainV2.comPort.MAV.sysid + ")");
});
}
}
if (speechEnable && speechEngine != null)
{
if (Settings.Instance.GetBoolean("speecharmenabled"))
{
string speech = armedstatus ? Settings.Instance["speecharm"] : Settings.Instance["speechdisarm"];
if (!string.IsNullOrEmpty(speech))
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(speech));
}
}
}
}
// 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.INVALID,
mavlink_version = 3 // MAVLink.MAVLINK_VERSION
};
// enumerate each link
foreach (var port in Comports)
{
// poll for params at heartbeat interval
if (!port.giveComport)
{
try
{
port.getParamPoll();
port.getParamPoll();
}
catch
{
}
}
// there are 3 hb types we can send, mavlink1, mavlink2 signed and unsigned
bool sentsigned = false;
bool sentmavlink1 = false;
bool sentmavlink2 = false;
// enumerate each mav
foreach (var MAV in port.MAVlist)
{
try
{
// are we talking to a mavlink2 device
if (MAV.mavlinkv2)
{
// is signing enabled
if (MAV.signing)
{
// check if we have already sent
if (sentsigned)
continue;
sentsigned = true;
}
else
{
// check if we have already sent
if (sentmavlink2)
continue;
sentmavlink2 = true;
}
}
else
{
// check if we have already sent
if (sentmavlink1)
continue;
sentmavlink1 = true;
}
port.sendPacket(htb, MAV.sysid, MAV.compid);
}
catch (Exception ex)
{
log.Error(ex);
// close the bad port
try
{
port.Close();
}
catch
{
}
// refresh the screen if needed
if (port == MainV2.comPort)
{
// refresh config window if needed
if (MyView.current != null)
{
this.Invoke((MethodInvoker) delegate()
{
if (MyView.current.Name == "HWConfig")
MyView.ShowScreen("HWConfig");
if (MyView.current.Name == "SWConfig")
MyView.ShowScreen("SWConfig");
});
}
}
}
}
}
heatbeatSend = DateTime.Now;
}
// if not connected or busy, sleep and loop
if (!comPort.BaseStream.IsOpen || comPort.giveComport == true)
{
if (!comPort.BaseStream.IsOpen)
{
// check if other ports are still open
foreach (var port in Comports)
{
if (port.BaseStream.IsOpen)
{
Console.WriteLine("Main comport shut, swapping to other mav");
comPort = port;
break;
}
}
}
System.Threading.Thread.Sleep(100);
}
// read the interfaces
foreach (var port in Comports.ToArray())
{
if (!port.BaseStream.IsOpen)
{
// skip primary interface
if (port == comPort)
continue;
// modify array and drop out
Comports.Remove(port);
port.Dispose();
break;
}
while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes &&
port.giveComport == false && serialThread)
{
try
{
port.readPacket();
}
catch (Exception ex)
{
log.Error(ex);
}
}
// update currentstate of sysids on the port
foreach (var MAV in port.MAVlist)
{
try
{
MAV.cs.UpdateCurrentSettings(null, false, port, MAV);
}
catch (Exception ex)
{
log.Error(ex);
}
}
}
}
catch (Exception e)
{
Tracking.AddException(e);
log.Error("Serial Reader fail :" + e.ToString());
try
{
comPort.Close();
}
catch (Exception ex)
{
log.Error(ex);
}
}
}
Console.WriteLine("SerialReader Done");
SerialThreadrunner.Set();
}
