/// <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>
private void SerialReader()
{
if (serialThread == true)
return;
serialThread = true;
SerialThreadrunner.Reset();
int minbytes = 0;
bool armedstatus = false;
DateTime speechcustomtime = DateTime.Now;
DateTime speechbatterytime = DateTime.Now;
DateTime linkqualitytime = DateTime.Now;
while (serialThread)
{
try
{
Thread.Sleep(1); // was 5
// update connect/disconnect button and info stats
UpdateConnectIcon();
// 30 seconds interval speech options
if (speechEnable && speechEngine != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
if (MainV2.getConfig("speechcustomenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechcustom")));
}
speechcustomtime = DateTime.Now;
}
// speech for battery alerts
if (speechEnable && speechEngine != null && (DateTime.Now - speechbatterytime).TotalSeconds > 10 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
//speechbatteryvolt
float warnvolt = 0;
float.TryParse(MainV2.getConfig("speechbatteryvolt"), out warnvolt);
float warnpercent = 0;
float.TryParse(MainV2.getConfig("speechbatterypercent"), out warnpercent);
if (MainV2.getConfig("speechbatteryenabled") == "True" && MainV2.comPort.MAV.cs.battery_voltage <= warnvolt && MainV2.comPort.MAV.cs.battery_voltage != 0.0)
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechbattery")));
speechbatterytime = DateTime.Now;
}
else if (MainV2.getConfig("speechbatteryenabled") == "True" && (MainV2.comPort.MAV.cs.battery_remaining) < warnpercent && MainV2.comPort.MAV.cs.battery_voltage != 0.0 && MainV2.comPort.MAV.cs.battery_remaining != 0.0)
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechbattery")));
speechbatterytime = DateTime.Now;
}
}
// speech altitude warning.
if (speechEnable && speechEngine != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
float warnalt = float.MaxValue;
float.TryParse(MainV2.getConfig("speechaltheight"), out warnalt);
try
{
if (MainV2.getConfig("speechaltenabled") == "True" && MainV2.comPort.MAV.cs.alt != 0.00 && (MainV2.comPort.MAV.cs.alt - (int)double.Parse(MainV2.getConfig("TXT_homealt"))) <= warnalt)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechalt")));
}
}
catch { } // silent fail
}
// 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);
continue;
}
// make sure we attenuate the link quality if we dont see any valid packets
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10)
{
MainV2.comPort.MAV.cs.linkqualitygcs = 0;
}
// attenuate the link qualty over time
if ((DateTime.Now - comPort.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 is no other packets are being read
GCSViews.FlightData.myhud.Invalidate();
}
}
// send a hb every seconds from gcs to ap
if (heatbeatSend.Second != DateTime.Now.Second && comPort.giveComport == false)
{
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,
};
comPort.sendPacket(htb);
foreach (var port in MainV2.Comports)
{
if (port == MainV2.comPort)
continue;
try
{
port.sendPacket(htb);
}
catch { }
}
heatbeatSend = DateTime.Now;
}
// data loss warning - wait min of 10 seconds, ignore first 30 seconds of connect, repeat at 5 seconds interval
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10 && (DateTime.Now - connecttime).TotalSeconds > 30 && (DateTime.Now - nodatawarning).TotalSeconds > 5)
{
if (speechEnable && speechEngine != null)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
{
MainV2.speechEngine.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds");
nodatawarning = DateTime.Now;
}
}
}
// get home point on armed status change.
if (armedstatus != MainV2.comPort.MAV.cs.armed)
{
armedstatus = MainV2.comPort.MAV.cs.armed;
// status just changed to armed
if (MainV2.comPort.MAV.cs.armed == true)
{
try
{
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"); });
}
}
if (speechEnable && speechEngine != null)
{
if (MainV2.getConfig("speecharmenabled") == "True")
{
if (armedstatus)
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speecharm")));
else
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechdisarm")));
}
}
}
// actualy read the packets
while (comPort.BaseStream.BytesToRead > minbytes && comPort.giveComport == false)
{
try
{
comPort.readPacket();
}
catch { }
}
// update currentstate of main port
try
{
comPort.MAV.cs.UpdateCurrentSettings(null, false, comPort);
}
catch { }
// read the other interfaces
foreach (var port in Comports)
{
if (!port.BaseStream.IsOpen)
{
// modify array and drop out
Comports.Remove(port);
break;
}
// skip primary interface
if (port == comPort)
continue;
while (port.BaseStream.BytesToRead > minbytes)
{
try
{
port.readPacket();
}
catch { }
}
// update currentstate of port
try
{
port.MAV.cs.UpdateCurrentSettings(null, false, port);
}
catch { }
}
}
catch (Exception e)
{
log.Error("Serial Reader fail :" + e.ToString());
try
{
comPort.Close();
}
catch { }
}
}
SerialThreadrunner.Set();
}
private void mavTimer_Tick(object sender, EventArgs e)
{
if (mavbusy)
return;
mavbusy = true;
mavlink.sysid = 7;
mavlink.compid = 1;
MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t();
hb.autopilot = (byte)(MAVLink.MAV_AUTOPILOT.ARDUPILOTMEGA);
//hb.system_status = MAVLink.MAV
mavlink.sendPacket(hb);
MAVLink.mavlink_gps_raw_int_t g = new MAVLink.mavlink_gps_raw_int_t();
lock (this)
{
if (angle >= 0)
{
// lat = (int)(5000 * Math.Cos(angle / 2 / Math.PI));
// lon = (int)(5000 * Math.Sin(angle / 2 / Math.PI));
double lat2, lon2;
getDstPoint(0.0f, 0.0, 500, (angle * Math.PI / 180), out lat2, out lon2);
lat = (int)(lat2 * 1.0e7);
lon = (int)(lon2 * 1.0e7);
double a2 = getBearing(0.0, 0.0, lat2, lon2);
Console.WriteLine("angle {0} {1}", angle, a2);
angle += 2;
if (angle > 360)
angle -= 360;
}
else if (angle >= -5)
{
lat = lon = 0;
angle--;
}
else
angle = 0;
// int.TryParse(tbxLat.Text, out lat);
// int.TryParse(tbxLon.Text, out lon);
// int.TryParse(tbxAlt.Text, out alt);
g.lat = lat;
g.lon = lon;
g.alt = alt;
}
g.fix_type = 3;
mavlink.sendPacket(g);
MAVLink.mavlink_vfr_hud_t vfr = new MAVLink.mavlink_vfr_hud_t();
vfr.airspeed = 10;
vfr.groundspeed = 10;
vfr.alt = alt / 1000.0f;
vfr.heading = (short)angle;
vfr.throttle = 10;
mavlink.sendPacket(vfr);
mavbusy = false;
//if (serialPort2.BytesToRead > 0)
//{
// //Console.WriteLine("mavlink read");
// Console.Write(serialPort2.ReadExisting());
//}
}
/// <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>
private void SerialReader()
{
if (serialThread == true)
return;
serialThread = true;
int minbytes = 0;
DateTime speechcustomtime = DateTime.Now;
DateTime linkqualitytime = DateTime.Now;
while (serialThread)
{
try
{
Thread.Sleep(5);
// update connect/disconnect button and info stats
UpdateConnectIcon();
// 30 seconds interval speech options
if (speechEnable && speechEngine != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 && MainV2.cs.lat != 0 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
//speechbatteryvolt
float warnvolt = 0;
float.TryParse(MainV2.getConfig("speechbatteryvolt"), out warnvolt);
if (MainV2.getConfig("speechbatteryenabled") == "True" && MainV2.cs.battery_voltage <= warnvolt)
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechbattery")));
}
if (MainV2.getConfig("speechcustomenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechcustom")));
}
speechcustomtime = DateTime.Now;
}
// speech altitude warning.
if (speechEnable && speechEngine != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
float warnalt = float.MaxValue;
float.TryParse(MainV2.getConfig("speechaltheight"), out warnalt);
try
{
if (MainV2.getConfig("speechaltenabled") == "True" && (MainV2.cs.alt - (int)double.Parse(MainV2.getConfig("TXT_homealt"))) <= warnalt)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechalt")));
}
}
catch { } // silent fail
}
// if not connected or busy, sleep and loop
if (!comPort.BaseStream.IsOpen || 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 - comPort.lastvalidpacket).TotalSeconds > 10)
{
MainV2.cs.linkqualitygcs = 0;
}
// attenuate the link qualty over time
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds >= 1)
{
if (linkqualitytime.Second != DateTime.Now.Second)
{
MainV2.cs.linkqualitygcs = (ushort)(MainV2.cs.linkqualitygcs * 0.8f);
linkqualitytime = DateTime.Now;
int checkthis;
GCSViews.FlightData.myhud.Invalidate();
}
}
// 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
};
comPort.sendPacket(htb);
heatbeatSend = DateTime.Now;
}
// data loss warning - ignore first 30 seconds of connect
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10 && (DateTime.Now - connecttime).TotalSeconds > 30)
{
if (speechEnable && speechEngine != null)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
MainV2.speechEngine.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds");
}
}
// actauly read the packets
while (comPort.BaseStream.BytesToRead > minbytes && giveComport == false)
{
try
{
comPort.readPacket();
}
catch { }
}
}
catch (Exception e)
{
log.Error("Serial Reader fail :" + e.ToString());
try
{
comPort.Close();
}
catch { }
}
}
}
/// <summary>
/// main serial reader thread
/// controls
/// serial reading
/// link quality stats
/// speech voltage - custom - alt warning - data lost
/// heartbeat packet sending
///
/// and cant fall out
/// </summary>
private void SerialReader()
{
if (serialThread == true)
return;
serialThread = true;
int minbytes = 10;
if (MONO)
minbytes = 0;
DateTime speechcustomtime = DateTime.Now;
DateTime linkqualitytime = DateTime.Now;
while (serialThread)
{
try
{
System.Threading.Thread.Sleep(5);
updateConnectIcon();
if (speechEnable && speechEngine != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 && MainV2.cs.lat != 0 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
//speechbatteryvolt
float warnvolt = 0;
float.TryParse(MainV2.getConfig("speechbatteryvolt"), out warnvolt);
if (MainV2.getConfig("speechbatteryenabled") == "True" && MainV2.cs.battery_voltage <= warnvolt)
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechbattery")));
}
if (MainV2.getConfig("speechcustomenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechcustom")));
}
speechcustomtime = DateTime.Now;
}
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10)
{
MainV2.cs.linkqualitygcs = 0;
}
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds >= 1)
{
if (linkqualitytime.Second != DateTime.Now.Second)
{
MainV2.cs.linkqualitygcs = (ushort)(MainV2.cs.linkqualitygcs * 0.8f);
linkqualitytime = DateTime.Now;
GCSViews.FlightData.myhud.Invalidate();
}
GC.Collect();
}
if (speechEnable && speechEngine != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
float warnalt = float.MaxValue;
float.TryParse(MainV2.getConfig("speechaltheight"), out warnalt);
try
{
if (MainV2.getConfig("speechaltenabled") == "True" && (MainV2.cs.alt - (int)double.Parse(MainV2.getConfig("TXT_homealt"))) <= warnalt)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechalt")));
}
}
catch { } // silent fail
}
if (!comPort.BaseStream.IsOpen || giveComport == true)
{
System.Threading.Thread.Sleep(100);
continue;
}
if (heatbeatSend.Second != DateTime.Now.Second)
{
// Console.WriteLine("remote lost {0}", cs.packetdropremote);
MAVLink.mavlink_heartbeat_t htb = new MAVLink.mavlink_heartbeat_t();
#if MAVLINK10
htb.type = (byte)MAVLink.MAV_TYPE.GCS;
htb.autopilot = (byte)MAVLink.MAV_AUTOPILOT.ARDUPILOTMEGA;
htb.mavlink_version = 3;
#else
htb.type = (byte)MAVLink.MAV_TYPE.MAV_GENERIC;
htb.autopilot = (byte)MAVLink.MAV_AUTOPILOT_TYPE.MAV_AUTOPILOT_ARDUPILOTMEGA;
htb.mavlink_version = 2;
#endif
comPort.sendPacket(htb);
heatbeatSend = DateTime.Now;
}
// data loss warning
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10)
{
if (speechEnable && speechEngine != null)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
MainV2.speechEngine.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds");
}
}
//Console.WriteLine(DateTime.Now.Millisecond + " " + comPort.BaseStream.BytesToRead);
while (comPort.BaseStream.BytesToRead > minbytes && giveComport == false)
comPort.readPacket();
}
catch (Exception e)
{
log.Info("Serial Reader fail :" + e.Message);
try
{
comPort.Close();
}
catch { }
}
}
}