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(); }