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