public MainV2()
{
instance = this;
comPort = new MAVLinkInterface();
comPort.BaseStream = new Comms.UdpSerial();
Comports.Add(comPort);
}
/// <summary>
/// main serial reader thread
/// controls
/// serial reading
/// link quality stats
/// speech voltage - custom - alt warning - data lost
/// heartbeat packet sending
///
/// and can't fall out
/// </summary>
internal async Task SerialReader()
{
if (serialThread == true)
return;
serialThread = true;
SerialThreadrunner.Reset();
int minbytes = 10;
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
// update connect/disconnect button and info stats
try
{
//UpdateConnectIcon();
}
catch (Exception ex)
{
log.Error(ex);
}
// 30 seconds interval speech options
if (MainV2.speechEnabled() && 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(ArduPilot.Common.speechConversion(comPort.MAV, "" + 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(ArduPilot.Common.speechConversion(comPort.MAV, "" + 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(
ArduPilot.Common.speechConversion(comPort.MAV, "" + Settings.Instance["speechbattery"]));
}
}
}
// speech for airspeed alerts
if (speechEnabled() && 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(
ArduPilot.Common.speechConversion(comPort.MAV, "" + Settings.Instance["speechlowairspeed"]));
speechlowspeedtime = DateTime.Now;
}
}
else if (MainV2.comPort.MAV.cs.groundspeed < warngroundspeed)
{
if (MainV2.speechEngine.IsReady)
{
MainV2.speechEngine.SpeakAsync(
ArduPilot.Common.speechConversion(comPort.MAV, "" + Settings.Instance["speechlowgroundspeed"]));
speechlowspeedtime = DateTime.Now;
}
}
else
{
speechlowspeedtime = DateTime.Now;
}
}
}
// speech altitude warning - message high warning
if ((speechEnabled() && 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(
ArduPilot.Common.speechConversion(comPort.MAV, "" + 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
}
}
// 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.apname != MAVLink.MAV_AUTOPILOT.INVALID &&
MainV2.comPort.MAV.aptype != MAVLink.MAV_TYPE.GIMBAL)
{
System.Threading.ThreadPool.QueueUserWorkItem(state =>
{
Thread.CurrentThread.Name = "Arm State change";
try
{
while (comPort.giveComport == true)
Thread.Sleep(100);
MainV2.comPort.MAV.cs.HomeLocation = new PointLatLngAlt(MainV2.comPort.getWP(0));
}
catch
{
// dont hang this loop
}
});
}
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(ArduPilot.Common.speechConversion(comPort.MAV, 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.ToArray())
{
if (!port.BaseStream.IsOpen)
continue;
// poll for params at heartbeat interval - primary mav on this port only
if (!port.giveComport)
{
try
{
// poll only when not armed
if (!port.MAV.cs.armed)
{
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
{
// poll for version if we dont have it - every mav every port
if (!port.giveComport && !MAV.cs.armed && (DateTime.Now.Second % 20) == 0 && MAV.cs.version < new Version(0, 1))
port.getVersion(MAV.sysid, MAV.compid, false);
// 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
}
}
}
}
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;
}
DateTime startread = DateTime.Now;
// must be open, we have bytes, we are not yielding the port,
// the thread is meant to be running and we only spend 1 seconds max in this read loop
while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes &&
port.giveComport == false && serialThread && startread.AddSeconds(1) > DateTime.Now)
{
try
{
await port.readPacketAsync().ConfigureAwait(false);
}
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)
{
log.Error("Serial Reader fail :" + e.ToString());
try
{
comPort.Close();
}
catch (Exception ex)
{
log.Error(ex);
}
}
}
Console.WriteLine("SerialReader Done");
SerialThreadrunner.Set();
}
