diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs index 2d34e84e2b..7106efcb18 100644 --- a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs +++ b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs @@ -1,6906 +1,7044 @@ -using log4net; -using MissionPlanner.ArduPilot; -using MissionPlanner.ArduPilot.Mavlink; -using MissionPlanner.Comms; -using MissionPlanner.Mavlink; -using MissionPlanner.Utilities; -using Newtonsoft.Json; -using System; -using System.Collections; -using System.Collections.Concurrent; -using System.Collections.Generic; -using System.Diagnostics; -using System.IO; -using System.Linq; -using System.Reactive.Subjects; -using System.Reflection; -using System.Runtime.InteropServices; -using System.Security.Cryptography; -using System.Text; -using System.Text.RegularExpressions; -using System.Threading; -using System.Threading.Tasks; -using Timer = System.Timers.Timer; - -namespace MissionPlanner -{ - public class MAVLinkInterface : MAVLink, IDisposable, IMAVLinkInterface, IMAVLinkInterfaceLogRead - { - private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - private ICommsSerial _baseStream; - - public ICommsSerial BaseStream - { - get { return _baseStream; } - set - { - // This is called every time user changes the port selection, so we need to make sure we cleanup - // any previous objects so we don't leave the cleanup of system resources to the garbage collector. - if (_baseStream != null) - { - try - { - if (_baseStream.IsOpen) - { - _baseStream.Close(); - } - } - catch - { - } - - IDisposable dsp = _baseStream as IDisposable; - if (dsp != null) - { - try - { - dsp.Dispose(); - } - catch - { - } - } - } - - _baseStream = value; - } - } - - public event EventHandler OnPacketReceived - { - add - { - log.Debug("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value?.Method?.Name + - " " + value?.Target?.GetType()?.Name + - " " + value); - _OnPacketReceived += value; - } - - remove - { - log.Debug("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + - value?.Method?.Name + " " + value?.Target?.GetType()?.Name + - " " + value); - _OnPacketReceived -= value; - } - } - - public event EventHandler<(byte, byte)> MAVDetected - { - add - { - log.Debug("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value?.Method?.Name + - " " + value?.Target?.GetType()?.Name + - " " + value); - _MAVDetected += value; - } - - remove - { - log.Debug("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + - value?.Method?.Name + " " + value?.Target?.GetType()?.Name + - " " + value); - _MAVDetected -= value; - } - } - - private EventHandler<(byte, byte)> _MAVDetected; - - public void DoOnPacketSent(MAVLinkMessage pkt) - { - _OnPacketSent?.Invoke(this, pkt); - } - - public event EventHandler OnPacketSent - { - add - { - log.Debug("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _OnPacketSent += value; - } - remove - { - log.Debug("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _OnPacketSent -= value; - } - } - - public static event EventHandler UpdateADSBPlanePosition - { - add - { - log.Info("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _UpdateADSBPlanePosition += value; - } - remove - { - log.Info("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _UpdateADSBPlanePosition -= value; - } - } - - public static event EventHandler<(string id, MAV_COLLISION_THREAT_LEVEL threat_level)> UpdateADSBCollision - { - add - { - log.Info("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _UpdateADSBCollision += value; - } - remove - { - log.Info("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _UpdateADSBCollision -= value; - } - } - - public class Mirror - { - public ICommsSerial MirrorStream { get; set; } - public bool MirrorStreamWrite { get; set; } - } - - public List Mirrors { get; set; } = new List(); - - public event EventHandler ParamListChanged - { - add - { - log.Debug("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _ParamListChanged += value; - } - remove - { - log.Debug("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _ParamListChanged -= value; - } - } - - public event EventHandler MavChanged - { - add - { - log.Info("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _MavChanged += value; - } - remove - { - log.Info("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _MavChanged -= value; - } - } - - public event EventHandler CommsClose - { - add - { - log.Info("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _CommsClose += value; - } - remove - { - log.Info("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + - " " + value.Target.GetType().Name + - " " + value); - _CommsClose -= value; - } - } - - public static byte gcssysid { get; set; } = 255; - - private string lastset = ""; - - /// - /// used to prevent comport access for exclusive use - /// - public bool giveComport - { - get - { - //Console.WriteLine("get giveComport {0} current {1}", Thread.CurrentThread.Name, _giveComport); - return _giveComport; - } - set - { - log.InfoFormat("set giveComport {0} current {1} new {2}", Thread.CurrentThread.Name, _giveComport, - value); - if (_giveComport && value) - { - // trying to set it true twice - log.Error(new System.Diagnostics.StackTrace().ToString()); - Debugger.Break(); - } - - if (value == true) - { - // wait for current read to end before continue - lock (readlock) - { - } - } - - lastset = Thread.CurrentThread.Name; - _giveComport = value; - } - } - - private volatile bool _giveComport = false; - - private DateTime lastparamset = DateTime.MinValue; - - /// - /// stores a list of strings during connect for possible debug - /// - internal List plaintxtlinebuffer = new List(30); - - /// - /// stores a single string during connect for possible debug - /// - internal string plaintxtline = ""; - private string buildplaintxtline = ""; - - public bool ReadOnly = false; - - public TerrainFollow Terrain; - - public event ProgressEventHandler Progress; - - private int _sysidcurrent = 0; - - public int sysidcurrent - { - get { return _sysidcurrent; } - set - { - if (_sysidcurrent == value) - return; - _sysidcurrent = value; - if (_MavChanged != null) _MavChanged(this, null); - } - } - - private int _compidcurrent = 0; - - public int compidcurrent - { - get { return _compidcurrent; } - set - { - if (_compidcurrent == value) - return; - _compidcurrent = value; - if (_MavChanged != null) _MavChanged(this, null); - } - } - - public MAVList MAVlist; - - public MAVState MAV - { - get { return MAVlist[sysidcurrent, compidcurrent]; } - set { MAVlist[sysidcurrent, compidcurrent] = value; } - } - - public double CONNECT_TIMEOUT_SECONDS = Settings.Instance.GetDouble("CONNECT_TIMEOUT_SECONDS", 30.0); - - /// - /// progress form to handle connect and param requests - /// - public IProgressReporterDialogue frmProgressReporter; - - /// - /// used for outbound packet sending - /// - public int packetcount { get; internal set; } = 0; - - private readonly Subject _bytesReceivedSubj = new Subject(); - private readonly Subject _bytesSentSubj = new Subject(); - - /// - /// Observable of the count of bytes received, notified when the bytes themselves are received - /// - public IObservable BytesReceived - { - get { return _bytesReceivedSubj; } - } - - /// - /// Observable of the count of bytes sent, notified when the bytes themselves are received - /// - public IObservable BytesSent - { - get { return _bytesSentSubj; } - } - - /// - /// Observable of the count of packets skipped (on reception), - /// calculated from periods where received packet sequence is not - /// contiguous - /// - public Subject WhenPacketLost { get; set; } - - public Subject WhenPacketReceived { get; set; } - - /// - /// used as a serial port write lock - /// - public volatile object writelock = new object(); - - /// - /// mavlink version - /// - private byte mavlinkversion = 0; - - /// - /// turns on console packet display - /// - public bool debugmavlink { get; set; } - - /// - /// enabled read from file mode - /// - public bool logreadmode - { - get { return _logreadmode; } - set { _logreadmode = value; } - } - - /// - /// used to disable all speech originating from this module - /// - public bool speechenabled { get; set; } = true; - - private bool _logreadmode = false; - - private BinaryReader _logplaybackfile; - - public DateTime lastlogread { get; set; } - public DateTime lastlogreadwall { get; set; } - - public BinaryReader logplaybackfile - { - get { return _logplaybackfile; } - set - { - _logplaybackfile = value; - if (_logplaybackfile != null && _logplaybackfile.BaseStream is FileStream) - log.Info("Logplaybackfile set " + ((FileStream)_logplaybackfile.BaseStream).Name); - MAVlist.Clear(); - } - } - - public BufferedStream logfile { get; set; } - public BufferedStream rawlogfile { get; set; } - - public int Mavlink1Count { get { return _mavlink1count; } } - public int Mavlink2Count { get { return _mavlink2count; } } - public int Mavlink2Signed { get { return _mavlink2signed; } } - - private int _mavlink1count = 0; - private int _mavlink2count = 0; - private int _mavlink2signed = 0; - private int _bps1 = 0; - private int _bps2 = 0; - private DateTime _bpstime { get; set; } - - public bool MirrorStreamWrite { - get { - if (Mirrors.Count > 0) - return Mirrors[0].MirrorStreamWrite; - - Mirrors.Add(new Mirror()); - return MirrorStreamWrite; - } - set - { - if (Mirrors.Count > 0) - Mirrors[0].MirrorStreamWrite = value; - else - Mirrors.Add(new Mirror() { MirrorStreamWrite = value }); - } - } - public ICommsSerial MirrorStream { - get - { - if (Mirrors.Count > 0) - return Mirrors[0].MirrorStream; - - Mirrors.Add(new Mirror()); - return MirrorStream; - } - set - { - if (Mirrors.Count > 0) - Mirrors[0].MirrorStream = value; - else - Mirrors.Add(new Mirror() { MirrorStream = value }); - } - } - - - public static ISpeech Speech; - - ~MAVLinkInterface() - { - this.Dispose(); - } - - public MAVLinkInterface() - { - // init fields - MAVlist = new MAVList(this); - this.packetcount = 0; - this._bytesReceivedSubj = new Subject(); - this._bytesSentSubj = new Subject(); - this.WhenPacketLost = new Subject(); - this.WhenPacketReceived = new Subject(); - - this.mavlinkversion = 0; - - this.debugmavlink = false; - this.logreadmode = false; - this.lastlogread = DateTime.MinValue; - this._logplaybackfile = null; - this.logfile = null; - this.rawlogfile = null; - this._bps1 = 0; - this._bps2 = 0; - this._bpstime = DateTime.MinValue; - _mavlink1count = 0; - _mavlink2count = 0; - _mavlink2signed = 0; - - AIS.Start(this); - - // new hearbeat detected - MAVDetected += OnMAVDetected; - } - - private void OnMAVDetected(object sender, (byte, byte) tuple) - { - - // check for a camera - if (tuple.Item2 == (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1 || - (tuple.Item2 >= (byte) MAVLink.MAV_COMPONENT.MAV_COMP_ID_CAMERA && - tuple.Item2 <= (byte) MAV_COMPONENT.MAV_COMP_ID_CAMERA6)) - { - MAVlist[tuple.Item1, tuple.Item2].Camera = new CameraProtocol(); - Task.Run(async () => - { - try - { - // Open holds this - while (!_openComplete) - { - await Task.Delay(1000); - } - - await Task.Delay(2000); - - if (MAVlist[tuple.Item1, tuple.Item2].Camera == null) - return; - - while(giveComport) - await Task.Delay(100); - - await MAVlist[tuple.Item1, tuple.Item2] - .Camera.StartID(MAVlist[tuple.Item1, tuple.Item2]) - .ConfigureAwait(false); - } - catch (Exception e) - { - log.Error(e); - } - }); - } - // gimbals - if (tuple.Item2 >= (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_GIMBAL && - tuple.Item2 <= (byte)MAV_COMPONENT.MAV_COMP_ID_GIMBAL6) - { - MAVlist[tuple.Item1, tuple.Item2].Gimbal = new GimbalProtocol(); - Task.Run(async () => - { - try - { - // Open holds this - while (!_openComplete) - { - await Task.Delay(1000); - } - - await Task.Delay(2000); - - MAVlist[tuple.Item1, tuple.Item2] - .Gimbal?.Discover(this); - } - catch (Exception e) - { - log.Error(e); - } - }); - } - - if (tuple.Item2 == (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1 || - (tuple.Item2 >= (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER && - tuple.Item2 <= (byte)MAV_COMPONENT.MAV_COMP_ID_ONBOARD_COMPUTER4)) - { - MAVlist[tuple.Item1, tuple.Item2].GimbalManager = new GimbalManagerProtocol(this, MAVlist[tuple.Item1, tuple.Item2].cs); - Task.Run(async () => - { - try - { - // Open holds this - while (!_openComplete) - { - await Task.Delay(1000); - } - - await Task.Delay(2000); - - MAVlist[tuple.Item1, tuple.Item2] - .GimbalManager?.Discover(); - } - catch (Exception e) - { - log.Error(e); - } - }); - } - } - - public MAVLinkInterface(Stream logfileStream) - : this() - { - logplaybackfile = new BinaryReader(logfileStream); - logreadmode = true; - } - - public void Close() - { - try - { - Terrain?.UnSub(); - Terrain = null; - } - catch - { - - } - - try - { - giveComport = false; - - if (logfile != null) - logfile.Close(); - } - catch - { - } - - try - { - if (rawlogfile != null) - rawlogfile.Close(); - } - catch - { - } - - try - { - if (logplaybackfile != null) - logplaybackfile.Close(); - } - catch - { - } - - try - { - if (BaseStream != null && BaseStream.IsOpen) - BaseStream.Close(); - } - catch - { - } - - try - { - if (_CommsClose != null) - _CommsClose(this, null); - } - catch - { - } - } - - public delegate IProgressReporterDialogue ProgressEventHandle(string title); - - public static event ProgressEventHandle CreateIProgressReporterDialogue; - - public void Open() - { - Open(false); - } - - public void Open(bool getparams, bool skipconnectedcheck = false, bool showui = true) - { - if (BaseStream == null || BaseStream.IsOpen && !skipconnectedcheck) - return; - - MAVlist.Clear(); - - if (showui) - frmProgressReporter = CreateIProgressReporterDialogue(Strings.ConnectingMavlink); - else - frmProgressReporter = new NoUIReporter(); - - if (getparams) - { - frmProgressReporter.DoWork += FrmProgressReporterDoWorkAndParams; - } - else - { - frmProgressReporter.DoWork += FrmProgressReporterDoWorkNOParams; - } - - frmProgressReporter.UpdateProgressAndStatus(-1, Strings.MavlinkConnecting); - - frmProgressReporter.RunBackgroundOperationAsync(); - - frmProgressReporter.Dispose(); - - if (_ParamListChanged != null) - { - _ParamListChanged(this, null); - } - } - - private void FrmProgressReporterDoWorkAndParams(IProgressReporterDialogue sender) - { - OpenBg(sender, true); - } - - private void FrmProgressReporterDoWorkNOParams(IProgressReporterDialogue sender) - { - OpenBg(sender, false); - } - - private void OpenBg(IProgressReporterDialogue PRsender, bool getparams) - { - _openComplete = false; - - frmProgressReporter.UpdateProgressAndStatus(-1, Strings.MavlinkConnecting); - - if (BaseStream is SerialPort) - { - // allow settings to settle - previous dtr - log.Debug("SerialPort Sleep 1"); - Thread.Sleep(1000); - } - - Terrain?.UnSub(); - Terrain = new TerrainFollow(this); - - bool hbseen = false; - - try - { - BaseStream.ReadBufferSize = 16 * 1024; - - lock (writelock) // so we dont have random traffic - { - log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate); - - if (BaseStream is UdpSerial) - { - PRsender.doWorkArgs.CancelRequestChanged += (o, e) => - { - ((UdpSerial) BaseStream).CancelConnect = true; - ((ProgressWorkerEventArgs) o) - .CancelAcknowledged = true; - }; - } - - BaseStream.Open(); - - BaseStream.DiscardInBuffer(); - - // other boards seem to have issues if there is no delay? posible bootloader timeout issue - if (BaseStream is SerialPort) - { - log.Debug("SerialPort Sleep 2"); - Thread.Sleep(1000); - } - } - - List hbhistory = new List(); - - DateTime start = DateTime.Now; - DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS); - - var countDown = new Timer {Interval = 1000, AutoReset = false}; - countDown.Elapsed += (sender, e) => - { - int secondsRemaining = Convert.ToInt32((deadline - e.SignalTime).TotalSeconds); - frmProgressReporter.UpdateProgressAndStatus(-1, string.Format(Strings.Trying, secondsRemaining)); - if (secondsRemaining > 0) countDown.Start(); - }; - countDown.Start(); - - int count = 0; - - plaintxtline = ""; - - while (true) - { - if (PRsender.doWorkArgs.CancelRequested) - { - PRsender.doWorkArgs.CancelAcknowledged = true; - countDown.Stop(); - if (BaseStream.IsOpen) - BaseStream.Close(); - giveComport = false; - return; - } - - log.Info(DateTime.Now.Millisecond + " Start connect loop "); - - if (DateTime.Now > deadline) - { - //if (Progress != null) - // Progress(-1, "No Heartbeat Packets"); - countDown.Stop(); - this.Close(); - - if (hbseen) - { - PRsender.doWorkArgs.ErrorMessage = Strings.Only1Hb; - throw new Exception(Strings.Only1HbD + plaintxtlinebuffer.Aggregate((a, b) => a + "\r\n" + b)); - } - else - { - PRsender.doWorkArgs.ErrorMessage = "No Heartbeat Packets Received"; - throw new TimeoutException(@"Can not establish a connection - -No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup -Mission Planner waits for 2 valid heartbeat packets before connecting -" + plaintxtlinebuffer.Aggregate((a, b) => a + "\r\n" + b)); - } - } - - Thread.Sleep(1); - - var buffer = getHeartBeat(); - if (buffer.Length > 0) - { - if (buffer.msgid == (ulong)MAVLINK_MSG_ID.HEARTBEAT) - { - mavlink_heartbeat_t hb = buffer.ToStructure(); - - // no GCS's and no broadcast compid's (ping adsb) - if (hb.type != (byte)MAV_TYPE.GCS && buffer.compid != 0) - { - hbhistory.Add(buffer); - } - } - else if (buffer.msgid == (ulong)MAVLINK_MSG_ID.HIGH_LATENCY2) - { - mavlink_high_latency2_t hl = buffer.ToStructure(); - // no GCS's and no broadcast compid's (ping adsb) - if (hl.type != (byte)MAV_TYPE.GCS && buffer.compid != 0) - { - hbhistory.Add(buffer); - } - } - else - { - continue; - } - } - - if (hbhistory.Count > 0) - hbseen = true; - - count++; - - // if we get no data, try enableing rts/cts - if (buffer.Length == 0 && BaseStream is SerialPort && start.AddSeconds(20) < DateTime.Now) - { - try - { - log.Debug("about to set RTS to " + !BaseStream.RtsEnable); - BaseStream.RtsEnable = !BaseStream.RtsEnable; - } - catch - { - } - } - - // SLCAN check - if (Regex.IsMatch(plaintxtline, @"\rT[0-9A-Z]{9,32}$", RegexOptions.Multiline)) - { - PRsender.doWorkArgs.ErrorMessage = - "This appears to be a CAN port, please use the DroneCAN screen"; - throw new Exception(@"This appears to be a CAN port, please use the DroneCAN screen"); - } - - // check we have hb's - if (hbhistory.Count > 0) - { - bool exit = false; - // get most seen hbs - var mostseenlist = hbhistory.GroupBy(s => MAVList.GetID(s.sysid, s.compid)) - .OrderByDescending(s => s.Count()).Where(s => s.Count() >= 2); - foreach (var mostseen in mostseenlist) - { - // get count on most seen - var seentimes = mostseen.Count(); - // get the most seen mavlinkmessage - var msg = mostseen.First(); - - // preference compid of 1, failover to anything that we have seen 4 times - if (seentimes >= 2 && msg.compid == 1 || seentimes >= 4) - { - if (msg.msgid == (ulong)MAVLINK_MSG_ID.HEARTBEAT) - SetupMavConnect(msg, (mavlink_heartbeat_t) msg.data); - if (msg.msgid == (ulong)MAVLINK_MSG_ID.HIGH_LATENCY2) - SetupMavConnect(msg, (mavlink_high_latency2_t)msg.data); - sysidcurrent = msg.sysid; - compidcurrent = msg.compid; - log.Info($"HB Selection {sysidcurrent}-{compidcurrent} seen {seentimes}"); - exit = true; - break; - } - } - - if (exit) - break; - } - } - - countDown.Stop(); - - char[] temp = ("Mission Planner " + getAppVersion() + "\0").ToCharArray(); - Array.Resize(ref temp, 50); - - // - generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT, - new mavlink_statustext_t() {severity = (byte) MAV_SEVERITY.INFO, text = temp.ToByteArray()}); - - // mavlink2 - generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT, - new mavlink_statustext_t() {severity = (byte) MAV_SEVERITY.INFO, text = temp.ToByteArray()}, - sysidcurrent, - compidcurrent, true, false); - - // mavlink2 - signed - generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT, - new mavlink_statustext_t() {severity = (byte) MAV_SEVERITY.INFO, text = temp.ToByteArray()}, - sysidcurrent, - compidcurrent, true, true); - - // this ensures a mavlink2 change has been noticed - getHeartBeat(); - - getVersion((byte) sysidcurrent, (byte) compidcurrent); - - doCommand((byte) sysidcurrent, (byte) compidcurrent, MAV_CMD.DO_SEND_BANNER, 0, 0, 0, 0, 0, 0, 0, - false); - - if (getparams) - { - frmProgressReporter.UpdateProgressAndStatus(0, - "Getting Params.. (sysid " + MAV.sysid + " compid " + MAV.compid + ") "); - - getParamListMavftp(MAV.sysid, MAV.compid); - } - - if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true) - { - giveComport = false; - if (BaseStream.IsOpen) - BaseStream.Close(); - return; - } - } - catch (Exception e) - { - try - { - BaseStream.Close(); - } - catch - { - } - - giveComport = false; - if (string.IsNullOrEmpty(PRsender.doWorkArgs.ErrorMessage)) - PRsender.doWorkArgs.ErrorMessage = Strings.ConnectFailed; - log.Error(e); - throw; - } - - //frmProgressReporter.Close(); - giveComport = false; - frmProgressReporter.UpdateProgressAndStatus(100, Strings.Done); - log.Info("Done open " + MAV.sysid + " " + MAV.compid); - MAV.packetslost = 0; - MAV.synclost = 0; - _openComplete = true; - } - - private string getAppVersion() - { - try - { - Assembly entryAssembly = Assembly.GetEntryAssembly(); - if (entryAssembly != null) - { - object[] customAttributes = - entryAssembly.GetCustomAttributes(typeof(AssemblyFileVersionAttribute), false); - if (customAttributes != null && customAttributes.Length != 0) - { - return ((AssemblyFileVersionAttribute) customAttributes[0]).Version; - } - } - } - catch - { - } - - return "0.0"; - } - - private uint request_id = 0; - - /// - /// devop read spi mpu6000 0 0 0xf5 1 - ///STABILIZE> Operation 1 OK: 1 bytes - ///0 1 2 3 4 5 6 7 8 9 a b c d e f - ///f0: -- -- -- -- -- 71 -- -- -- -- -- -- -- -- -- -- - /// - /// spi/i2c - /// spi - /// i2c - /// i2c - /// - /// - /// - public byte device_op(byte sysid, byte compid, out byte[] outputbuffer, MAVLink.DEVICE_OP_BUSTYPE bustype, - string name, byte bus, - byte address, byte regstart, byte count, byte[] writebytes = null) - { - var responce = false; - var buffer = new byte[0]; - byte result = 0; - - var sub = SubscribeToPacketType(MAVLINK_MSG_ID.DEVICE_OP_READ_REPLY, (m) => - { - var mtype = (MAVLINK_MSG_ID)m.msgid; - if (mtype == MAVLINK_MSG_ID.DEVICE_OP_READ_REPLY) - { - var msg = (mavlink_device_op_read_reply_t)m.data; - if (msg.result != 0) - log.InfoFormat(name + " Operation {0} failed: {1}", msg.request_id, msg.result); - else - log.InfoFormat(name + " Operation {0} OK: {1} bytes", msg.request_id, msg.count); - for (var i = 0; i < msg.count; i++) - { - var reg = i + msg.regstart; - log.InfoFormat("{0,2:X}:{1,2:X} ", reg, msg.data[i]); - if ((i + 1) % 16 == 0) - Console.WriteLine(); - if (msg.count % 16 != 0) - Console.WriteLine(); - } - - buffer = msg.data.Take(msg.count).ToArray(); - responce = true; - result = msg.result; - } - - return true; - }, sysid, compid); - - var sub2 = SubscribeToPacketType(MAVLINK_MSG_ID.DEVICE_OP_WRITE_REPLY, (m) => - { - var mtype = (MAVLINK_MSG_ID) m.msgid; - if (mtype == MAVLINK_MSG_ID.DEVICE_OP_WRITE_REPLY) - { - var msg = (mavlink_device_op_write_reply_t) m.data; - if (msg.result != 0) - log.InfoFormat(name + " Operation {0} failed: {1}", msg.request_id, msg.result); - else - log.InfoFormat(name + " Operation {0} OK", msg.request_id); - - responce = true; - result = msg.result; - } - - return true; - }, sysid, compid); - - if (writebytes != null) - { - var write = new mavlink_device_op_write_t() - { - target_system = (byte) sysid, - target_component = (byte) compid, - request_id = request_id++, - bustype = (byte) bustype, - bus = bus, - busname = name.MakeBytesSize(40), - address = address, - regstart = regstart, - count = (byte) writebytes.Length, - data = writebytes.MakeSize(128) - }; - - generatePacket(MAVLINK_MSG_ID.DEVICE_OP_WRITE, write); - } - else - { - var read = new MAVLink.mavlink_device_op_read_t() - { - target_system = (byte) sysid, - target_component = (byte) compid, - request_id = request_id++, - bustype = (byte) bustype, - bus = bus, - busname = name.MakeBytesSize(40), - address = address, - regstart = regstart, - count = count - }; - generatePacket(MAVLINK_MSG_ID.DEVICE_OP_READ, read); - } - - log.InfoFormat("bustype {0} name {1} bus {2} address {3}", bustype, name, bus, address); - var start = DateTime.Now; - while (!responce && start.AddSeconds(1) > DateTime.Now) - { - Thread.Sleep(1); - } - - UnSubscribeToPacketType(sub); - UnSubscribeToPacketType(sub2); - - outputbuffer = buffer; - return result; - } - - private void SetupMavConnect(MAVLinkMessage message, mavlink_heartbeat_t hb) - { - mavlinkversion = hb.mavlink_version; - MAVlist[message.sysid, message.compid].aptype = (MAV_TYPE) hb.type; - MAVlist[message.sysid, message.compid].apname = (MAV_AUTOPILOT) hb.autopilot; - - setAPType(message.sysid, message.compid); - - MAVlist[message.sysid, message.compid].sysid = message.sysid; - MAVlist[message.sysid, message.compid].compid = message.compid; - MAVlist[message.sysid, message.compid].recvpacketcount = message.seq; - log.InfoFormat("ID sys {0} comp {1} ver{2} type {3} name {4}", message.sysid, message.compid, - mavlinkversion, - MAVlist[message.sysid, message.compid].aptype.ToString(), MAVlist[message.sysid, message.compid].apname.ToString()); - } - - private void SetupMavConnect(MAVLinkMessage message, mavlink_high_latency2_t hl) - { - mavlinkversion = 0; - MAVlist[message.sysid, message.compid].aptype = (MAV_TYPE)hl.type; - MAVlist[message.sysid, message.compid].apname = (MAV_AUTOPILOT)hl.autopilot; - MAVlist[message.sysid, message.compid].isHighLatency = true; - - setAPType(message.sysid, message.compid); - - MAVlist[message.sysid, message.compid].sysid = message.sysid; - MAVlist[message.sysid, message.compid].compid = message.compid; - MAVlist[message.sysid, message.compid].recvpacketcount = message.seq; - log.InfoFormat("ID HL sys {0} comp {1} ver{2} type {3} name {4}", message.sysid, message.compid, - mavlinkversion, - MAVlist[message.sysid, message.compid].aptype.ToString(), MAVlist[message.sysid, message.compid].apname.ToString()); - } - - public MAVLinkMessage getHeartBeat() - { - return getHeartBeatAsync().AwaitSync(); - } - - public async Task getHeartBeatAsync() - { - giveComport = true; - - sendPacket(new MAVLink.mavlink_heartbeat_t() - { - type = (byte) MAVLink.MAV_TYPE.GCS, - autopilot = (byte) MAVLink.MAV_AUTOPILOT.INVALID, - mavlink_version = 3 // MAVLink.MAVLINK_VERSION - }, gcssysid, 190); - - DateTime start = DateTime.Now; - int readcount = 0; - while (true) - { - MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); - readcount++; - if (buffer.Length > 5) - { - //log.Info("getHB packet received: " + buffer.Length + " btr " + BaseStream.BytesToRead + " type " + buffer.msgid ); - if (buffer.msgid == (byte) MAVLINK_MSG_ID.HEARTBEAT) - { - mavlink_heartbeat_t hb = buffer.ToStructure(); - - if (hb.type != (byte) MAV_TYPE.GCS) - { - SetupMavConnect(buffer, hb); - - giveComport = false; - return buffer; - } - } - if (buffer.msgid == (byte)MAVLINK_MSG_ID.HIGH_LATENCY2) - { - mavlink_high_latency2_t hl = buffer.ToStructure(); - - if (hl.type != (byte)MAV_TYPE.GCS) - { - SetupMavConnect(buffer, hl); - - giveComport = false; - return buffer; - } - } - } - - if (DateTime.Now > start.AddMilliseconds(2200) || readcount > 200) // was 1200 , now 2.2 sec - { - giveComport = false; - return MAVLinkMessage.Invalid; - } - } - } - - /// - /// Send a mavlink packet - /// - /// data - /// target sysid - /// target compid - public void sendPacket(object indata, int sysid, int compid) - { - bool validPacket = false; - - if (BaseStream == null || !BaseStream.IsOpen) - return; - - foreach (var ty in MAVLINK_MESSAGE_INFOS) - { - if (ty.type == indata.GetType()) - { - validPacket = true; - generatePacket((int) ty.msgid, indata, sysid, compid); - return; - } - } - - if (!validPacket) - { - log.Info("Mavlink : NOT VALID PACKET sendPacket() " + indata.GetType().ToString()); - } - } - - private void generatePacket(MAVLINK_MSG_ID messageType, object indata) - { - generatePacket((int) messageType, indata); - } - - private void generatePacket(int messageType, object indata) - { - //uses currently targeted mavs sysid and compid - generatePacket(messageType, indata, MAV.sysid, MAV.compid); - } - - public void generatePacket(MAVLINK_MSG_ID messageType, object indata, int sysid, int compid, - bool forcemavlink2 = false, - bool forcesigning = false) - { - generatePacket((int) messageType, indata, sysid, compid, forcemavlink2, forcesigning); - } - - /// - /// Generate a Mavlink Packet and write to serial - /// - /// type number = MAVLINK_MSG_ID - /// struct of data - public void generatePacket(int messageType, object indata, int sysid, int compid, bool forcemavlink2 = false, - bool forcesigning = false) - { - if (BaseStream == null || !BaseStream.IsOpen) - { - return; - } - - if (ReadOnly) - { - // allow these messages - if (messageType == (byte) MAVLINK_MSG_ID.MISSION_REQUEST_LIST || - messageType == (byte) MAVLINK_MSG_ID.MISSION_REQUEST_PARTIAL_LIST || - messageType == (byte) MAVLINK_MSG_ID.MISSION_REQUEST || - messageType == (byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT || - messageType == (byte) MAVLINK_MSG_ID.PARAM_REQUEST_LIST || - messageType == (byte) MAVLINK_MSG_ID.PARAM_REQUEST_READ || - messageType == (byte) MAVLINK_MSG_ID.RALLY_FETCH_POINT || - messageType == (byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT - ) - { - } - else - { - return; - } - } - - byte[] packet = new byte[0]; - lock (writelock) - { - byte[] data = MavlinkUtil.StructureToByteArray(indata); - - int i = 0; - - // are we mavlink2 enabled for this sysid/compid - if (!MAVlist[sysid, compid].mavlinkv2 && messageType < 256 && !forcemavlink2) - { - var info = MAVLINK_MESSAGE_INFOS.SingleOrDefault(p => p.msgid == messageType); - if (data.Length != info.minlength) - { - Array.Resize(ref data, (int) info.minlength); - } - - //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); - packet = new byte[data.Length + 6 + 2]; - - packet[0] = MAVLINK_STX_MAVLINK1; - packet[1] = (byte) data.Length; - packet[2] = (byte) packetcount; - - packetcount++; - - packet[3] = gcssysid; - packet[4] = (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; - packet[5] = (byte) messageType; - - i = 6; - foreach (byte b in data) - { - packet[i] = b; - i++; - } - - ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6); - - checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint) messageType).crc, - checksum); - - byte ck_a = (byte) (checksum & 0xFF); ///< High byte - byte ck_b = (byte) (checksum >> 8); ///< Low byte - - packet[i] = ck_a; - i += 1; - packet[i] = ck_b; - i += 1; - } - else - { - // trim packet for mavlink2 - MavlinkUtil.trim_payload(ref data); - - // space for signing if needed - if (MAVlist[sysid, compid].signing || forcesigning) - packet = new byte[data.Length + MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES + - MAVLINK_SIGNATURE_BLOCK_LEN]; - else - packet = new byte[data.Length + MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES]; - - packet[0] = MAVLINK_STX; - packet[1] = (byte) data.Length; - packet[2] = 0; // incompat - if (MAVlist[sysid, compid].signing || forcesigning) // current mav - packet[2] |= MAVLINK_IFLAG_SIGNED; - packet[3] = 0; // compat - packet[4] = (byte) packetcount; - - packetcount++; - - packet[5] = gcssysid; - packet[6] = (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; - packet[7] = (byte) (messageType & 0xff); - packet[8] = (byte) ((messageType >> 8) & 0xff); - packet[9] = (byte) ((messageType >> 16) & 0xff); - - i = 10; - foreach (byte b in data) - { - packet[i] = b; - i++; - } - - ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + MAVLINK_NUM_HEADER_BYTES); - - checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint) messageType).crc, - checksum); - - byte ck_a = (byte) (checksum & 0xFF); ///< High byte - byte ck_b = (byte) (checksum >> 8); ///< Low byte - - packet[i] = ck_a; - i += 1; - packet[i] = ck_b; - i += 1; - - if (MAVlist[sysid, compid].signing || forcesigning) - { - //https://docs.google.com/document/d/1ETle6qQRcaNWAmpG2wz0oOpFKSF_bcTmYMQvtTGI8ns/edit - - /* - 8 bits of link ID - 48 bits of timestamp - 48 bits of signature - */ - - // signature = sha256_48(secret_key + header + payload + CRC + link-ID + timestamp) - - var timestamp = (UInt64) ((DateTime.UtcNow - new DateTime(2015, 1, 1)).TotalMilliseconds * 100); - - if (timestamp == MAVlist[sysid, compid].timestamp) - timestamp++; - - MAVlist[sysid, compid].timestamp = timestamp; - - var timebytes = BitConverter.GetBytes(timestamp); - - var sig = new byte[7]; // 13 includes the outgoing hash - sig[0] = MAVlist[sysid, compid].sendlinkid; - Array.Copy(timebytes, 0, sig, 1, 6); // timestamp - - //Console.WriteLine("gen linkid {0}, time {1} {2} {3} {4} {5} {6} {7}", sig[0], sig[1], sig[2], sig[3], sig[4], sig[5], sig[6], timestamp); - - var signingKey = MAVlist[sysid, compid].signingKey; - - if (signingKey == null || signingKey.Length != 32) - { - signingKey = new byte[32]; - } - - using (SHA256CryptoServiceProvider signit = new SHA256CryptoServiceProvider()) - { - signit.TransformBlock(signingKey, 0, signingKey.Length, null, 0); - signit.TransformBlock(packet, 0, i, null, 0); - signit.TransformFinalBlock(sig, 0, sig.Length); - var ctx = signit.Hash; - // trim to 48 - Array.Resize(ref ctx, 6); - - foreach (byte b in sig) - { - packet[i] = b; - i++; - } - - foreach (byte b in ctx) - { - packet[i] = b; - i++; - } - } - } - } - - if (BaseStream.IsOpen) - { - BaseStream.Write(packet, 0, i); - _bytesSentSubj.OnNext(i); - } - - try - { - SaveToTlog(new Span(packet, 0, i)); - } - catch - { - } - } - try - { - // callback outside of lock - if(packet.Length > 0) - _OnPacketSent?.Invoke(this, new MAVLinkMessage(packet)); - } - catch - { - } - } - - public void SaveToTlog(Span packet) - { - try - { - if (logfile != null && logfile.CanWrite && !logreadmode) - { - lock (logfile) - { - byte[] datearray = - BitConverter.GetBytes( - (UInt64) ((DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds * 1000)); - Array.Reverse(datearray); - logfile.Write(datearray, 0, datearray.Length); - logfile.Write(packet.ToArray(), 0, packet.Length); - } - } - } - catch - { - } - } - - public bool Write(byte[] packet) - { - lock (writelock) - { - if (BaseStream.IsOpen) - { - BaseStream.Write(packet, 0, packet.Length); - _bytesSentSubj.OnNext(packet.Length); - } - - try - { - SaveToTlog(new Span(packet, 0, packet.Length)); - } - catch - { - } - } - try - { - // callback outside of lock - _OnPacketSent?.Invoke(this, new MAVLinkMessage(packet)); - } - catch - { - } - return true; - } - - public bool Write(string line) - { - lock (writelock) - { - BaseStream.Write(line); - } - - _bytesSentSubj.OnNext(line.Length); - return true; - } - - public bool setupSigning(byte sysid, byte compid, string userseed, byte[] key = null) - { - byte[] shauser; - bool clearkey = false; - - if (key == null) - { - clearkey = String.IsNullOrEmpty(userseed); - - // sha the user input string - using (SHA256CryptoServiceProvider signit = new SHA256CryptoServiceProvider()) - { - shauser = signit.ComputeHash(Encoding.UTF8.GetBytes(userseed)); - Array.Resize(ref shauser, 32); - } - } - else - { - shauser = key; - Array.Resize(ref shauser, 32); - } - - mavlink_setup_signing_t sign = new mavlink_setup_signing_t(); - if (!clearkey) - { - sign.initial_timestamp = - (UInt64) ((DateTime.UtcNow - new DateTime(2015, 1, 1)).TotalMilliseconds * 100); - sign.secret_key = shauser; - } - else - { - sign.initial_timestamp = 0; - sign.secret_key = new byte[32]; - } - - sign.target_component = (byte) compid; - sign.target_system = (byte) sysid; - - generatePacket((int) MAVLINK_MSG_ID.SETUP_SIGNING, sign, sysid, compid); - - generatePacket((int) MAVLINK_MSG_ID.SETUP_SIGNING, sign, sysid, compid); - - if (!clearkey) - { - // we will auto adapt to this key - so no need to assign it - //MAVlist[sysid, compid].signingKey = shauser; - } - else - { - // clear the key - MAVlist[sysid, compid].signingKey = null; - return disableSigning(sysid, compid); - } - - return enableSigning(sysid, compid); - } - - public bool enableSigning(int sysid, int compid) - { - MAVlist[sysid, compid].signing = true; - MAVlist[sysid, compid].mavlinkv2 = true; - - return MAVlist[sysid, compid].signing; - } - - public bool disableSigning(int sysid, int compid) - { - MAVlist[sysid, compid].signing = false; - MAVlist[sysid, compid].mavlinkv2 = false; - - return MAVlist[sysid, compid].signing; - } - - [Obsolete] - /// - /// set param on apm, used for param rename - /// - /// - /// - /// - public bool setParam(string[] paramnames, double value) - { - foreach (string paramname in paramnames) - { - if (setParam(paramname, value)) - { - return true; - } - } - - return false; - } - - [Obsolete] - public bool setParam(string paramname, double value, bool force = false) - { - return setParam((byte) sysidcurrent, (byte) compidcurrent, paramname, value, force); - } - - public bool setParam(byte sysid, byte compid, string paramname, double value, bool force = false) - { - return setParamAsync(sysid, compid, paramname, value, force).AwaitSync(); - } - - /// - /// Set parameter on apm - /// - /// name as a string - /// - public async Task setParamAsync(byte sysid, byte compid, string paramname, double value, - bool force = false) - { - if (!MAVlist[sysid, compid].param.ContainsKey(paramname)) - { - log.Warn("Trying to set Param that doesnt exist " + paramname + "=" + value); - return false; - } - - MAVLinkParam param = MAVlist[sysid, compid].param[paramname]; - if (param.Value == value && !force) - { - log.Warn("setParam " + paramname + " not modified as same"); - return true; - } - - giveComport = true; - - // param type is set here, however it is always sent over the air as a float 100int = 100f. - var req = new mavlink_param_set_t - { - target_system = sysid, - target_component = compid, - param_type = (byte) MAVlist[sysid, compid].param_types[paramname] - }; - - char[] temp = paramname.ToCharArray(); - - Array.Resize(ref temp, 16); - req.param_id = temp.ToByteArray(); - if ((MAVlist[sysid, compid].cs.capabilities & (uint) MAV_PROTOCOL_CAPABILITY.PARAM_FLOAT) > 0 || - MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA) - { - req.param_value = new MAVLinkParam(paramname, value, (MAV_PARAM_TYPE.REAL32)).float_value; - } - else - { - req.param_value = new MAVLinkParam(paramname, value, - (MAV_PARAM_TYPE) MAVlist[sysid, compid].param_types[paramname]).float_value; - } - - int currentparamcount = MAVlist[sysid, compid].param.Count; - - bool complete = false; - - var sub1 = SubscribeToPacketType(MAVLINK_MSG_ID.PARAM_VALUE, buffer => - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - mavlink_param_value_t par = buffer.ToStructure(); - - string st = Encoding.UTF8.GetString(par.param_id); - - int pos = st.IndexOf('\0'); - - if (pos != -1) - { - st = st.Substring(0, pos); - } - - if (st != paramname) - { - log.InfoFormat("MAVLINK bad param response - {0} vs {1}", paramname, st); - return true; - } - - if (MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA) - { - var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); - MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, BitConverter.GetBytes(par.param_value), - MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE) par.param_type, param.default_value); - } - else - { - var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); - MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, BitConverter.GetBytes(par.param_value), - (MAV_PARAM_TYPE) par.param_type, (MAV_PARAM_TYPE) par.param_type); - } - - log.Info("setParam gotback " + st + " : " + MAVlist[sysid, compid].param[st]); - - lastparamset = DateTime.UtcNow; - - // check if enabeling this param has added subparams, queue on gui thread - if (currentparamcount < par.param_count) - { - /* - MainV2.instance.BeginInvoke((Action) delegate - { - Loading.ShowLoading(String.Format(Strings.ParamRefreshRequired, currentparamcount, - par.param_count)); - }); - */ - } - - complete = true; - } - - return true; - }, sysid, compid); - - try - { - generatePacket((byte) MAVLINK_MSG_ID.PARAM_SET, req, sysid, compid); - - log.InfoFormat("setParam '{0}' = '{1}' sysid {2} compid {3}", paramname, value, sysid, - compid); - - DateTime start = DateTime.Now; - int retrys = 3; - while (true) - { - if (complete) - return true; - - if (!(start.AddMilliseconds(700) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("setParam Retry " + retrys); - generatePacket((byte) MAVLINK_MSG_ID.PARAM_SET, req, sysid, compid); - start = DateTime.Now; - retrys--; - continue; - } - - throw new TimeoutException("Timeout on read - setParam " + paramname); - } - - await readPacketAsync().ConfigureAwait(false); - } - } - finally - { - giveComport = false; - UnSubscribeToPacketType(sub1); - } - } - - /// - /// With GUI - /// - public void getParamList() - { - log.InfoFormat("getParamList {0} {1}", sysidcurrent, compidcurrent); - - frmProgressReporter = CreateIProgressReporterDialogue(Strings.GettingParams + " " + sysidcurrent); - - frmProgressReporter.DoWork += FrmProgressReporterGetParams; - frmProgressReporter.UpdateProgressAndStatus(-1, Strings.GettingParamsD); - - frmProgressReporter.RunBackgroundOperationAsync(); - - frmProgressReporter.Dispose(); - - _ParamListChanged?.Invoke(this, null); - } - - private void FrmProgressReporterGetParams(IProgressReporterDialogue sender) - { - getParamListMavftp(MAV.sysid, MAV.compid); - } - - /// - /// Get param list via mavftp with old style fallback - /// - /// - /// - /// MAVLinkParamList - public MAVLinkParamList getParamListMavftp(byte sysid, byte compid) - { - return getParamListMavftpAsync(sysid, compid).AwaitSync(); - } - - public async Task getParamListMavftpAsync(byte sysid, byte compid) - { - var sub2 = SubscribeToPacketType(MAVLINK_MSG_ID.STATUSTEXT, buffer => - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.STATUSTEXT) - { - var msg = buffer.ToStructure(); - - string logdata = Encoding.UTF8.GetString(msg.text); - - int ind = logdata.IndexOf('\0'); - if (ind != -1) - logdata = logdata.Substring(0, ind); - - if (logdata.ToLower().Contains("copter") || logdata.ToLower().Contains("rover") || - logdata.ToLower().Contains("plane")) - { - MAVlist[sysid, compid].VersionString = logdata; - // Version string may allow to better identify type - setAPType(sysid, compid); - } - else if (logdata.ToLower().Contains("nuttx") || logdata.ToLower().Contains("chibios")) - { - MAVlist[sysid, compid].SoftwareVersions = logdata; - } - else if (logdata.ToLower().Contains("px4v2") || - (Regex.IsMatch(logdata, @"\s[0-9A-F]+\s[0-9A-F]+\s[0-9A-F]+") && !logdata.Contains("IOMCU"))) - { - MAVlist[sysid, compid].SerialString = logdata; - } - else if (logdata.ToLower().Contains("frame")) - { - MAVlist[sysid, compid].FrameString = logdata; - } - else - { - } - } - - return true; - }, (byte)sysid, (byte)compid); - - // get the banner - doCommand((byte) sysid, (byte) compid, MAV_CMD.DO_SEND_BANNER, 0, 0, 0, 0, 0, 0, 0, - false); - - try - { - if (((MAVlist[sysid,compid].cs.capabilities & (int) MAV_PROTOCOL_CAPABILITY.FTP) > 0) && Settings.Instance.GetBoolean("UseMavFtpParams", true)) - { - if (frmProgressReporter != null) - frmProgressReporter.UpdateProgressAndStatus(-1, $"Checking for Param MAVFTP {sysid}-{compid}"); - var cancel = new CancellationTokenSource(); - var paramfileTask = Task.Run(() => - { - var ftp = new MAVFtp(this, sysid, compid); - ftp.Progress += (s,i) => - { - if (frmProgressReporter != null) - frmProgressReporter.UpdateProgressAndStatus(i, - $"Getting Param MAVFTP {sysid}-{compid} : {s}"); - }; - return ftp.GetFile( - // always try to get defualts, AP will send orginal format if not avalable - "@PARAM/param.pck?withdefaults=1", cancel, true, 110); - }); - while (!paramfileTask.IsCompleted) - { - if (frmProgressReporter != null) - if (frmProgressReporter.doWorkArgs.CancelRequested) - { - cancel.Cancel(); - frmProgressReporter.doWorkArgs.CancelAcknowledged = true; - return MAVlist[sysid, compid].param; - } - - await Task.Delay(10).ConfigureAwait(false); - } - - if (frmProgressReporter != null) - if (frmProgressReporter.doWorkArgs.CancelRequested) - return MAVlist[sysid, compid].param; - - var paramfile = await paramfileTask; - if (paramfile != null && paramfile.Length > 0) - { - var mavlist = parampck.unpack(paramfile.ToArray()); - if (mavlist != null) - { - MAVlist[sysid, compid].param.Clear(); - MAVlist[sysid, compid].param.TotalReported = - mavlist.Count; - MAVlist[sysid, compid].param.AddRange(mavlist); - var gen = new MavlinkParse(); - mavlist.ForEach(a => - { - MAVlist[sysid, compid].param_types[a.Name] = - a.Type; - SaveToTlog(gen.GenerateMAVLinkPacket10( - MAVLINK_MSG_ID.PARAM_VALUE, - new mavlink_param_value_t((float) a.Value, (ushort) mavlist.Count, - 0, - a.Name.MakeBytesSize(16), (byte) a.Type), sysid, compid)); - }); - - UnSubscribeToPacketType(sub2); - return MAVlist[sysid, compid].param; - } - } - } - } - catch (Exception e) - { - log.Error(e); - } - - UnSubscribeToPacketType(sub2); - return await getParamListAsync(sysid, compid); - } - - /// - /// Without GUI - /// - /// - /// - /// - public MAVLinkParamList getParamList(byte sysid, byte compid) - { - return getParamListAsync(sysid, compid).AwaitSync(); - } - - /// - /// Get param list from apm - /// - /// - public async Task getParamListAsync(byte sysid, byte compid) - { - List indexsreceived = new List(); - - // create new list so if canceled we use the old list - MAVLinkParamList newparamlist = new MAVLinkParamList(); - - int param_total = 1; - - mavlink_param_request_list_t req = new mavlink_param_request_list_t - { - target_system = sysid, - target_component = compid - }; - - DateTime start = DateTime.Now; - - DateTime lastmessage = DateTime.MinValue; - - //hires.Stopwatch stopwatch = new hires.Stopwatch(); - int retry = 0; - bool onebyone = false; - bool missing_params = false; - DateTime lastonebyone = DateTime.MinValue; - - var sub2 = SubscribeToPacketType(MAVLINK_MSG_ID.STATUSTEXT, buffer => - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.STATUSTEXT) - { - var msg = buffer.ToStructure(); - - string logdata = Encoding.UTF8.GetString(msg.text); - - int ind = logdata.IndexOf('\0'); - if (ind != -1) - logdata = logdata.Substring(0, ind); - - if (logdata.ToLower().Contains("copter") || logdata.ToLower().Contains("rover") || - logdata.ToLower().Contains("plane")) - { - MAVlist[sysid, compid].VersionString = logdata; - } - else if (logdata.ToLower().Contains("nuttx") || logdata.ToLower().Contains("chibios")) - { - MAVlist[sysid, compid].SoftwareVersions = logdata; - } - else if (logdata.ToLower().Contains("px4v2") || - (Regex.IsMatch(logdata, @"\s[0-9A-F]+\s[0-9A-F]+\s[0-9A-F]+") && !logdata.Contains("IOMCU"))) - { - MAVlist[sysid, compid].SerialString = logdata; - } - else if (logdata.ToLower().Contains("frame")) - { - MAVlist[sysid, compid].FrameString = logdata; - } - else - { - } - } - - return true; - }, (byte)sysid, (byte)compid); - - int seenvalid = 0; - - var sub1 = SubscribeToPacketType(MAVLINK_MSG_ID.PARAM_VALUE, buffer => - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - // if we are doing one by one dont update start time - if (!onebyone) - start = DateTime.Now; - - mavlink_param_value_t par = buffer.ToStructure(); - - // set new target - param_total = (par.param_count); - newparamlist.TotalReported = param_total; - - if (param_total == 0) - return true; - - string paramID = Encoding.UTF8.GetString(par.param_id); - - int pos = paramID.IndexOf('\0'); - if (pos != -1) - { - paramID = paramID.Substring(0, pos); - } - - // check if we already have it - if (indexsreceived.Contains(par.param_index)) - { - log.Info("Already got " + (par.param_index) + " '" + paramID + "' " + - (indexsreceived.Count * 100) / param_total); - if (frmProgressReporter != null) - this.frmProgressReporter.UpdateProgressAndStatus( - (indexsreceived.Count * 100) / param_total, "Already Got param " + paramID); - return true; - } - - seenvalid++; - //Console.WriteLine(DateTime.Now.Millisecond + " gp2 "); - - log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + - (par.param_count) + " name: " + paramID + " " + par.param_value); - - //Console.WriteLine(DateTime.Now.Millisecond + " gp2a "); - - // item uses float based param system - if ((MAVlist[sysid, compid].cs.capabilities & (uint) MAV_PROTOCOL_CAPABILITY.PARAM_FLOAT) > 0 || - MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA) - { - var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); - newparamlist[paramID] = new MAVLinkParam(paramID, BitConverter.GetBytes(par.param_value), - MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE) par.param_type); - } - else - { - var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); - newparamlist[paramID] = new MAVLinkParam(paramID, BitConverter.GetBytes(par.param_value), - (MAV_PARAM_TYPE) par.param_type, (MAV_PARAM_TYPE) par.param_type); - } - - //Console.WriteLine(DateTime.Now.Millisecond + " gp2b "); - - // exclude index of 65535 - if (par.param_index != 65535) - indexsreceived.Add(par.param_index); - - lock(MAVlist[sysid, compid].param_types) - MAVlist[sysid, compid].param_types[paramID] = (MAV_PARAM_TYPE) par.param_type; - - //Console.WriteLine(DateTime.Now.Millisecond + " gp3 "); - - if (frmProgressReporter != null) - this.frmProgressReporter.UpdateProgressAndStatus((indexsreceived.Count * 100) / param_total, - Strings.Gotparam + paramID); - - // we hit the last param, jump striaght to retry if params missing - if (par.param_index == (param_total - 1)) - missing_params |= indexsreceived.Count < param_total; - } - - return true; - }, (byte)sysid, (byte)compid); - - - generatePacket((byte)MAVLINK_MSG_ID.PARAM_REQUEST_LIST, req); - - short tenbytenindex = 0; - - do - { - if (frmProgressReporter != null && frmProgressReporter.doWorkArgs.CancelRequested) - { - frmProgressReporter.doWorkArgs.CancelAcknowledged = true; - - UnSubscribeToPacketType(sub1); - UnSubscribeToPacketType(sub2); - frmProgressReporter.doWorkArgs.ErrorMessage = "User Canceled"; - return MAVlist[sysid, compid].param; - } - - // 4 seconds between valid packets or got to last param and have some missing - if ((missing_params || !(start.AddMilliseconds(4000) > DateTime.Now)) && !logreadmode) - { - // if we have less than 75% of the total use full list pull - if (!onebyone && (retry < 2) && ((param_total <= 1) || (indexsreceived.Count < ((param_total / 4) * 3)))) - { - if (param_total > 1) - { - // only increment retry counter if we have a valid total - // otherwise keep requesting full list - retry++; - } - log.InfoFormat("Get Param whole list retry {0} got {1} 75%={2} count {3}", retry, - indexsreceived.Count, ((param_total / 4) * 3), param_total); - generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_LIST, req); - start = DateTime.Now; - missing_params = false; - continue; - } - - onebyone = true; - - if (lastonebyone.AddMilliseconds(1000) < DateTime.Now) - { - int queued = 0; - short startindex = 0; - var backupseenvalid = seenvalid; - if (param_total > 1) - startindex = (short) (tenbytenindex % (param_total - 1)); - - seenvalid = 0; - - // seeing more than what we requested... skip this round - maybe full param list is still inflight - if(backupseenvalid > 10) - continue; - - log.Info("Get param 10 by 10 - got " + indexsreceived.Count + " of " + param_total + - " request from startindex " + startindex + " seen since last requests " + backupseenvalid); - - // try getting individual params - bool found_missing_param = false; - for (short i = startindex; i <= (param_total - 1); i++) - { - if (!indexsreceived.Contains(i)) - { - found_missing_param = true; - - if (frmProgressReporter != null && frmProgressReporter.doWorkArgs.CancelRequested) - { - frmProgressReporter.doWorkArgs.CancelAcknowledged = true; - UnSubscribeToPacketType(sub1); - UnSubscribeToPacketType(sub2); - frmProgressReporter.doWorkArgs.ErrorMessage = "User Canceled"; - return MAVlist[sysid, compid].param; - } - - // prevent dropping out of this get params loop - try - { - queued++; - tenbytenindex = (short)(i + 1); - - mavlink_param_request_read_t req2 = new mavlink_param_request_read_t - { - target_system = sysid, - target_component = compid, - param_index = i, - param_id = new byte[] {0} - }; - - Array.Resize(ref req2.param_id, 16); - - generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_READ, req2); - - if (queued >= 10) - { - break; - } - } - catch (Exception excp) - { - log.Info("GetParam Failed index: " + i + " " + excp); - UnSubscribeToPacketType(sub1); - UnSubscribeToPacketType(sub2); - throw excp; - } - } - } - if (!found_missing_param) - { - // no missing param after start index, start from the begining - tenbytenindex = 0; - } - - lastonebyone = DateTime.Now; - } - } - - await readPacketAsync(); - - if (logreadmode && logplaybackfile.BaseStream.Position >= logplaybackfile.BaseStream.Length) - { - break; - } - - if (!logreadmode && !BaseStream.IsOpen) - { - var exp = new Exception("Not Connected"); - frmProgressReporter.doWorkArgs.ErrorMessage = exp.Message; - UnSubscribeToPacketType(sub1); - UnSubscribeToPacketType(sub2); - throw exp; - } - } while (indexsreceived.Count < param_total); - - if (indexsreceived.Count != param_total) - { - var exp = new Exception("Missing Params " + indexsreceived.Count + " vs " + param_total); - frmProgressReporter.doWorkArgs.ErrorMessage = exp.Message; - UnSubscribeToPacketType(sub1); - UnSubscribeToPacketType(sub2); - throw exp; - } - - UnSubscribeToPacketType(sub1); - UnSubscribeToPacketType(sub2); - - log.InfoFormat("list count {0}, rcvd total {1} indexseen count {2}", newparamlist.Count, param_total, - indexsreceived.Count); - - MAVlist[sysid, compid].param.Clear(); - MAVlist[sysid, compid].param.TotalReported = param_total; - MAVlist[sysid, compid].param.AddRange(newparamlist); - return MAVlist[sysid, compid].param; - } - - private int _parampoll = 0; - - public void getParamPoll() - { - // check if we have all - if (MAV.param.TotalReceived >= MAV.param.TotalReported) - { - return; - } - - // if we are connected as primary to a vehicle where we dont have all the params, poll for them - short i = (short) (_parampoll % MAV.param.TotalReported); - - GetParam((byte) sysidcurrent, (byte) compidcurrent, "", i, false); - - _parampoll++; - } - - [Obsolete] - public float GetParam(string name) - { - return GetParam(name, -1); - } - - [Obsolete] - public float GetParam(short index) - { - return GetParam("", index); - } - - [Obsolete] - public float GetParam(string name = "", short index = -1, bool requireresponce = true) - { - return GetParam(MAV.sysid, MAV.compid, name, index, requireresponce); - } - - public float GetParam(byte sysid, byte compid, string name = "", short index = -1, bool requireresponce = true) - { - return GetParamAsync(sysid, compid, name, index, requireresponce).AwaitSync(); - } - - /// - /// Get param by either index or name - /// - /// - /// - /// - public async Task GetParamAsync(byte sysid, byte compid, string name = "", short index = -1, - bool requireresponce = true) - { - if (name == "" && index == -1) - return 0; - - log.Info("GetParam name: '" + name + "' or index: " + index + " " + sysid + ":" + compid); - - MAVLinkMessage buffer; - giveComport = true; - mavlink_param_request_read_t req = new mavlink_param_request_read_t - { - target_system = sysid, - target_component = compid, - param_index = index, - param_id = new byte[] {0} - }; - if (index == -1) - { - req.param_id = name.MakeBytes(); - } - - Array.Resize(ref req.param_id, 16); - - generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_READ, req, sysid, compid); - - if (!requireresponce) - { - giveComport = false; - return 0f; - } - - DateTime start = DateTime.Now; - int retrys = 3; - - while (true) - { - if (!(start.AddMilliseconds(700) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("GetParam Retry " + retrys); - generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_READ, req, sysid, compid); - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - throw new TimeoutException("Timeout on read - GetParam"); - } - - buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - mavlink_param_value_t par = buffer.ToStructure(); - - string st = Encoding.UTF8.GetString(par.param_id); - - int pos = st.IndexOf('\0'); - - if (pos != -1) - { - st = st.Substring(0, pos); - } - - // not the correct id - if (!(par.param_index == index || st == name)) - { - log.ErrorFormat("Wrong Answer {0} - {1} - {2} --- '{3}' vs '{4}'", par.param_index, - Encoding.UTF8.GetString(par.param_id), par.param_value, - Encoding.UTF8.GetString(req.param_id).TrimEnd(), st); - continue; - } - - // update table - if (MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA) - { - var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); - MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, - BitConverter.GetBytes(par.param_value), MAV_PARAM_TYPE.REAL32, - (MAV_PARAM_TYPE) par.param_type); - } - else - { - var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); - MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, - BitConverter.GetBytes(par.param_value), (MAV_PARAM_TYPE) par.param_type, - (MAV_PARAM_TYPE) par.param_type); - } - - lock(MAVlist[sysid, compid].param_types) - MAVlist[sysid, compid].param_types[st] = (MAV_PARAM_TYPE) par.param_type; - - log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + - (par.param_count) + " name: " + st); - - giveComport = false; - - return par.param_value; - } - } - } - } - - /// - /// Stops all requested data packets. - /// - public void stopall(bool forget) - { - mavlink_request_data_stream_t req = new mavlink_request_data_stream_t - { - target_system = MAV.sysid, - target_component = MAV.compid, - req_message_rate = 10, - start_stop = 0, - req_stream_id = 0 - }; - - // no error on bad - try - { - generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); - Thread.Sleep(20); - generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); - Thread.Sleep(20); - generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); - log.Info("Stopall Done"); - } - catch - { - } - } - - [Obsolete] - public void setWPACK(MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - setWPACK(MAV.sysid, MAV.compid, type); - } - - public void setWPACK(byte sysid, byte compid, MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - mavlink_mission_ack_t req = new mavlink_mission_ack_t - { - target_system = sysid, - target_component = compid, - type = 0, - mission_type = (byte) type - }; - - generatePacket((byte) MAVLINK_MSG_ID.MISSION_ACK, req); - } - - public bool setWPCurrent(byte sysid, byte compid, ushort index) - { - return setWPCurrentAsync(sysid, compid, index).AwaitSync(); - } - - public async Task setWPCurrentAsync(byte sysid, byte compid, ushort index) - { - giveComport = true; - MAVLinkMessage buffer; - - mavlink_mission_set_current_t req = new mavlink_mission_set_current_t - { - target_system = sysid, - target_component = compid, - seq = index - }; - - generatePacket((byte) MAVLINK_MSG_ID.MISSION_SET_CURRENT, req); - - DateTime start = DateTime.Now; - int retrys = 5; - - while (true) - { - if (!(start.AddMilliseconds(2000) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("setWPCurrent Retry " + retrys); - generatePacket((byte) MAVLINK_MSG_ID.MISSION_SET_CURRENT, req); - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - throw new TimeoutException("Timeout on read - setWPCurrent"); - } - - buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_CURRENT && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - giveComport = false; - return true; - } - } - } - } - - [Obsolete("Mavlink 09 - use doCommand", true)] - public bool doAction(object actionid) - { - // mavlink 09 - throw new NotImplementedException(); - } - - public bool doDFUBoot(byte sysid, byte compid) - { - doCommand(sysid, compid, MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, - 42, 24, 71, 99, 0, 0, 0, false); - - return true; - } - - /// - /// Control high latency mode - /// - /// true to enable, false to disable - /// - public bool doHighLatency(bool onoff= false) - { - int param1 = onoff ? 1 : 0; - - var ans1 = doCommand((byte)sysidcurrent, (byte)compidcurrent, MAV_CMD.CONTROL_HIGH_LATENCY, - param1, 0, 0, 0, 0, 0, 0); - - return ans1; - } - - /// - /// Control ICE Engine - /// - /// true to enable, false to disable - /// - public bool doEngineControl(byte sysid, byte compid, bool onoff = false) - { - int param1 = onoff ? 1 : 0; - - return doCommand(sysid, compid, MAV_CMD.DO_ENGINE_CONTROL, - param1, 0, 0, 0, 0, 0, 0); - } - - /// - /// reboot the vehicle - /// - /// reboot into bootloader mode? - /// use current sysid/compid or scan for it - /// - public bool doReboot(bool bootloadermode = false, bool currentvehicle = true) - { - int param1 = 1; - if (bootloadermode) - { - param1 = 3; - } - - // reboot the current selected mav - if (currentvehicle) - { - var ans1 = doCommand((byte) sysidcurrent, (byte) compidcurrent, MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, - param1, 0, 0, 0, 0, 0, 0); - var ans2 = false; - if (!ans1) - { - ans2 = doCommand((byte)sysidcurrent, (byte)compidcurrent, MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 0, - 0, 0, 0, 0, 0); - } - // Successful reboot - if (ans1 || ans2) - { - // Not going into bootloader and on a serial connection - if (!bootloadermode && (BaseStream is SerialPort)) - { - // Direct USB will disconnect after a reboot, wait and see if we should re-connect - Thread.Sleep(500); - if (!BaseStream.IsOpen) - { - Open(true); - } - } - return true; - } - - giveComport = false; - return false; - } - else - { - // scan for hb on unknown mav - MAVLinkMessage buffer = getHeartBeat(); - - if (buffer.Length > 5) - { - mavlink_heartbeat_t hb = buffer.ToStructure(); - - mavlinkversion = hb.mavlink_version; - MAV.aptype = (MAV_TYPE) hb.type; - MAV.apname = (MAV_AUTOPILOT) hb.autopilot; - MAV.sysid = buffer.sysid; - MAV.compid = buffer.compid; - } - - // reboot if we have seen hb - if (MAV.sysid != 0 && MAV.compid != 0) - { - doCommand((byte) sysidcurrent, (byte) compidcurrent, MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, param1, 0, - 0, 0, 0, 0, 0); - doCommand((byte) sysidcurrent, (byte) compidcurrent, MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 0, 0, 0, - 0, 0, 0); - } - } - - giveComport = false; - return true; - } - - [Obsolete] - public bool doARM(bool armit, bool force = false) - { - return doARM(MAV.sysid, MAV.compid, armit, force); - } - - public bool doARM(byte sysid, byte compid, bool armit, bool force = false) - { - return doARMAsync(sysid, compid, armit, force).AwaitSync(); - } - - public async Task doARMAsync(byte sysid, byte compid, bool armit, bool force = false) - { - const float magic_force_arm_value = 2989.0f; - const float magic_force_disarm_value = 21196.0f; - - if (force) - { - if (armit) - return await doCommandAsync(sysid, compid, MAV_CMD.COMPONENT_ARM_DISARM, 1, magic_force_arm_value, - 0, 0, 0, 0, - 0).ConfigureAwait(false); - else - return await doCommandAsync(sysid, compid, MAV_CMD.COMPONENT_ARM_DISARM, 0, - magic_force_disarm_value, 0, 0, 0, - 0, 0).ConfigureAwait(false); - } - else - { - if (armit) - return await doCommandAsync(sysid, compid, MAV_CMD.COMPONENT_ARM_DISARM, 1, 0, 0, 0, 0, 0, 0) - .ConfigureAwait(false); - else - return await doCommandAsync(sysid, compid, MAV_CMD.COMPONENT_ARM_DISARM, 0, 0, 0, 0, 0, - 0, 0).ConfigureAwait(false); - } - } - - [Obsolete] - public bool doAbortLand() - { - return doCommand(MAV_CMD.DO_GO_AROUND, 0, 0, 0, 0, 0, 0, 0); - } - - [Obsolete] - public bool doMotorTest(int motor, MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout, - int motorcount = 0) - { - return doCommand(MAV_CMD.DO_MOTOR_TEST, (float) motor, (float) (byte) thr_type, - (float) throttle, (float) timeout, (float) motorcount, 0, 0); - } - - [Obsolete] - public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7, - bool requireack = true) - { - return doCommand(MAV.sysid, MAV.compid, actionid, p1, p2, p3, p4, p5, p6, p7, requireack, null); - } - - public bool doCommand(byte sysid, byte compid, MAV_CMD actionid, float p1, float p2, float p3, - float p4, - float p5, float p6, float p7, bool requireack = true, Action uicallback = null) - { - return doCommandAsync(sysid, compid, actionid, p1, p2, p3, p4, p5, p6, p7, requireack, uicallback) - .AwaitSync(); - } - - public async Task doCommandAsync(byte sysid, byte compid, MAV_CMD actionid, float p1, float p2, float p3, - float p4, - float p5, float p6, float p7, bool requireack = true, Action uicallback = null) - { - if (BaseStream == null || BaseStream.IsOpen == false) - return false; - - MAVLinkMessage buffer; - - mavlink_command_long_t req = new mavlink_command_long_t - { - target_system = sysid, - target_component = compid, - command = (ushort) actionid, - confirmation = 0, - param1 = p1, - param2 = p2, - param3 = p3, - param4 = p4, - param5 = p5, - param6 = p6, - param7 = p7 - }; - - log.InfoFormat("doCommand cmd {0} {1} {2} {3} {4} {5} {6} {7}", actionid.ToString(), p1, p2, p3, p4, p5, p6, - p7); - - if (requireack) - giveComport = true; - - generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid); - - if (!requireack) - { - giveComport = false; - return true; - } - - DateTime GUI = DateTime.Now; - - DateTime start = DateTime.Now; - int retrys = 3; - - int timeout = 2000; - - // imu calib take a little while - if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1) - { - // this is for advanced accel offsets, and blocks execution - giveComport = false; - return true; - } - else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p6 == 1) - { - // compassmot - // send again just incase - generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid); - giveComport = false; - return true; - } - else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION) - { - retrys = 1; - timeout = 25000; - } - else if (actionid == MAV_CMD.FLASH_BOOTLOADER) - { - retrys = 1; - timeout = 25000; - } - else if (actionid == MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN) - { - generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid); - giveComport = false; - return true; - } - else if (actionid == MAV_CMD.COMPONENT_ARM_DISARM) - { - // 10 seconds as may need an imu calib - timeout = 10000; - } - else if (actionid == MAV_CMD.GET_HOME_POSITION) - { - giveComport = false; - return true; - } - - while (true) - { - if (DateTime.Now > GUI.AddMilliseconds(100)) - { - GUI = DateTime.Now; - - uicallback?.Invoke(); - } - - if (!(start.AddMilliseconds(timeout) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("doCommand Retry " + retrys); - req.confirmation++; - generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid); - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - throw new TimeoutException("Timeout on read - doCommand"); - } - - buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.COMMAND_ACK && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - var ack = buffer.ToStructure(); - - if (ack.command != req.command) - { - log.InfoFormat("doCommand cmd resp {0} - {1} - Commands dont match", (MAV_CMD) ack.command, - (MAV_RESULT) ack.result); - continue; - } - - log.InfoFormat("doCommand cmd resp {0} - {1}", (MAV_CMD) ack.command, (MAV_RESULT) ack.result); - - - if (ack.result == (byte)MAV_RESULT.IN_PROGRESS) - { - start = DateTime.Now; - retrys = 0; - continue; - } - else if (ack.result == (byte) MAV_RESULT.ACCEPTED) - { - giveComport = false; - return true; - } - else - { - giveComport = false; - return false; - } - } - } - } - } - - public bool doCommandInt(byte sysid, byte compid, MAV_CMD actionid, float p1, float p2, float p3, float p4, - int p5, int p6, float p7, bool requireack = true, Action uicallback = null, - MAV_FRAME frame = MAV_FRAME.GLOBAL) - { - return doCommandIntAsync(sysid, compid, actionid, p1, p2, p3, p4, p5, p6, p7, requireack, uicallback, frame) - .AwaitSync(); - } - - public async Task doCommandIntAsync(byte sysid, byte compid, MAV_CMD actionid, float p1, float p2, - float p3, float p4, - int p5, int p6, float p7, bool requireack = true, Action uicallback = null, - MAV_FRAME frame = MAV_FRAME.GLOBAL) - { - if (BaseStream == null || BaseStream.IsOpen == false) - return false; - - MAVLinkMessage buffer; - - mavlink_command_int_t req = new mavlink_command_int_t() - { - target_system = sysid, - target_component = compid, - - command = (ushort) actionid, - - param1 = p1, - param2 = p2, - param3 = p3, - param4 = p4, - x = p5, - y = p6, - z = p7, - autocontinue = 0, - current = 0, - frame = (byte) frame - }; - - log.InfoFormat("doCommandIntAsync cmd {0} {1} {2} {3} {4} {5} {6} {7}", actionid.ToString(), p1, p2, p3, p4, - p5, p6, - p7); - - if (requireack) - giveComport = true; - - generatePacket((byte) MAVLINK_MSG_ID.COMMAND_INT, req, sysid, compid); - - if (!requireack) - { - giveComport = false; - return true; - } - - DateTime GUI = DateTime.Now; - - DateTime start = DateTime.Now; - int retrys = 3; - - int timeout = 2000; - - while (true) - { - if (DateTime.Now > GUI.AddMilliseconds(100)) - { - GUI = DateTime.Now; - - uicallback?.Invoke(); - } - - if (!(start.AddMilliseconds(timeout) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("doCommandIntAsync Retry " + retrys); - generatePacket((byte) MAVLINK_MSG_ID.COMMAND_INT, req, sysid, compid); - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - throw new TimeoutException("Timeout on read - doCommand"); - } - - buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.COMMAND_ACK && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - var ack = buffer.ToStructure(); - - if (ack.command != req.command) - { - log.InfoFormat("doCommandIntAsync cmd resp {0} - {1} - Commands dont match", - (MAV_CMD) ack.command, - (MAV_RESULT) ack.result); - continue; - } - - log.InfoFormat("doCommandIntAsync cmd resp {0} - {1}", (MAV_CMD) ack.command, - (MAV_RESULT) ack.result); - - if (ack.result == (byte) MAV_RESULT.ACCEPTED) - { - giveComport = false; - return true; - } - else - { - giveComport = false; - return false; - } - } - } - } - } - - public void SendAck() - { - mavlink_command_ack_t ack = new mavlink_command_ack_t(); - ack.command = (ushort) MAV_CMD.PREFLIGHT_CALIBRATION; - ack.result = 0; - - // send twice - generatePacket(MAVLINK_MSG_ID.COMMAND_ACK, ack); - Thread.Sleep(20); - generatePacket(MAVLINK_MSG_ID.COMMAND_ACK, ack); - } - - public void SendSerialControl(SERIAL_CONTROL_DEV port, ushort timeoutms, byte[] data, uint baudrate = 0, - bool close = false) - { - mavlink_serial_control_t ctl = new mavlink_serial_control_t - { - baudrate = baudrate, - device = (byte) port, - timeout = timeoutms, - data = new byte[70], - count = 0 - }; - - if (close) - { - ctl.flags = 0; - } - else - { - ctl.flags = (byte) SERIAL_CONTROL_FLAG.EXCLUSIVE; // | SERIAL_CONTROL_FLAG.MULTI); - } - - if (data != null && data.Length != 0) - { - int packets = (data.Length / 70) + 1; - int len = data.Length; - while (len > 0) - { - if (packets == 1) - ctl.flags |= (byte) SERIAL_CONTROL_FLAG.RESPOND; - - byte n = (byte) Math.Min(70, len); - - ctl.count = n; - Array.Copy(data, data.Length - len, ctl.data, 0, n); - - // dont flood the port - Thread.Sleep(10); - - generatePacket((byte) MAVLINK_MSG_ID.SERIAL_CONTROL, ctl); - - len -= n; - packets--; - } - } - else - { - if (!close) - ctl.flags |= (byte) SERIAL_CONTROL_FLAG.RESPOND | (byte) SERIAL_CONTROL_FLAG.MULTI; - - generatePacket((byte) MAVLINK_MSG_ID.SERIAL_CONTROL, ctl); - } - } - - public void SendRCOverride(byte sysid, byte compid, UInt16 rc1, UInt16 rc2, UInt16 rc3, UInt16 rc4, UInt16 rc5, - UInt16 rc6, UInt16 rc7, UInt16 rc8) - { - mavlink_rc_channels_override_t rc = new mavlink_rc_channels_override_t - { - target_component = compid, - target_system = sysid, - chan1_raw = rc1, - chan2_raw = rc2, - chan3_raw = rc3, - chan4_raw = rc4, - chan5_raw = rc5, - chan6_raw = rc6, - chan7_raw = rc7, - chan8_raw = rc8 - }; - - sendPacket(rc, rc.target_system, rc.target_component); - } - - public void SendManualControl(byte sysid, byte compid, Int16 x, Int16 y, Int16 z, Int16 r, UInt16 buttons) - { - mavlink_manual_control_t mc = new mavlink_manual_control_t - { - target = sysid, - x = x, - y = y, - z = z, - r = r, - buttons = buttons - }; - - sendPacket(mc, sysid, compid); - } - - public void requestDatastream(MAV_DATA_STREAM id, byte hzrate, int sysid = -1, int compid = -1) - { - requestDatastream(id, (int) hzrate, sysid, compid); - } - - public void requestDatastream(MAV_DATA_STREAM id, int hzrate, int sysid = -1, int compid = -1) - { - if (sysid == -1) - sysid = sysidcurrent; - - if (compid == -1) - compid = compidcurrent; - - if (hzrate == -1) - return; - - double pps = 0; - - switch (id) - { - case MAV_DATA_STREAM.ALL: - - break; - - case MAV_DATA_STREAM.EXTENDED_STATUS: - if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.SYS_STATUS)) - { - if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.SYS_STATUS] < - DateTime.UtcNow.AddSeconds(-2)) - break; - pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.SYS_STATUS]; - } - - if (hzratecheck(pps, hzrate)) - { - return; - } - - break; - - case MAV_DATA_STREAM.EXTRA1: - if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.ATTITUDE)) - { - if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.ATTITUDE] < - DateTime.UtcNow.AddSeconds(-2)) - break; - pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.ATTITUDE]; - } - - if (hzratecheck(pps, hzrate)) - { - return; - } - - break; - - case MAV_DATA_STREAM.EXTRA2: - if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.VFR_HUD)) - { - if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.VFR_HUD] < - DateTime.UtcNow.AddSeconds(-2)) - break; - pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.VFR_HUD]; - } - - if (hzratecheck(pps, hzrate)) - { - return; - } - - break; - - case MAV_DATA_STREAM.EXTRA3: - if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.AHRS)) - { - if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.AHRS] < - DateTime.UtcNow.AddSeconds(-2)) - break; - pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.AHRS]; - } - - if (hzratecheck(pps, hzrate)) - { - return; - } - - break; - - case MAV_DATA_STREAM.POSITION: - if (MAVlist[sysid, compid] - .packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.GLOBAL_POSITION_INT)) - { - if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.GLOBAL_POSITION_INT] < - DateTime.UtcNow.AddSeconds(-2)) - break; - pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.GLOBAL_POSITION_INT]; - } - - if (hzratecheck(pps, hzrate)) - { - return; - } - - break; - - case MAV_DATA_STREAM.RAW_CONTROLLER: - if (MAVlist[sysid, compid] - .packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.RC_CHANNELS_SCALED)) - { - if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RC_CHANNELS_SCALED] < - DateTime.UtcNow.AddSeconds(-2)) - break; - pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RC_CHANNELS_SCALED]; - } - - if (hzratecheck(pps, hzrate)) - { - return; - } - - break; - - case MAV_DATA_STREAM.RAW_SENSORS: - if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.RAW_IMU)) - { - if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RAW_IMU] < - DateTime.UtcNow.AddSeconds(-2)) - break; - pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RAW_IMU]; - } - - if (hzratecheck(pps, hzrate)) - { - return; - } - - break; - - case MAV_DATA_STREAM.RC_CHANNELS: - if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.RC_CHANNELS_RAW)) - { - if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RC_CHANNELS_RAW] < - DateTime.UtcNow.AddSeconds(-2)) - break; - pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RC_CHANNELS_RAW]; - } - - if (hzratecheck(pps, hzrate)) - { - return; - } - - break; - } - - if (pps == 0 && hzrate == 0) - { - return; - } - - log.InfoFormat("Request stream {0} at {1} hz for {2}:{3}", - Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate, sysid, compid); - getDatastream((byte) sysid, (byte) compid, id, hzrate); - } - - // returns true for ok - private bool hzratecheck(double pps, int hzrate) - { - if (double.IsInfinity(pps)) - { - return false; - } - // 0 request - else if (hzrate == 0 && pps == 0) - { - return true; - } - - // range check pps, include packetloss - if (pps > hzrate - 1 && pps < hzrate + 0.1) - return true; - - return false; - } - - [Obsolete] - private void getDatastream(MAV_DATA_STREAM id, int hzrate) - { - getDatastream(MAV.sysid, MAV.compid, id, hzrate); - } - - private void getDatastream(byte sysid, byte compid, MAV_DATA_STREAM id, int hzrate) - { - if (hzrate == -1) - return; - - mavlink_request_data_stream_t req = new mavlink_request_data_stream_t - { - target_system = sysid, - target_component = compid, - req_message_rate = (byte) hzrate, - start_stop = 1, - req_stream_id = (byte) id - }; - - // send each one twice. - generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req, sysid, compid); - generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req, sysid, compid); - } - - [Obsolete] - public ushort getWPCount(MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - return getWPCountAsync(MAV.sysid, MAV.compid, type).AwaitSync(); - } - - public ushort getWPCount(byte sysid, byte compid, MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - return getWPCountAsync(sysid, compid, type).AwaitSync(); - } - - /// - /// Returns WP count - /// - /// - public async Task getWPCountAsync(byte sysid, byte compid, - MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - giveComport = true; - MAVLinkMessage buffer; - mavlink_mission_request_list_t req = new mavlink_mission_request_list_t - { - target_system = sysid, - target_component = compid, - mission_type = (byte) type - }; - - // request list - generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req); - - DateTime start = DateTime.Now; - int retrys = 6; - - while (true) - { - if (!(start.AddMilliseconds(700) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("getWPCount Retry " + retrys + " - giv com " + giveComport); - generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req); - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - //return (byte)int.Parse(param["WP_TOTAL"].ToString()); - throw new TimeoutException("Timeout on read - getWPCount"); - } - - buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_COUNT && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - var count = buffer.ToStructure(); - // check this gcs sent it - if (count.target_system != gcssysid || - count.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - log.Info("wpcount: " + count.count); - giveComport = false; - return count.count; // should be ushort, but apm has limited wp count < byte - } - } - } - } - - [Obsolete] - public Locationwp getHomePosition() - { - return getHomePositionAsync((byte) sysidcurrent, (byte) compidcurrent).AwaitSync(); - } - - public async Task getHomePositionAsync(byte sysid, byte compid) - { - Locationwp? ans = null; - - var sub1 = SubscribeToPacketType(MAVLINK_MSG_ID.HOME_POSITION, buffer => - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.HOME_POSITION) - { - var home = buffer.ToStructure(); - - ans = new Locationwp().Set(home.latitude / 1.0e7, home.longitude / 1.0e7, home.altitude / 1000.0, - (byte) MAV_CMD.WAYPOINT); - } - - return true; - }, (byte)sysid, (byte)compid); - - try - { - doCommand(sysid, compid, MAV_CMD.GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, false); - - giveComport = true; - DateTime start = DateTime.Now; - int retrys = 3; - - while (true) - { - if (ans != null) - return ans.Value; - - if (!(start.AddMilliseconds(700) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("getHomePosition Retry " + retrys + " - giv com " + giveComport); - doCommand(sysid, compid, MAV_CMD.GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, false); - start = DateTime.Now; - retrys--; - continue; - } - - throw new TimeoutException("Timeout on read - getHomePosition"); - } - - await readPacketAsync().ConfigureAwait(false); - } - } - finally - { - giveComport = false; - UnSubscribeToPacketType(sub1); - } - } - - [Obsolete] - public Locationwp getWP(ushort index, MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - return getWP(MAV.sysid, MAV.compid, index, type); - } - - public Locationwp getWP(byte sysid, byte compid, ushort index, - MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - return getWPAsync(sysid, compid, index, type).AwaitSync(); - } - - /// - /// Gets specfied WP - /// - /// - /// WP - public async Task getWPAsync(byte sysid, byte compid, ushort index, - MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - while (giveComport == true) - Thread.Sleep(10); - - bool use_int = (MAVlist[sysid, compid].cs.capabilities & (uint) MAV_PROTOCOL_CAPABILITY.MISSION_INT) > 0; - - object req; - - if (use_int) - { - mavlink_mission_request_int_t reqi = new mavlink_mission_request_int_t - { - target_system = sysid, - target_component = compid, - mission_type = (byte) type, - seq = index - }; - - // request - generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT, reqi); - - req = reqi; - } - else - { - mavlink_mission_request_t reqf = new mavlink_mission_request_t - { - target_system = sysid, - target_component = compid, - mission_type = (byte) type, - seq = index - }; - - // request - generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST, reqf); - - req = reqf; - } - - giveComport = true; - Locationwp loc = new Locationwp(); - - DateTime start = DateTime.Now; - int retrys = 5; - - while (true) - { - if (!(start.AddMilliseconds(2500) > DateTime.Now)) // apm times out after 5000ms - { - if (retrys > 0) - { - log.Info("getWP Retry " + retrys); - if (use_int) - generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT, req); - else - generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST, req); - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - throw new TimeoutException("Timeout on read - getWP"); - } - - //Console.WriteLine("getwp read " + DateTime.Now.Millisecond); - MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); - //Console.WriteLine("getwp readend " + DateTime.Now.Millisecond); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM && buffer.sysid == sysid && - buffer.compid == compid) - { - //Console.WriteLine("getwp ans " + DateTime.Now.Millisecond); - - var wp = buffer.ToStructure(); - if (wp.target_system != gcssysid || - wp.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - // received a packet, but not what we requested - if (index != wp.seq) - { - generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST, req); - continue; - } - - loc.frame = wp.frame; - loc.id = (ushort) (wp.command); - loc.p1 = (wp.param1); - loc.p2 = (wp.param2); - loc.p3 = (wp.param3); - loc.p4 = (wp.param4); - - loc.alt = ((wp.z)); - loc.lat = ((wp.x)); - loc.lng = ((wp.y)); - - log.InfoFormat("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng, - loc.frame); - - break; - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM_INT && buffer.sysid == sysid && - buffer.compid == compid) - { - //Console.WriteLine("getwp ans " + DateTime.Now.Millisecond); - - var wp = buffer.ToStructure(); - // check this gcs sent it - if (wp.target_system != gcssysid || - wp.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - // received a packet, but not what we requested - if (index != wp.seq) - { - generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT, req); - continue; - } - - loc.frame = wp.frame; - loc.id = (ushort) (wp.command); - loc.p1 = (wp.param1); - loc.p2 = (wp.param2); - loc.p3 = (wp.param3); - loc.p4 = (wp.param4); - - loc.alt = ((wp.z)); - loc.lat = ((wp.x / 1.0e7)); - loc.lng = ((wp.y / 1.0e7)); - - if (!Locationwp.isLocationCommand(loc.id)) - { - loc.lat = wp.x; - loc.lng = wp.y; - } - - log.InfoFormat("getWPint {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, - loc.lng, - loc.frame); - - break; - } - else - { - //log.Info(DateTime.Now + " PC getwp " + buffer.msgid); - } - } - } - - giveComport = false; - return loc; - } - - public object DebugPacket(MAVLinkMessage datin) - { - string text = ""; - return DebugPacket(datin, ref text, true); - } - - public object DebugPacket(MAVLinkMessage datin, bool PrintToConsole) - { - string text = ""; - return DebugPacket(datin, ref text, PrintToConsole); - } - - public object DebugPacket(MAVLinkMessage datin, ref string text) - { - return DebugPacket(datin, ref text, true); - } - - /// - /// Print entire decoded packet to console - /// - /// packet byte array - /// struct of data - public object DebugPacket(MAVLinkMessage datin, ref string text, bool PrintToConsole, string delimeter = " ") - { - string textoutput = ""; - try - { - if (datin.Length > 5) - { - textoutput = - string.Format( - "{0,2:X}{8}{1,2:X}{8}{2,2:X}{8}{3,2:X}{8}{4,2:X}{8}{5,2:X}{8}{6,2:X}{8}{7,6:X}{8}", - datin.header, - datin.payloadlength, datin.incompat_flags, datin.compat_flags, datin.seq, datin.sysid, - datin.compid, datin.msgid, delimeter); - - object data = datin.data; - - Type test = data.GetType(); - - { - textoutput = textoutput + test.Name + delimeter; - - foreach (var field in test.GetFields()) - { - // field.Name has the field's name. - - object fieldValue = field.GetValue(data); // Get value - - if (field.FieldType.IsArray) - { - textoutput = textoutput + field.Name + delimeter; - if (fieldValue.GetType() == typeof(byte[])) - { - try - { - byte[] crap = (byte[]) fieldValue; - - foreach (byte fiel in crap) - { - if (fiel == 0) - { - break; - } - else - { - textoutput = textoutput + (char) fiel; - } - } - } - catch - { - } - } - - if (fieldValue.GetType() == typeof(char[])) - { - try - { - char[] crap = (char[]) fieldValue; - - foreach (char fiel in crap) - { - if (fiel == 0) - { - break; - } - else - { - textoutput = textoutput + fiel; - } - } - } - catch - { - } - } - - if (fieldValue.GetType() == typeof(short[])) - { - try - { - short[] crap = (short[]) fieldValue; - - foreach (short fiel in crap) - { - if (fiel == 0) - { - break; - } - else - { - textoutput = textoutput + Convert.ToString(fiel, 16) + "|"; - } - } - } - catch - { - } - } - - textoutput = textoutput + delimeter; - } - else - { - textoutput = textoutput + field.Name + delimeter + fieldValue.ToString() + delimeter; - } - } - - var sig = ""; - if (datin.sig != null) - sig = Convert.ToBase64String(datin.sig); - - textoutput = textoutput + delimeter + "sig " + sig + delimeter + "Len" + delimeter + - datin.Length + delimeter + "crc16" + delimeter + datin.crc16 + "\r\n"; - if (PrintToConsole) - Console.Write(textoutput); - - if (text != null) - text = textoutput; - } - - return data; - } - } - catch - { - textoutput = textoutput + "\r\n"; - } - - return null; - } - - [Obsolete] - public void setWPPartialUpdate(ushort startwp, ushort endwp, - MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - setWPPartialUpdateAsync(MAV.sysid, MAV.compid, startwp, endwp, type).ConfigureAwait(false).GetAwaiter() - .GetResult(); - } - - /// - /// Set start and finish for partial wp upload. - /// - /// - /// - public async Task setWPPartialUpdateAsync(byte sysid, byte compid, ushort startwp, ushort endwp, - MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - mavlink_mission_write_partial_list_t req = new mavlink_mission_write_partial_list_t - { - target_system = sysid, - target_component = compid, - start_index = (short) startwp, - end_index = (short) endwp, - mission_type = (byte) type - }; - - generatePacket((byte) MAVLINK_MSG_ID.MISSION_WRITE_PARTIAL_LIST, req); - } - - [Obsolete] - public void setWPTotal(ushort wp_total, MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - setWPTotalAsync(MAV.sysid, MAV.compid, wp_total, type).ConfigureAwait(false).GetAwaiter().GetResult(); - return; - } - - /// - /// Sets wp total count - /// - /// - public async Task setWPTotalAsync(byte sysid, byte compid, ushort wp_total, - MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) - { - giveComport = true; - mavlink_mission_count_t req = new mavlink_mission_count_t - { - target_system = sysid, - target_component = compid, - count = wp_total, - mission_type = (byte) type - }; - - log.Info("setWPTotal req MISSION_COUNT " + req.ToJSON(Formatting.None)); - generatePacket((byte) MAVLINK_MSG_ID.MISSION_COUNT, req); - - DateTime start = DateTime.Now; - int retrys = 3; - - while (true) - { - if (!(start.AddMilliseconds(700) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("setWPTotal Retry " + retrys); - generatePacket((byte) MAVLINK_MSG_ID.MISSION_COUNT, req); - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - throw new TimeoutException("Timeout on read - setWPTotal"); - } - - MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 9) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - var request = buffer.ToStructure(); - // check this gcs sent it - if (request.target_system != gcssysid || - request.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - if (request.seq == 0 || request.seq == 1) - { - if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) - { - if (MAV.param["WP_TOTAL"] != null) - MAV.param["WP_TOTAL"].Value = wp_total - 1; - if (MAV.param["CMD_TOTAL"] != null) - MAV.param["CMD_TOTAL"].Value = wp_total - 1; - if (MAV.param["MIS_TOTAL"] != null) - MAV.param["MIS_TOTAL"].Value = wp_total - 1; - MAVlist[req.target_system, req.target_component].wps.Clear(); - } - - if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) - MAVlist[req.target_system, req.target_component].fencepoints.Clear(); - if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) - MAVlist[req.target_system, req.target_component].rallypoints.Clear(); - - giveComport = false; - return; - } - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT && - buffer.sysid == req.target_system && buffer.compid == req.target_component) - { - var request = buffer.ToStructure(); - // check this gcs sent it - if (request.target_system != gcssysid || - request.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - if (request.seq == 0 || request.seq == 1) - { - if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) - { - if (MAV.param["WP_TOTAL"] != null) - MAV.param["WP_TOTAL"].Value = wp_total - 1; - if (MAV.param["CMD_TOTAL"] != null) - MAV.param["CMD_TOTAL"].Value = wp_total - 1; - if (MAV.param["MIS_TOTAL"] != null) - MAV.param["MIS_TOTAL"].Value = wp_total - 1; - MAVlist[req.target_system, req.target_component].wps.Clear(); - } - - if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) - MAVlist[req.target_system, req.target_component].fencepoints.Clear(); - if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) - MAVlist[req.target_system, req.target_component].rallypoints.Clear(); - - giveComport = false; - return; - } - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ACK && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - var ans = buffer.ToStructure(); - log.Info("setWPTotal ACK 47 : " + buffer.msgid + " ans " + - Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString())); - // check this gcs sent it - if (ans.target_system != gcssysid || - ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - giveComport = false; - return; // (MAV_MISSION_RESULT)ans.type; - } - else - { - //Console.WriteLine(DateTime.Now + " PC getwp " + buffer.msgid); - } - } - } - } - - [Obsolete] - public void InjectGpsData(byte[] data, ushort length) - { - InjectGpsData(MAV.sysid, MAV.compid, data, length); - } - - private int inject_seq_no = 0; - - /// - /// used to inject data into the gps ie rtcm/sbp/ubx - /// - /// - public void InjectGpsData(byte sysid, byte compid, byte[] data, ushort length, bool rtcm_message = true) - { - if (data == null) throw new ArgumentNullException(nameof(data)); - // new message - if (rtcm_message) - { - mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t(); - var msglen = 180; - - if (length > msglen * 4) - { - log.Error("Message too large " + length); - return; - } - - // number of packets we need, including a termination packet if needed - var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1; - - if (nopackets >= 4) - nopackets = 4; - - // flags = isfrag(1)/frag(2)/seq(5) - - for (int a = 0; a < nopackets; a++) - { - // check if its a fragment - if (nopackets > 1) - gps.flags = 1; - else - gps.flags = 0; - - // add fragment number - gps.flags += (byte) ((a & 0x3) << 1); - - // add seq number - gps.flags += (byte) ((inject_seq_no & 0x1f) << 3); - - // create the empty buffer - gps.data = new byte[msglen]; - - // calc how much data we are copying - int copy = Math.Min(length - a * msglen, msglen); - - // copy the data - Array.Copy(data, a * msglen, gps.data, 0, copy); - - // set the length - gps.len = (byte) copy; - - generatePacket((byte) MAVLINK_MSG_ID.GPS_RTCM_DATA, gps, sysid, compid); - } - - inject_seq_no++; - } - else - { - mavlink_gps_inject_data_t gps = new mavlink_gps_inject_data_t(); - var msglen = 110; - - var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1; - - for (int a = 0; a < len; a++) - { - gps.data = new byte[msglen]; - - int copy = Math.Min(length - a * msglen, msglen); - - Array.Copy(data, a * msglen, gps.data, 0, copy); - gps.len = (byte) copy; - gps.target_component = compid; - gps.target_system = sysid; - - generatePacket((byte) MAVLINK_MSG_ID.GPS_INJECT_DATA, gps, sysid, compid); - } - } - } - - [Obsolete] - public MAV_MISSION_RESULT setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current = 0, - byte autocontinue = 1, bool use_int = false, MAV_MISSION_TYPE mission_type = MAV_MISSION_TYPE.MISSION) - { - return setWP(MAV.sysid, MAV.compid, loc, index, frame, current, autocontinue, use_int, mission_type); - } - - public MAV_MISSION_RESULT setWP(byte sysid, byte compid, Locationwp loc, ushort index, MAV_FRAME frame, - byte current = 0, - byte autocontinue = 1, bool use_int = false, MAV_MISSION_TYPE mission_type = MAV_MISSION_TYPE.MISSION) - { - return setWPAsync(sysid, compid, loc, index, frame, current, autocontinue, use_int, mission_type) - .AwaitSync(); - } - - /// - /// Save wp to eeprom - /// - /// location struct - /// wp no - /// global or relative - /// 0 = no , 2 = guided mode - public async Task setWPAsync(byte sysid, byte compid, Locationwp loc, ushort index, - MAV_FRAME frame, byte current = 0, - byte autocontinue = 1, bool use_int = false, MAV_MISSION_TYPE mission_type = MAV_MISSION_TYPE.MISSION) - { - if (use_int) - { - mavlink_mission_item_int_t req = new mavlink_mission_item_int_t - { - target_system = sysid, - target_component = compid, - command = loc.id, - current = current, - autocontinue = autocontinue, - mission_type = (byte) mission_type, - frame = (byte) frame - }; - - if (!Locationwp.isLocationCommand(loc.id)) - { - req.y = (int) (loc.lng); - req.x = (int) (loc.lat); - } - else - { - req.y = (int) (loc.lng * 1.0e7); - req.x = (int) (loc.lat * 1.0e7); - } - - req.z = (float) (loc.alt); - - req.param1 = loc.p1; - req.param2 = loc.p2; - req.param3 = loc.p3; - req.param4 = loc.p4; - - req.seq = index; - - return await setWPAsync(req).ConfigureAwait(false); - } - else - { - mavlink_mission_item_t req = new mavlink_mission_item_t - { - target_system = sysid, - target_component = compid, - command = loc.id, - current = current, - autocontinue = autocontinue, - mission_type = (byte) mission_type, - frame = (byte) frame, - y = (float) (loc.lng), - x = (float) (loc.lat), - z = (float) (loc.alt), - param1 = loc.p1, - param2 = loc.p2, - param3 = loc.p3, - param4 = loc.p4, - seq = index - }; - - return await setWPAsync(req).ConfigureAwait(false); - } - } - - [Obsolete] - public async Task setWPAsync(mavlink_mission_item_t req) - { - giveComport = true; - - ushort index = req.seq; - - log.InfoFormat("setWP {7}:{8} {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, - req.param1, - req.x, req.y, req.z, index, req.target_system, req.target_component); - - // request - generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM, req); - - DateTime start = DateTime.Now; - int retrys = 10; - - while (true) - { - if (!(start.AddMilliseconds(450) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("setWP Retry " + retrys); - generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM, req); - - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - throw new TimeoutException("Timeout on read - setWP"); - } - - MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ACK && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - var ans = buffer.ToStructure(); - log.Info("set wp " + index + " ACK 47 : " + buffer.msgid + " ans " + - Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString())); - // check this gcs sent it - if (ans.target_system != gcssysid || - ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - if (req.current == 2) - { - MAVlist[req.target_system, req.target_component].GuidedMode = (Locationwp) req; - } - else if (req.current == 3) - { - } - else - { - if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) - MAVlist[req.target_system, req.target_component].wps[req.seq] = (Locationwp) req; - if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) - MAVlist[req.target_system, req.target_component].fencepoints[req.seq] = - (Locationwp) req; - if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) - MAVlist[req.target_system, req.target_component].rallypoints[req.seq] = - (Locationwp) req; - } - - //if (ans.target_system == req.target_system && ans.target_component == req.target_component) - { - giveComport = false; - return (MAV_MISSION_RESULT) ans.type; - } - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST && - buffer.sysid == req.target_system && buffer.compid == req.target_component) - { - var ans = buffer.ToStructure(); - - if (ans.target_system != gcssysid || - ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - if (ans.seq == (index + 1)) - { - log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer.msgid); - giveComport = false; - - if (req.current == 2) - { - MAVlist[req.target_system, req.target_component].GuidedMode = (Locationwp) req; - } - else if (req.current == 3) - { - } - else - { - if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) - MAVlist[req.target_system, req.target_component].wps[req.seq] = (Locationwp) req; - if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) - MAVlist[req.target_system, req.target_component].fencepoints[req.seq] = - (Locationwp) req; - if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) - MAVlist[req.target_system, req.target_component].rallypoints[req.seq] = - (Locationwp) req; - } - - //if (ans.target_system == req.target_system && ans.target_component == req.target_component) - { - giveComport = false; - return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED; - } - } - else - { - log.InfoFormat( - "set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + - buffer.msgid + - " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component); - // resend point now - start = DateTime.MinValue; - } - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT && - buffer.sysid == req.target_system && buffer.compid == req.target_component) - { - var ans = buffer.ToStructure(); - - if (ans.target_system != gcssysid || - ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - if (ans.seq == (index + 1)) - { - log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer.msgid); - giveComport = false; - - if (req.current == 2) - { - MAVlist[req.target_system, req.target_component].GuidedMode = (Locationwp) req; - } - else if (req.current == 3) - { - } - else - { - MAVlist[req.target_system, req.target_component].wps[req.seq] = (Locationwp) req; - } - - //if (ans.target_system == req.target_system && ans.target_component == req.target_component) - { - giveComport = false; - return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED; - } - } - else - { - log.InfoFormat( - "set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + - buffer.msgid + - " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component); - // resend point now - start = DateTime.MinValue; - } - } - else - { - //Console.WriteLine(DateTime.Now + " PC setwp " + buffer.msgid); - } - } - } - - // return MAV_MISSION_RESULT.MAV_MISSION_INVALID; - } - - [Obsolete] - public async Task setWPAsync(mavlink_mission_item_int_t req) - { - giveComport = true; - - ushort index = req.seq; - - log.Info("setWPint req MISSION_ITEM_INT " + req.ToJSON(Formatting.None)); - //log.InfoFormat("setWPint {7}:{8} {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, - // req.x / 1.0e7, req.y /1.0e7 , req.z, index, req.target_system, req.target_component); - - // request - generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM_INT, req); - - DateTime start = DateTime.Now; - int retrys = 10; - - while (true) - { - if (!(start.AddMilliseconds(450) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("setWP Retry " + retrys); - generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM_INT, req); - - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - throw new TimeoutException("Timeout on read - setWP"); - } - - MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ACK && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - var ans = buffer.ToStructure(); - log.Info("setWPint resp MISSION_ACK " + buffer.ToJSON(Formatting.None)); - //log.Info("set wp " + index + " ACK 47 : " + buffer.msgid + " ans " + - // Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString())); - // check this gcs sent it - if (ans.target_system != gcssysid || - ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - if (req.current == 2) - { - MAVlist[req.target_system, req.target_component].GuidedMode = (Locationwp) req; - } - else if (req.current == 3) - { - } - else - { - if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) - MAVlist[req.target_system, req.target_component].wps[req.seq] = (Locationwp) req; - if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) - MAVlist[req.target_system, req.target_component].fencepoints[req.seq] = - (Locationwp) req; - if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) - MAVlist[req.target_system, req.target_component].rallypoints[req.seq] = - (Locationwp) req; - } - - //if (ans.target_system == req.target_system && ans.target_component == req.target_component) - { - giveComport = false; - return (MAV_MISSION_RESULT) ans.type; - } - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST && - buffer.sysid == req.target_system && buffer.compid == req.target_component) - { - var ans = buffer.ToStructure(); - if (ans.target_system != gcssysid || - ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - if (ans.seq == (index + 1)) - { - log.Info("setWPint resp MISSION_REQUEST" + buffer.ToJSON(Formatting.None)); - //log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer.msgid); - - if (req.current == 2) - { - MAVlist[req.target_system, req.target_component].GuidedMode = (Locationwp) req; - } - else if (req.current == 3) - { - } - else - { - if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) - MAVlist[req.target_system, req.target_component].wps[req.seq] = (Locationwp) req; - if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) - MAVlist[req.target_system, req.target_component].fencepoints[req.seq] = - (Locationwp) req; - if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) - MAVlist[req.target_system, req.target_component].rallypoints[req.seq] = - (Locationwp) req; - } - - //if (ans.target_system == req.target_system && ans.target_component == req.target_component) - { - giveComport = false; - return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED; - } - } - else - { - log.InfoFormat( - "set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + - buffer.msgid + - " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component); - // resend point now - start = DateTime.MinValue; - } - } - else - { - //Console.WriteLine(DateTime.Now + " PC setwp " + buffer.msgid); - } - } - } - - // return MAV_MISSION_RESULT.MAV_MISSION_INVALID; - } - - public int getRequestedWPNo(byte sysid, byte compid) - { - return getRequestedWPNoAsync(sysid, compid).AwaitSync(); - } - - public async Task getRequestedWPNoAsync(byte sysid, byte compid) - { - giveComport = true; - DateTime start = DateTime.Now; - - while (true) - { - if (!(start.AddMilliseconds(1500) > DateTime.Now)) - { - giveComport = false; - throw new TimeoutException("Timeout on read - getRequestedWPNo"); - } - - MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte)MAVLINK_MSG_ID.MISSION_REQUEST && buffer.sysid == sysid && - buffer.compid == compid) - { - var ans = buffer.ToStructure(); - - log.InfoFormat("getRequestedWPNo seq {0} ts {1} tc {2}", ans.seq, ans.target_system, - ans.target_component); - - giveComport = false; - - return ans.seq; - } - if (buffer.msgid == (byte)MAVLINK_MSG_ID.MISSION_REQUEST_INT && buffer.sysid == sysid && - buffer.compid == compid) - { - var ans = buffer.ToStructure(); - - log.InfoFormat("getRequestedWPNo INT seq {0} ts {1} tc {2}", ans.seq, ans.target_system, - ans.target_component); - - giveComport = false; - - return ans.seq; - } - } - } - } - - [Obsolete] - public void setGuidedModeWP(Locationwp gotohere, bool setguidedmode = true) - { - setGuidedModeWP(MAV.sysid, MAV.compid, gotohere, setguidedmode); - } - - public void setGuidedModeWP(byte sysid, byte compid, Locationwp gotohere, bool setguidedmode = true) - { - if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0) - return; - - try - { - gotohere.id = (ushort) MAV_CMD.WAYPOINT; - - if (setguidedmode) - { - // fix for followme change - if (MAVlist[sysid, compid].cs.mode.ToUpper() != "GUIDED") - setMode(sysid, compid, "GUIDED"); - } - - log.InfoFormat("setGuidedModeWP {0}:{1} lat {2} lng {3} alt {4}", sysid, compid, gotohere.lat, - gotohere.lng, gotohere.alt); - - if (MAVlist[sysid, compid].cs.firmware == Firmwares.ArduPlane) - { - MAV_MISSION_RESULT ans = setWP(sysid, compid, gotohere, 0, (MAV_FRAME)gotohere.frame, (byte) 2); - - if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED) - { - throw new Exception("Guided Mode Failed"); - } - } - else - { - setPositionTargetGlobalInt((byte) sysid, (byte) compid, - true, false, false, false, (MAV_FRAME)gotohere.frame, - gotohere.lat, gotohere.lng, gotohere.alt, 0, 0, 0, 0, 0); - } - } - catch (Exception ex) - { - log.Error(ex); - } - } - - [Obsolete] - public void setNewWPAlt(Locationwp gotohere) - { - setNewWPAlt((byte) sysidcurrent, (byte) compidcurrent, gotohere); - } - - public void setNewWPAlt(byte sysid, byte compid, Locationwp gotohere) - { - try - { - gotohere.id = (ushort) MAV_CMD.WAYPOINT; - - log.InfoFormat("setNewWPAlt {0}:{1} lat {2} lng {3} alt {4}", sysid, compid, gotohere.lat, gotohere.lng, - gotohere.alt); - - MAV_MISSION_RESULT ans = setWP(sysid, compid, gotohere, 0, MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte) 3); - - if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED) - { - throw new Exception("Alt Change Failed"); - } - - /* - // currently plane supports just an alt change, copter requires all lat/lng/alt - setPositionTargetGlobalInt((byte)sysid, (byte)compid, - true, false, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, - gotohere.lat, gotohere.lng, gotohere.alt, 0, 0, 0, 0, 0); - */ - } - catch (Exception ex) - { - log.Error(ex); - throw; - } - } - - public void setPositionTargetGlobalInt(byte sysid, byte compid, bool pos, bool vel, bool acc, bool yaw, - MAV_FRAME frame, double lat, double lng, double alt, double vx, double vy, double vz, double yawangle, - double yawrate) - { - // for mavlink SET_POSITION_TARGET messages - const ushort MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2)); - const ushort MAVLINK_SET_POS_TYPE_MASK_ALT_IGNORE = ((0 << 0) | (0 << 1) | (1 << 2)); - const ushort MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = ((1 << 3) | (1 << 4) | (1 << 5)); - const ushort MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = ((1 << 6) | (1 << 7) | (1 << 8)); - const ushort MAVLINK_SET_POS_TYPE_MASK_FORCE = ((1 << 9)); - const ushort MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE = ((1 << 10) | (1 << 11)); - - mavlink_set_position_target_global_int_t target = new mavlink_set_position_target_global_int_t() - { - target_system = sysid, - target_component = compid, - alt = (float) alt, - lat_int = (int) (lat * 1e7), - lon_int = (int) (lng * 1e7), - coordinate_frame = (byte) frame, - vx = (float) vx, - vy = (float) vy, - vz = (float) vz, - yaw = (float) yawangle, - yaw_rate = (float) yawrate - }; - - target.type_mask = ushort.MaxValue; - target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_FORCE; - - if (pos && lat != 0 && lng != 0) - target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; - if (pos && lat == 0 && lng == 0) - target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_ALT_IGNORE; - if (vel) - target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; - if (acc) - target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; - if (yaw) - target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; - - if (pos) - { - if (lat != 0) - MAVlist[sysid, compid].GuidedMode.x = (int) (lat * 1e7); - if (lng != 0) - MAVlist[sysid, compid].GuidedMode.y = (int) (lng * 1e7); - MAVlist[sysid, compid].GuidedMode.z = (float) alt; - } - - bool pos_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE) > 0; - bool vel_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE) > 0; - bool acc_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE) > 0; - - generatePacket((byte) MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT, target, sysid, compid); - } - - [Obsolete] - public void setDigicamControl(bool shot) - { - mavlink_digicam_control_t req = new mavlink_digicam_control_t(); - - req.target_system = MAV.sysid; - req.target_component = MAV.compid; - req.shot = (shot == true) ? (byte) 1 : (byte) 0; - - if (!doCommand((byte) sysidcurrent, (byte) compidcurrent, MAV_CMD.DO_DIGICAM_CONTROL, 0, 0, 0, 0, 1, 0, 0)) - { - generatePacket((byte) MAVLINK_MSG_ID.DIGICAM_CONTROL, req); - } - } - - [Obsolete] - public void setMountConfigure(MAV_MOUNT_MODE mountmode, bool stabroll, bool stabpitch, bool stabyaw) - { - byte stab_roll = (stabroll == true) ? (byte) 1 : (byte) 0; - byte stab_pitch = (stabpitch == true) ? (byte) 1 : (byte) 0; - byte stab_yaw = (stabyaw == true) ? (byte) 1 : (byte) 0; - - // p1 : mount mode - // p2, p3, p4 : stabilize roll, pitch, yaw - // p5, p6, p7 : empty - // no ack required - doCommand(MAV.sysid, MAV.compid, MAV_CMD.DO_MOUNT_CONFIGURE, (byte)mountmode, stab_roll, stab_pitch, stab_yaw, 0, 0, 0, false); - } - - [Obsolete] - public void setMountControl(double pa, double pb, double pc, bool islatlng) - { - setMountControl(MAV.sysid, MAV.compid, pa, pb, pc, islatlng); - } - - public void setMountControl(byte sysid, byte compid, double pa, double pb, double pc, bool islatlng) - { - if (!islatlng) - { - // pa is pitch, pb is roll, pc is yaw in centi-degrees. convert to degrees - doCommand(sysid, compid, MAV_CMD.DO_MOUNT_CONTROL, (float)(pa * 0.01), (float)(pb * 0.01), (float)(pc * 0.01), 0, 0, 0, (float)MAV_MOUNT_MODE.MAVLINK_TARGETING, false); - } - else - { - // pa is lat in degrees, placed in param5 in deg * 10e7 - // pb is lon in degrees, placed in param6 in deg * 10e7 - // pc is alt in meters, placed in param5 in meters - doCommand(sysid, compid, MAV_CMD.DO_MOUNT_CONTROL, 0, 0, 0, (float)pc, (float)(pa * 10000000.0f), (float)(pb * 10000000.0f), (float)MAV_MOUNT_MODE.MAVLINK_TARGETING, false); - } - } - - [Obsolete] - public void setMode(string modein) - { - setMode(MAV.sysid, MAV.compid, modein); - } - - public void setMode(byte sysid, byte compid, string modein) - { - mavlink_set_mode_t mode = new mavlink_set_mode_t(); - - if (translateMode(sysid, compid, modein, ref mode)) - { - log.Info("setMode " + modein + " (" + mode.custom_mode + ")"); - setMode(sysid, compid, mode); - } - } - - [Obsolete] - public void setMode(mavlink_set_mode_t mode, MAV_MODE_FLAG base_mode = 0) - { - setMode(MAV.sysid, MAV.compid, mode, base_mode); - } - - public void setMode(byte sysid, byte compid, mavlink_set_mode_t mode, MAV_MODE_FLAG base_mode = 0) - { - mode.base_mode |= (byte) base_mode; - - // new - doCommand(sysid, compid, MAV_CMD.DO_SET_MODE, mode.base_mode, mode.custom_mode, 0, 0, 0, 0, 0, false); - - // old - generatePacket((byte) (byte) MAVLINK_MSG_ID.SET_MODE, mode, sysid, compid); - Thread.Sleep(10); - generatePacket((byte) (byte) MAVLINK_MSG_ID.SET_MODE, mode, sysid, compid); - } - - private double t7 = 1.0e7; - - public bool debug = false; - - private static bool ByteArrayCompare(ReadOnlySpan a1, ReadOnlySpan a2) - { - return a1.SequenceEqual(a2); - } - - /// - /// used for a readlock on readpacket - /// - private SemaphoreSlim readlock = new SemaphoreSlim(1, 1); - - /// - /// Serial Reader to read mavlink packets. POLL method - /// - /// - public MAVLinkMessage readPacket() - { - return readPacketAsync().AwaitSync(); - } - - public async Task readPacketAsync() - { - byte[] buffer = new byte[MAVLINK_MAX_PACKET_LEN + 25]; - int count = 0; - int length = 0; - int readcount = 0; - MAVLinkMessage message = null; - bool skipheader = false; - - //These need to be function wide, because of the extension of ReadLock try/catch - byte sysid; - byte compid; - byte packetSeqNo; - uint msgid; - bool packetSeemValid = false; - - DateTime start = DateTime.Now; - - if (debug) - Console.WriteLine(DateTime.Now.Millisecond + " SR0 " + BaseStream?.BytesToRead); - - await readlock.WaitAsync().ConfigureAwait(false); - - try - { - if (debug) - Console.WriteLine(DateTime.Now.Millisecond + " SR1 " + BaseStream?.BytesToRead); - - readloop: - - while ((BaseStream != null && BaseStream.IsOpen) || logreadmode) - { - try - { - if (readcount > 300) - { - break; - } - - readcount++; - if (logreadmode) - { - message = readlogPacketMavlink(); - buffer = message.buffer; - if (buffer == null || buffer.Length == 0) - return MAVLinkMessage.Invalid; - } - else - { - if (BaseStream.ReadTimeout != 1200) - BaseStream.ReadTimeout = - 1200; // 1200 ms between chars - the gps detection requires this. - - // time updated for internal reference - MAV.cs.datetime = DateTime.Now; - - if (!skipheader) - { - DateTime to = DateTime.UtcNow.AddMilliseconds(BaseStream.ReadTimeout); - - if (debug) - Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream?.BytesToRead); - - while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0) - { - if (DateTime.UtcNow > to) - { - log.InfoFormat("MAVLINK: 1 wait time out btr {0} len {1}", - BaseStream?.BytesToRead, - length); - throw new TimeoutException("Timeout"); - } - - await Task.Delay(1).ConfigureAwait(false); - if (debug) - Console.WriteLine(DateTime.Now.Millisecond + " SR0b " + - BaseStream?.BytesToRead); - } - - if (debug) - Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream?.BytesToRead); - if (BaseStream.IsOpen) - { - BaseStream.Read(buffer, count, 1); - if (rawlogfile != null && rawlogfile.CanWrite) - rawlogfile.WriteByte(buffer[count]); - } - - if (debug) - Console.WriteLine(DateTime.Now.Millisecond + " SR1b " + BaseStream?.BytesToRead); - } - - skipheader = false; - } - } - catch (Exception e) - { - skipheader = false; - log.Info("MAVLink readpacket read error: " + e.ToString()); - break; - } - - // check if looks like a mavlink packet and check for exclusions and write to console - if (buffer[0] != MAVLINK_STX_MAVLINK1 && buffer[0] != MAVLINK_STX) - { - if (buffer[0] >= 0x20 && buffer[0] <= 127 || buffer[0] == '\n' || buffer[0] == '\r') - { - // Write printable characters outside of mavlink packets to console - Console.Write((char)buffer[0]); - - // check for line termination and log it - if (buffer[0] == '\r' || buffer[0] == '\n') - { - // check new line is valid - if (buildplaintxtline.Length > 3) - { - plaintxtline = buildplaintxtline; - plaintxtlinebuffer.Insert(0, plaintxtline); - while (plaintxtlinebuffer.Count >= 52) - plaintxtlinebuffer.RemoveAt(plaintxtlinebuffer.Count - 1); - } - - log.Info(plaintxtline); - // reset for next line - buildplaintxtline = ""; - } - else - { - buildplaintxtline += (char)buffer[0]; - } - } - - _bytesReceivedSubj.OnNext(1); - count = 0; - buffer[1] = 0; - continue; - } - - // reset count on valid packet - readcount = 0; - - if (debug) - Console.WriteLine(DateTime.Now.Millisecond + " SR2 " + BaseStream?.BytesToRead); - - // check for a header - if (buffer[0] == MAVLINK_STX_MAVLINK1 || buffer[0] == MAVLINK_STX) - { - var mavlinkv2 = buffer[0] == MAVLINK_STX ? true : false; - - int headerlength = mavlinkv2 ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN; - int headerlengthstx = headerlength + 1; - - // if we have the header, and no other chars, get the length and packet identifiers - if (count < headerlength && !logreadmode) - { - DateTime to = DateTime.UtcNow.AddMilliseconds(BaseStream.ReadTimeout); - - while (BaseStream.IsOpen && BaseStream.BytesToRead < headerlength - count) - { - if (DateTime.UtcNow > to) - { - log.InfoFormat("MAVLINK: 2 wait time out btr {0} len {1}", BaseStream.BytesToRead, - length); - throw new TimeoutException("Timeout"); - } - - await Task.Delay(1).ConfigureAwait(false); - } - - if (debug) - Console.WriteLine(DateTime.Now.Millisecond + " SR2a " + BaseStream?.BytesToRead); - //var start1 = DateTime.Now; - int read = BaseStream.Read(buffer, count + 1, headerlength - count); - //var end = DateTime.Now - start1; - //var lapse = end.TotalMilliseconds; - //Console.WriteLine("read: " + lapse); - if (debug) - Console.WriteLine(DateTime.Now.Millisecond + " SR2b " + BaseStream?.BytesToRead); - if (rawlogfile != null && rawlogfile.CanWrite) - rawlogfile.Write(buffer, count + 1, read); - count += read; - } - - // packet length - if (buffer[0] == MAVLINK_STX) - { - length = buffer[1] + headerlengthstx + - MAVLINK_NUM_CHECKSUM_BYTES; // data + header + checksum - magic - length - if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0) - { - length += MAVLINK_SIGNATURE_BLOCK_LEN; - } - } - else - { - length = buffer[1] + headerlengthstx + - MAVLINK_NUM_CHECKSUM_BYTES; // data + header + checksum - U - length - } - - if (debug) - Console.WriteLine(DateTime.Now.Millisecond + " SR3 " + BaseStream?.BytesToRead); - - if (count >= headerlength || logreadmode) - { - try - { - if (logreadmode) - { - } - else - { - DateTime to = DateTime.UtcNow.AddMilliseconds(BaseStream.ReadTimeout); - - while (BaseStream.IsOpen && BaseStream.BytesToRead < (length - (headerlengthstx))) - { - if (DateTime.UtcNow > to) - { - log.InfoFormat("MAVLINK: 3 wait time out btr {0} len {1}", - BaseStream.BytesToRead, length); - break; - } - - await Task.Delay(1).ConfigureAwait(false); - } - - if (BaseStream.IsOpen) - { - //var start1 = DateTime.UtcNow; - int read = BaseStream.Read(buffer, headerlengthstx, length - (headerlengthstx)); - //var end = DateTime.UtcNow - start1; - //var lapse = end.TotalMilliseconds; - //Console.WriteLine("read: " + lapse); - - if (read != (length - headerlengthstx)) - log.InfoFormat("MAVLINK: bad read {0}, {1}, {2}", headerlengthstx, length, - count); - if (rawlogfile != null && rawlogfile.CanWrite) - { - // write only what we read, temp is the whole packet, so 6-end - rawlogfile.Write(buffer, headerlengthstx, read); - } - } - } - - count = length; - } - catch - { - break; - } - - break; - } - } - - count++; - if (count == 299) - break; - } - - if (debug) - Console.WriteLine(DateTime.Now.Millisecond + " SR4 " + BaseStream?.BytesToRead); - //ReadLock ended here - - // resize the packet to the correct length - Array.Resize(ref buffer, count); - - // add byte count - _bytesReceivedSubj.OnNext(buffer.Length); - - // update bps statistics - if (_bpstime.Second != DateTime.UtcNow.Second) - { - long btr = 0; - if (BaseStream != null && BaseStream.IsOpen) - { - btr = BaseStream.BytesToRead; - } - else if (logreadmode) - { - btr = logplaybackfile.BaseStream.Length - logplaybackfile.BaseStream.Position; - } - - if (printbps) - Console.Write( - "bps {0} loss {1} left {2} mem {3} mav2 {4} sign {5} mav1 {6} mav2 {7} signed {8} \n", - _bps1, - MAV.synclost, btr, - GC.GetTotalMemory(false) / 1024 / 1024.0, MAV.mavlinkv2, MAV.signing, - _mavlink1count, - _mavlink2count, _mavlink2signed); - _bps2 = _bps1; // prev sec - _bps1 = 0; // current sec - _bpstime = DateTime.UtcNow; - _mavlink1count = 0; - _mavlink2count = 0; - _mavlink2signed = 0; - } - - _bps1 += buffer.Length; - - if (buffer.Length == 0) - return MAVLinkMessage.Invalid; - - if (message == null) - message = new MAVLinkMessage(buffer, DateTime.UtcNow); - - msgid = message.msgid; - - message_info msginfo = MAVLINK_MESSAGE_INFOS.GetMessageInfo(msgid); - - // calc crc - var sigsize = (message.sig != null) ? MAVLINK_SIGNATURE_BLOCK_LEN : 0; - ushort crc = MavlinkCRC.crc_calculate(buffer, - message.Length - sigsize - MAVLINK_NUM_CHECKSUM_BYTES); - - // calc extra bit of crc for mavlink 1.0/2.0 - if (message.header == MAVLINK_STX_MAVLINK1 || message.header == MAVLINK_STX) - { - crc = MavlinkCRC.crc_accumulate(msginfo.crc, crc); - } - - // check message length size vs table (mavlink1 explicit size check | mavlink2 allow all, undersize 0 trimmed, and oversize unknown extension) - if (!message.ismavlink2 && message.payloadlength != msginfo.minlength) - { - if (msginfo.length == 0) // pass for unknown packets - { - log.InfoFormat("unknown packet type {0}", message.msgid); - } - else - { - log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", buffer.Length, - message.msgid); - return MAVLinkMessage.Invalid; - } - } - - // check crc - if ((message.crc16 >> 8) != (crc >> 8) || - (message.crc16 & 0xff) != (crc & 0xff)) - { - if (buffer.Length > 5 && msginfo.name != null) - log.InfoFormat("Mavlink Bad Packet (crc fail) len {0} crc {1} vs {4} pkno {2} {3}", - buffer.Length, - crc, message.msgid, msginfo.name.ToString(), - message.crc16); - if (logreadmode) - log.InfoFormat("bad packet pos {0} ", logplaybackfile.BaseStream.Position); - // unknown packet - if (buffer.Length > 5 && String.IsNullOrEmpty(msginfo.name)) - ProcessMirrorStream(buffer); - - // look within the last 20 bytes - int offset = buffer.Search(new byte[] { MAVLINK_STX }, Math.Max(1, buffer.Length - 20)); - if (offset > 0) - { - var newbuffer = new byte[MAVLINK_MAX_PACKET_LEN + 25]; - Array.ConstrainedCopy(buffer, offset, newbuffer, 0, buffer.Length - offset); - count = buffer.Length - offset - 1; - log.InfoFormat($"rewind oldlen {buffer.Length} offset {offset} cnt {count}"); - buffer = newbuffer; - message = null; - skipheader = true; - goto readloop; - } - - return MAVLinkMessage.Invalid; - } - - sysid = message.sysid; - compid = message.compid; - packetSeqNo = message.seq; - - // create a state for any sysid/compid includes gcs on log playback - if (!MAVlist.Contains(sysid, compid)) - { - // create an item - hidden - MAVlist.AddHiddenList(sysid, compid); - // prevent packetloss counter on connect - MAVlist[sysid, compid].recvpacketcount = unchecked(packetSeqNo - (byte)1); - } - - // once set it cannot be reverted - if (!MAVlist[sysid, compid].mavlinkv2) - MAVlist[sysid, compid].mavlinkv2 = message.buffer[0] == MAVLINK_STX ? true : false; - - // stat count - if (message.buffer[0] == MAVLINK_STX) - _mavlink2count++; - else if (message.buffer[0] == MAVLINK_STX_MAVLINK1) - _mavlink1count++; - - //check if sig was included in packet, and we are not ignoring the signature (signing isnt checked else we wont enable signing) - //logreadmode we always ignore signing as they would not be in the log if they failed the signature - if (message.sig != null && !MAVlist[sysid, compid].signingignore && !logreadmode) - { - _mavlink2signed++; - - bool valid = false; - - // check existing key - if (MAVlist[sysid, compid].signingKey != null) - valid = CheckSignature(MAVlist[sysid, compid].signingKey, message, sysid, compid); - - if (!valid) - { - foreach (var AuthKey in MAVAuthKeys.Keys.Values) - { - valid = CheckSignature(AuthKey.Key, message, sysid, compid); - if (valid) - break; - } - - if (!valid) - { - log.InfoFormat("Packet failed signature but passed crc"); - return MAVLinkMessage.Invalid; - } - } - - var timestamp = new DateTime(2015, 1, 1).AddMilliseconds(message.sigTimestamp / 100.0); - var delta = DateTime.UtcNow - timestamp; - } - - // packet is now verified - - // log before any processing that could send response packets (e.g. timesync) - try - { - SaveToTlog(new Span(buffer)); - - if (logfile != null) - { - lock (logfile) - { - if (msgid == 0) - { - if (logfile != null && logfile.CanWrite) - logfile.Flush(); - if (rawlogfile != null && rawlogfile.CanWrite) - rawlogfile.Flush(); - } - } - } - } - catch (Exception ex) - { - log.Error(ex); - } - - // extract wp's/rally/fence/camera feedback/params from stream, including gcs packets on playback. - // may also send responses (e.g. timesync reply) - if (buffer.Length >= 5) - { - processInfoFromStream(ref message, sysid, compid); - } - - // if its a gcs packet - dont process further - if (buffer.Length >= 5 && (sysid == gcssysid || sysid == 253) && logreadmode) // gcs packet - { - return message; - } - - // update packet loss statistics - if (!logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < DateTime.UtcNow) - { - MAVlist[sysid, compid].packetlosttimer = DateTime.UtcNow; - MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost * 0.8f); - MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost * 0.8f); - } - else if (logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < lastlogread) - { - MAVlist[sysid, compid].packetlosttimer = lastlogread.ToUniversalTime(); - MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost * 0.8f); - MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost * 0.8f); - } - - packetSeemValid = false; - //The following IF had to be splitted to accomodate the new place of Readlock end. - //packetSeemValid helps it. - if ((message.header == MAVLINK_STX_MAVLINK1 || - message.header == MAVLINK_STX) && - buffer.Length >= message.payloadlength) - { - packetSeemValid = true; - // check if we lost pacakets based on seqno - int expectedPacketSeqNo = ((MAVlist[sysid, compid].recvpacketcount + 1) % 0x100); - - { - // the second part is to work around a 3dr radio bug sending dup seqno's - if (packetSeqNo != expectedPacketSeqNo && - packetSeqNo != MAVlist[sysid, compid].recvpacketcount) - { - MAVlist[sysid, compid].synclost++; // actual sync loss's - int numLost = 0; - - if (packetSeqNo < ((MAVlist[sysid, compid].recvpacketcount + 1))) - // recvpacketcount = 255 then 10 < 256 = true if was % 0x100 this would fail - { - numLost = 0x100 - expectedPacketSeqNo + packetSeqNo; - } - else - { - numLost = packetSeqNo - expectedPacketSeqNo; - } - - MAVlist[sysid, compid].packetslost += numLost; - WhenPacketLost.OnNext(numLost); - - if (!logreadmode) - log.InfoFormat("mav {2}-{4} seqno {0} exp {3} pkts lost {1}", packetSeqNo, - numLost, - sysid, - expectedPacketSeqNo, compid); - } - - MAVlist[sysid, compid].packetsnotlost++; - - //Console.WriteLine("{0} {1}", sysid, packetSeqNo); - - MAVlist[sysid, compid].recvpacketcount = packetSeqNo; - } - WhenPacketReceived.OnNext(1); - - // packet stats per mav - if (!MAVlist[sysid, compid].packetspersecond.ContainsKey(msgid) || - double.IsInfinity(MAVlist[sysid, compid].packetspersecond[msgid])) - MAVlist[sysid, compid].packetspersecond[msgid] = 0; - if (!MAVlist[sysid, compid].packetspersecondbuild.ContainsKey(msgid)) - MAVlist[sysid, compid].packetspersecondbuild[msgid] = DateTime.UtcNow; - - MAVlist[sysid, compid].packetspersecond[msgid] = (((1000 / - ((DateTime.UtcNow - - MAVlist[sysid, compid] - .packetspersecondbuild[msgid]) - .TotalMilliseconds) + - MAVlist[sysid, compid].packetspersecond[ - msgid]) / 2)); - - MAVlist[sysid, compid].packetspersecondbuild[msgid] = DateTime.UtcNow; - } - //Console.WriteLine("Packet {0}",temp[5]); - // store packet history - - - //ReadLock Release moved here to include packet seqence checking. - //Releasing the lock earlier casued packet sequence errors, when readPacketAsync() was called repeatedly. - //Especially during retries. - - } // end readlock - finally - { - readlock.Release(); - } - - try - { - //The packet header checking IF had to be splitted to allow moving the Readlock try/catch. - if (packetSeemValid) - { - MAVlist[sysid, compid].addPacket(message); - - // adsb packets are forwarded and can be from any sysid/compid - if (msgid == (byte)MAVLINK_MSG_ID.ADSB_VEHICLE) - { - var adsb = message.ToStructure(); - - var id = adsb.ICAO_address.ToString("X5"); - - if (_UpdateADSBPlanePosition != null) - _UpdateADSBPlanePosition(this, new adsb.PointLatLngAltHdg( - adsb.lat / 1e7, - adsb.lon / 1e7, - adsb.altitude / 1000, - adsb.heading * 0.01f, - adsb.hor_velocity, - id, - DateTime.Now - ) - { - CallSign = Encoding.UTF8.GetString(adsb.callsign), - Squawk = adsb.squawk, - Raw = adsb, - VerticalSpeed = adsb.ver_velocity, - } - ); - } - - if (msgid == (byte)MAVLINK_MSG_ID.COLLISION) - { - var coll = message.ToStructure(); - - var id = coll.id.ToString("X5"); - - var coll_type = (MAV_COLLISION_SRC)coll.src; - - var action = (MAV_COLLISION_ACTION)coll.action; - - if (action > MAV_COLLISION_ACTION.REPORT) - { - // we are reacting to a threat - } - - var threat_level = (MAV_COLLISION_THREAT_LEVEL)coll.threat_level; - - _UpdateADSBCollision?.Invoke(this, (id, threat_level)); - } - - // set seens sysid's based on hb packet - this will hide 3dr radio packets - if (msgid == (uint)MAVLINK_MSG_ID.UAVCAN_NODE_STATUS) - { - var cannode = message.ToStructure(); - - // add a seen sysid - if (!MAVlist.Contains(sysid, compid, false)) - { - // ensure its set from connect or log playback - MAVlist.Create(sysid, compid); - MAVlist[sysid, compid].aptype = MAV_TYPE.ONBOARD_CONTROLLER; - MAVlist[sysid, compid].apname = MAV_AUTOPILOT.INVALID; - MAVlist[sysid, compid].CANNode = true; - setAPType(sysid, compid); - - // new device, so request node info - doCommand(sysid, compid, MAV_CMD.UAVCAN_GET_NODE_INFO, 0, 0, 0, 0, 0, 0, 0, false); - } - } - - if (msgid == (uint)MAVLINK_MSG_ID.UAVCAN_NODE_INFO) - { - var cannode = message.ToStructure(); - - var name = Encoding.UTF8.GetString(cannode.name); - - MAVlist[sysid, compid].VersionString = name; - - MAVlist[sysid, compid].SoftwareVersions = - cannode.sw_version_major + "." + cannode.sw_version_minor; - } - - // set seens sysid's based on hb packet - this will hide 3dr radio packets ( which send a RADIO_STATUS, but not a HEARTBEAT ) - if (msgid == (byte)MAVLINK_MSG_ID.HEARTBEAT) - { - mavlink_heartbeat_t hb = message.ToStructure(); - - // not a gcs - //if (hb.type != (byte)MAV_TYPE.GCS) - { - // add a seen sysid - if (!MAVlist.Contains(sysid, compid, false)) - { - // ensure its set from connect or log playback - MAVlist.Create(sysid, compid); - MAVlist[sysid, compid].aptype = (MAV_TYPE)hb.type; - MAVlist[sysid, compid].apname = (MAV_AUTOPILOT)hb.autopilot; - setAPType(sysid, compid); - _MAVDetected?.Invoke(this, (sysid, compid)); - } - - // attach to the only remote device. / default to first device seen - if (MAVlist.Count == 1) - { - // set it private as compidset will trigger new mavstate - _sysidcurrent = sysid; - compidcurrent = compid; - } - } - } - - //treat a high latency packet as a HB - if (msgid == (byte)MAVLINK_MSG_ID.HIGH_LATENCY2) - { - mavlink_high_latency2_t hl = message.ToStructure(); - - // not a gcs - //if (hb.type != (byte)MAV_TYPE.GCS) - { - // add a seen sysid - if (!MAVlist.Contains(sysid, compid, false)) - { - // ensure its set from connect or log playback - MAVlist.Create(sysid, compid); - MAVlist[sysid, compid].aptype = (MAV_TYPE)hl.type; - MAVlist[sysid, compid].apname = (MAV_AUTOPILOT)hl.autopilot; - MAVlist[sysid, compid].isHighLatency = true; - setAPType(sysid, compid); - _MAVDetected?.Invoke(this, (sysid, compid)); - } - - // attach to the only remote device. / default to first device seen - if (MAVlist.Count == 1) - { - // set it private as compidset will trigger new mavstate - _sysidcurrent = sysid; - compidcurrent = compid; - } - } - } - - // process for all mavs, filtering done inside - subscription handler - PacketReceived(message); - - _OnPacketReceived?.Invoke(this, message); - - if (debugmavlink) - DebugPacket(message); - - if (msgid == (byte)MAVLINK_MSG_ID.STATUSTEXT) // status text - { - var msg = message.ToStructure(); - - byte sev = msg.severity; - - string logdata = Encoding.UTF8.GetString(msg.text); - int ind = logdata.IndexOf('\0'); - if (ind != -1) - logdata = logdata.Substring(0, ind); - log.Info(DateTime.Now + " " + sev + " " + logdata); - - MAVlist[sysid, compid].cs.messages.Add((DateTime.Now, logdata)); - - // cap list at 1000 element - while (MAVlist[sysid, compid].cs.messages.Count > 1000) - MAVlist[sysid, compid].cs.messages.RemoveAt(0); - - // gymbals etc are a child/slave to the main sysid, this displays the children messages under the current displayed vehicle - if (sysid == sysidcurrent && compid != compidcurrent) - MAVlist[sysidcurrent, compidcurrent].cs.messages - .Add((DateTime.Now, compid + " : " + logdata)); - - bool printit = false; - - MAV_SEVERITY mavsev = MAV_SEVERITY.EMERGENCY; - if (sev <= (byte)Settings.Instance.GetInt32("severity", 4)) - { - printit = true; - mavsev = (MAV_SEVERITY)sev; - } - - if (logdata.StartsWith("Tuning:") || logdata.StartsWith("PreArm:") || - logdata.StartsWith("Arm:")) - printit = true; - - if (printit) - { - // high severity msg publish to current selected mav - if (sysid == sysidcurrent && compid != compidcurrent) - { - MAVlist[sysid, compidcurrent].cs.messageHigh = compid + " : " + logdata; - MAVlist[sysid, compidcurrent].cs.messageHighSeverity = mavsev; - } - - MAVlist[sysid, compid].cs.messageHigh = logdata; - MAVlist[sysid, compid].cs.messageHighSeverity = mavsev; - - if (Speech != null && - Speech.IsReady && - Settings.Instance["speechenable"] != null && - Settings.Instance["speechenable"].ToString() == "True") - { - if (speechenabled && !logdata.StartsWith("PreArm:")) // Suppress audible PreArm messages - Speech.SpeakAsync(logdata); - } - } - } - - if (Settings.Instance["autoParamCommit"] == null || - Settings.Instance.GetBoolean("autoParamCommit") == true) - { - if (lastparamset != DateTime.MinValue && lastparamset.AddSeconds(10) < DateTime.UtcNow) - { - lastparamset = DateTime.MinValue; - - if (BaseStream.IsOpen) - { - doCommand((byte)sysidcurrent, (byte)compidcurrent, MAV_CMD.PREFLIGHT_STORAGE, 1, 0, 0, - 0, 0, 0, 0, - false); - } - } - } - - ProcessMirrorStream(buffer); - } - } - catch (Exception ex) - { - log.Error(ex); - } - - // update last valid packet receive time - MAVlist[sysid, compid].lastvalidpacket = DateTime.UtcNow; - - return message; - } - - private void ProcessMirrorStream(byte[] buffer) - { - foreach (var Mirror in Mirrors) - { - var MirrorStream = Mirror.MirrorStream; - try - { - // full rw from mirror stream - if (MirrorStream != null && MirrorStream.IsOpen) - { - MirrorStream.Write(buffer, 0, buffer.Length); - - while (MirrorStream.BytesToRead > 0) - { - var len = MirrorStream.BytesToRead; - - byte[] buf = new byte[len]; - - len = MirrorStream.Read(buf, 0, len); - - if (Mirror.MirrorStreamWrite) - lock (writelock) - { - BaseStream.Write(buf, 0, len); - - if (rawlogfile != null && rawlogfile.CanWrite) - rawlogfile.Write(buf, 0, len); - } - } - } - - } - catch - { - } - } - } - - private bool CheckSignature(byte[] AuthKey, MAVLinkMessage message, byte sysid, byte compid) - { - bool valid; - using (SHA256CryptoServiceProvider signit = new SHA256CryptoServiceProvider()) - { - signit.TransformBlock(AuthKey, 0, AuthKey.Length, null, 0); - signit.TransformFinalBlock(message.buffer, 0, message.Length - MAVLINK_SIGNATURE_BLOCK_LEN + 7); - var ctx = signit.Hash; - - //Console.WriteLine("rec linkid {0}, time {1} {2} {3} {4} {5} {6} {7}", message.sig[0], message.sig[1], message.sig[2], message.sig[3], message.sig[4], message.sig[5], message.sig[6], message.sigTimestamp); - - valid = ByteArrayCompare(new ReadOnlySpan(ctx, 0, 6), new ReadOnlySpan(message.sig, 7, 6)); - - if (!valid) - return valid; - - // got valid key - MAVlist[sysid, compid].linkid = message.sig[0]; - - MAVlist[sysid, compid].signingKey = AuthKey; - - enableSigning(sysid, compid); - - return valid; - } - } - - private void PacketReceived(MAVLinkMessage buffer) - { - MAVLINK_MSG_ID type = (MAVLINK_MSG_ID) buffer.msgid; - (MAVLINK_MSG_ID msgId, Func function, bool exclusive, byte sysid, byte compid)[] list; - - // lock to grab the list, because this is public, to prevent a recursion - lock (Subscriptions) - { - list = Subscriptions.ToArray(); - } - - foreach (var item in list) - { - //exact match, and 0,0 = current primary - if (item.msgId == type && buffer.sysid == item.sysid && buffer.compid == item.compid || - item.msgId == type && item.sysid == 0 && item.compid == 0 && buffer.sysid == sysidcurrent && buffer.compid == compidcurrent) - { - try - { - item.function(buffer); - } - catch (Exception ex) - { - log.Error(ex); - } - } - } - } - - readonly private List<(MAVLINK_MSG_ID msgId, Func function, bool exclusive, byte sysid, byte compid)> Subscriptions = - new List<(MAVLINK_MSG_ID, Func, bool, byte, byte)>(); - - /// - /// Subscribe to a packet on the current target MAV. use OnPacketReceived to get all MAVs - /// - /// - /// - /// - /// - public int SubscribeToPacketType(MAVLINK_MSG_ID msgid, - Func function, byte sysid, byte compid, bool exclusive = false) - { - log.Info($"SubscribeToPacketType {msgid} {function} {exclusive} {sysid} {compid}"); - - var item = (msgid, function, exclusive, sysid, compid); - - lock (Subscriptions) - { - if (exclusive) - { - foreach (var subitem in Subscriptions) - { - if (subitem.msgId == item.msgid) - { - Subscriptions.Remove(subitem); - break; - } - } - } - - log.Debug("SubscribeToPacketType " + item.msgid + " " + item.function); - - Subscriptions.Add(item); - } - - return item.GetHashCode(); - } - - public void UnSubscribeToPacketType(int id) - { - lock (Subscriptions) - { - log.Debug("UnSubscribeToPacketType " + id); - var sub = Subscriptions.ToArray(); - - foreach (var valueTuple in sub) - { - if (id == valueTuple.GetHashCode()) - { - Subscriptions.Remove(valueTuple); - break; - } - } - } - } - - - - /// - /// Processes a MAVLink packet received from the stream and updates cached state for the - /// addressed vehicle/component. This includes tracking mission/fence/rally and parameter - /// data from sent or received traffic, and may also trigger protocol side effects such as - /// sending responses for messages that require them (for example, TIMESYNC handling). - /// - /// The MAVLink packet to process. Its contents may be used to update local mission/parameter state and may cause protocol responses to be sent. - /// The system id associated with the packet when resolving vehicle state updates. - /// The component id associated with the packet when resolving vehicle state updates. - private void processInfoFromStream(ref MAVLinkMessage buffer, byte sysid, byte compid) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_COUNT) - { - // clear old - mavlink_mission_count_t wp = buffer.ToStructure(); - - if (wp.target_system == gcssysid) - { - wp.target_system = sysid; - wp.target_component = compid; - } - - if (wp.mission_type == (byte) MAV_MISSION_TYPE.MISSION) - MAVlist[wp.target_system, wp.target_component].wps.Clear(); - if (wp.mission_type == (byte) MAV_MISSION_TYPE.FENCE) - MAVlist[wp.target_system, wp.target_component].fencepoints.Clear(); - if (wp.mission_type == (byte) MAV_MISSION_TYPE.RALLY) - MAVlist[wp.target_system, wp.target_component].rallypoints.Clear(); - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM) - { - mavlink_mission_item_t wp = buffer.ToStructure(); - - if (wp.target_system == gcssysid) - { - wp.target_system = sysid; - wp.target_component = compid; - } - - if (wp.current == 2) - { - // guide mode wp - if (wp.mission_type == (byte) MAV_MISSION_TYPE.MISSION) - MAVlist[wp.target_system, wp.target_component].GuidedMode = (Locationwp) wp; - } - else - { - if (wp.mission_type == (byte) MAV_MISSION_TYPE.MISSION) - MAVlist[wp.target_system, wp.target_component].wps[wp.seq] = (Locationwp) wp; - if (wp.mission_type == (byte) MAV_MISSION_TYPE.FENCE) - MAVlist[wp.target_system, wp.target_component].fencepoints[wp.seq] = (Locationwp) wp; - if (wp.mission_type == (byte) MAV_MISSION_TYPE.RALLY) - MAVlist[wp.target_system, wp.target_component].rallypoints[wp.seq] = (Locationwp) wp; - } - - //Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command); - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM_INT) - { - mavlink_mission_item_int_t wp = buffer.ToStructure(); - - if (wp.target_system == gcssysid) - { - wp.target_system = sysid; - wp.target_component = compid; - } - - if (wp.current == 2) - { - // guide mode wp - if (wp.mission_type == (byte) MAV_MISSION_TYPE.MISSION) - MAVlist[wp.target_system, wp.target_component].GuidedMode = (Locationwp) wp; - } - else - { - if (wp.mission_type == (byte) MAV_MISSION_TYPE.MISSION) - MAVlist[wp.target_system, wp.target_component].wps[wp.seq] = wp; - if (wp.mission_type == (byte) MAV_MISSION_TYPE.FENCE) - MAVlist[wp.target_system, wp.target_component].fencepoints[wp.seq] = wp; - if (wp.mission_type == (byte) MAV_MISSION_TYPE.RALLY) - MAVlist[wp.target_system, wp.target_component].rallypoints[wp.seq] = wp; - } - - //Console.WriteLine("WP INT # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command); - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.HOME_POSITION) - { - var home = buffer.ToStructure(); - - var loc = new PointLatLngAlt(home.latitude / 1.0e7, home.longitude / 1.0e7, home.altitude / 1000.0); - - MAVlist[buffer.sysid, buffer.compid].cs.HomeLocation = loc; - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT) - { - mavlink_set_position_target_global_int_t setpos = - buffer.ToStructure(); - - MAVlist[setpos.target_system, setpos.target_component].GuidedMode = (Locationwp) setpos; - - //Console.WriteLine("SET_POSITION_TARGET_GLOBAL_INT x {0} y {1} z {2} frame {3}", setpos.lat_int/1e7, setpos.lon_int/1e7, setpos.alt, setpos.coordinate_frame); - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.RALLY_POINT) - { - mavlink_rally_point_t rallypt = buffer.ToStructure(); - - if (rallypt.target_system == gcssysid) - { - rallypt.target_system = sysid; - rallypt.target_component = compid; - } - - MAVlist[rallypt.target_system, rallypt.target_component].rallypoints[rallypt.idx] = - new mavlink_mission_item_int_t(0, 0, 0, 0, rallypt.lat, rallypt.lng, rallypt.alt, rallypt.idx, - (ushort) MAV_CMD.RALLY_POINT, rallypt.target_system, rallypt.target_component, - (byte) MAV_FRAME.GLOBAL_RELATIVE_ALT, 0, 0, (byte) MAV_MISSION_TYPE.RALLY); - - //Console.WriteLine("RP # {0} {1} {2} {3} {4}", rallypt.idx, rallypt.lat, rallypt.lng, rallypt.alt, rallypt.break_alt); - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.CAMERA_FEEDBACK) - { - mavlink_camera_feedback_t camerapt = buffer.ToStructure(); - - if (MAVlist[sysid, compid].camerapoints.Count == 0 || - MAVlist[sysid, compid].camerapoints.Last().time_usec != camerapt.time_usec) - { - MAVlist[sysid, compid].camerapoints.RemoveAll(a => - a.cam_idx * 256 + a.img_idx == camerapt.cam_idx * 256 + camerapt.img_idx); - MAVlist[sysid, compid].camerapoints.Add(camerapt); - } - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.FENCE_POINT) - { - mavlink_fence_point_t fencept = buffer.ToStructure(); - - if (fencept.target_system == gcssysid) - { - fencept.target_system = sysid; - fencept.target_component = compid; - } - - MAVlist[fencept.target_system, fencept.target_component].fencepoints[fencept.idx] = - new mavlink_mission_item_int_t(fencept.count - 1, 0, 0, 0, (int) (fencept.lat * 1e7), - (int) (fencept.lng * 1e7), 0, fencept.idx, - fencept.idx == 0 - ? (ushort) MAV_CMD.FENCE_RETURN_POINT - : (ushort) MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION, - fencept.target_system, fencept.target_component, (byte) MAV_FRAME.GLOBAL_RELATIVE_ALT, 0, 0, - (byte) MAV_MISSION_TYPE.FENCE); - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE) - { - mavlink_param_value_t value = buffer.ToStructure(); - - string st = Encoding.UTF8.GetString(value.param_id); - - int pos = st.IndexOf('\0'); - - if (pos != -1) - { - st = st.Substring(0, pos); - } - - lock(MAVlist[sysid, compid].param_types) - MAVlist[sysid, compid].param_types[st] = (MAV_PARAM_TYPE) value.param_type; - - if (MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA && - buffer.compid != (byte) MAV_COMPONENT.MAV_COMP_ID_UDP_BRIDGE) - { - var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); - MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, BitConverter.GetBytes(value.param_value), - MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE) value.param_type); - } - else - { - var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); - MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, BitConverter.GetBytes(value.param_value), - (MAV_PARAM_TYPE) value.param_type, (MAV_PARAM_TYPE) value.param_type); - } - - if (value.param_count != ushort.MaxValue) - MAVlist[sysid, compid].param.TotalReported = value.param_count; - } - else if (buffer.msgid == (byte) MAVLINK_MSG_ID.TIMESYNC) - { - Int64 now_ns = - (Int64) ((DateTime.Now - new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc)).TotalMilliseconds * - 1000000); - - mavlink_timesync_t tsync = buffer.ToStructure(); - // tc1 - client - // ts1 - server - - // system does not know the time - if (tsync.tc1 == 0) - { - tsync.tc1 = now_ns; - if (BaseStream != null && BaseStream.IsOpen) - sendPacket(tsync, buffer.sysid, buffer.compid); - } // system knows the time - else if (tsync.tc1 > 0) - { - Int64 offset_ns = (tsync.ts1 + now_ns - tsync.tc1 * 2) / 2; - Int64 dt = MAVlist[buffer.sysid, buffer.compid].time_offset_ns - offset_ns; - - if (Math.Abs(dt) > 10000000) // 10 millisecond skew - { - MAVlist[buffer.sysid, buffer.compid].time_offset_ns = offset_ns; // hard-set it. - } - else - { - var offset_avg_alpha = 0.6; - var avg = (offset_avg_alpha * offset_ns) + - (1.0 - offset_avg_alpha) * MAVlist[buffer.sysid, buffer.compid].time_offset_ns; - MAVlist[buffer.sysid, buffer.compid].time_offset_ns = (long) avg; - } - } - } - } - - [Obsolete] - public bool getVersion(bool responcerequired = true) - { - return getVersion(MAV.sysid, MAV.compid, responcerequired); - } - - public bool getVersion(byte sysid, byte compid, bool responcerequired = true) - { - return getVersionAsync(sysid, compid, responcerequired).AwaitSync(); - } - - public async Task getVersionAsync(byte sysid, byte compid, bool responcerequired = true) - { - mavlink_autopilot_version_request_t req = new mavlink_autopilot_version_request_t(); - - req.target_component = compid; - req.target_system = sysid; - - // use *all three* methods - doCommand(MAVLink.MAV_CMD.REQUEST_MESSAGE, - (float)MAVLink.MAVLINK_MSG_ID.AUTOPILOT_VERSION, - 0, 0, 0, 0, 0, 0, false); - - // MAV_CMD.REQUEST_AUTOPILOT_CAPABILITIES is deprecated - doCommand(MAV_CMD.REQUEST_AUTOPILOT_CAPABILITIES, 0, 0, 0, 0, 0, 0, 0, false); - - // AUTOPILOT_VERSION_REQUEST is deprecated - generatePacket((byte) MAVLINK_MSG_ID.AUTOPILOT_VERSION_REQUEST, req); - - if (!responcerequired) - return true; - - var responce = false; - - var sub = SubscribeToPacketType(MAVLINK_MSG_ID.AUTOPILOT_VERSION, (buffer) => - { - if (buffer.msgid == (byte)MAVLINK_MSG_ID.AUTOPILOT_VERSION && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - responce = true; - return true; - } - return true; - }, (byte)sysid, (byte)compid); - - DateTime start = DateTime.Now; - int retrys = 3; - - while (!responce) - { - if (!(start.AddMilliseconds(500) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("getVersion Retry " + retrys); - generatePacket((byte)MAVLINK_MSG_ID.AUTOPILOT_VERSION_REQUEST, req); - start = DateTime.Now; - retrys--; - continue; - } - - break; - } - - await Task.Delay((100)); - } - - UnSubscribeToPacketType(sub); - - return responce; - } - - [Obsolete] - public async Task<(PointLatLngAlt plla, int total)> getFencePoint(int no) - { - MAVLinkMessage buffer; - - giveComport = true; - - PointLatLngAlt plla = new PointLatLngAlt(); - mavlink_fence_fetch_point_t req = new mavlink_fence_fetch_point_t(); - - req.idx = (byte) no; - req.target_component = MAV.compid; - req.target_system = MAV.sysid; - - // request point - generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req); - - DateTime start = DateTime.Now; - int retrys = 3; - - while (true) - { - if (!(start.AddMilliseconds(700) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("getFencePoint Retry " + retrys + " - giv com " + giveComport); - generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req); - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - throw new TimeoutException("Timeout on read - getFencePoint"); - } - - buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.FENCE_POINT && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - mavlink_fence_point_t fp = buffer.ToStructure(); - - // check this gcs sent it - if (fp.target_system != gcssysid || - fp.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER || - fp.idx != no) - continue; - - giveComport = false; - - plla.Lat = fp.lat; - plla.Lng = fp.lng; - plla.Tag = fp.idx.ToString(); - - return (plla, fp.count); - } - } - } - } - - [Obsolete] - public async Task GetLog(ushort no) - { - return new FileStream(await GetLog(MAV.sysid, MAV.compid, no), FileMode.Open, FileAccess.ReadWrite); - } - - public async Task GetLog(byte sysid, byte compid, ushort no) - { - var filename = Path.GetTempFileName(); - using (FileStream ms = new FileStream(filename, FileMode.Create, FileAccess.ReadWrite)) - { - Hashtable set = new Hashtable(); - - giveComport = false; - MAVLinkMessage buffer = MAVLinkMessage.Invalid; - - if (Progress != null) - { - Progress((int) 0, ""); - } - - uint totallength = 0; - uint ofs = 0; - uint bps = 0; - DateTime bpstimer = DateTime.Now; - - ConcurrentQueue queue = new ConcurrentQueue(); - EventHandler handler = (sender, msg) => - { - queue.Enqueue(msg); - }; - OnPacketReceived += handler; - - _OnPacketReceived.GetInvocationList().ForEach(a => log.Info(a.GetMethodInfo().ToJSON())); - - - mavlink_log_request_data_t req = new mavlink_log_request_data_t(); - - req.target_component = compid; - req.target_system = sysid; - req.id = no; - req.ofs = ofs; - // entire log - req.count = 0xFFFFFFFF; - - // request point - generatePacket((byte) MAVLINK_MSG_ID.LOG_REQUEST_DATA, req); - - DateTime start = DateTime.Now; - int retrys = 3; - - - while (true) - { - if (!(start.AddMilliseconds(3000) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("GetLog Retry " + retrys + " - giv com " + giveComport); - generatePacket((byte) MAVLINK_MSG_ID.LOG_REQUEST_DATA, req); - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - OnPacketReceived -= handler; - throw new TimeoutException("Timeout on read - GetLog"); - } - - var start1 = DateTime.Now; - if (!queue.TryDequeue(out buffer)) - { - Thread.Sleep(10); - buffer = MAVLinkMessage.Invalid; - } - var end = DateTime.Now - start1; - var lapse = end.TotalMilliseconds; - //Console.WriteLine("readPacketAsync: " + lapse); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.LOG_DATA && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - var data = buffer.ToStructure(); - - if (data.id != no) - continue; - - // reset retrys - retrys = 3; - start = DateTime.Now; - - bps += data.count; - - // record what we have received - set[(data.ofs / 90).ToString()] = 1; - - if (ms.Position != data.ofs) - ms.Seek((long) data.ofs, SeekOrigin.Begin); - ms.Write(data.data, 0, data.count); - - // update new start point - req.ofs = data.ofs + data.count; - - if (bpstimer.Second != DateTime.Now.Second) - { - if (Progress != null) - { - Progress((int) req.ofs, ""); - } - - //Console.WriteLine("log dl bps: " + bps.ToString()); - bpstimer = DateTime.Now; - bps = 0; - } - - // if data is less than max packet size or 0 > exit - if (data.count < 90 || data.count == 0) - { - totallength = data.ofs + data.count; - log.Info("start fillin len " + totallength + " count " + set.Count + " datalen " + - data.count); - break; - } - } - } - } - - log.Info("set count " + set.Count); - log.Info("count total " + ((totallength) / 90 + 1)); - log.Info("totallength " + totallength); - log.Info("current length " + ms.Length); - - while (true && ((BaseStream != null && BaseStream.IsOpen) || logreadmode)) - { - if (totallength == ms.Length && set.Count >= ((totallength) / 90 + 1)) - { - giveComport = false; - OnPacketReceived -= handler; - return filename; - } - - if (!(start.AddMilliseconds(500) > DateTime.Now)) - { - for (int a = 0; a < ((totallength) / 90 + 1); a++) - { - if (!set.ContainsKey(a.ToString())) - { - // request large chunk if they are back to back - uint bytereq = 90; - int b = a + 1; - while (!set.ContainsKey(b.ToString())) - { - bytereq += 90; - b++; - } - - req.ofs = (uint) (a * 90); - req.count = bytereq; - log.Info("req missing " + req.ofs + " bytes " + req.count + " got " + set.Count + "/" + - ((totallength) / 90 + 1)); - generatePacket((byte) MAVLINK_MSG_ID.LOG_REQUEST_DATA, req); - start = DateTime.Now; - break; - } - } - } - - if (!queue.TryDequeue(out buffer)) - { - Thread.Sleep(10); - buffer = MAVLinkMessage.Invalid; - } - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.LOG_DATA && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - var data = buffer.ToStructure(); - - if (data.id != no) - continue; - - // reset retrys - retrys = 3; - start = DateTime.Now; - - bps += data.count; - - // record what we have received - set[(data.ofs / 90).ToString()] = 1; - - ms.Seek((long) data.ofs, SeekOrigin.Begin); - ms.Write(data.data, 0, data.count); - - // update new start point - req.ofs = data.ofs + data.count; - - if (bpstimer.Second != DateTime.Now.Second) - { - if (Progress != null) - { - Progress((int) req.ofs, ""); - } - - //Console.WriteLine("log dl bps: " + bps.ToString()); - bpstimer = DateTime.Now; - bps = 0; - } - - // check if we have next set and invalidate to request next packets - if (set.ContainsKey(((data.ofs / 90) + 1).ToString())) - { - start = DateTime.MinValue; - } - - // if data is less than max packet size or 0 > exit - if (data.count < 90 || data.count == 0) - { - continue; - } - } - } - } - - OnPacketReceived -= handler; - throw new Exception("Failed to get log"); - } - } - - [Obsolete] - public List GetLogList() - { - List ans = new List(); - - return GetLogEntry(0, ushort.MaxValue).AwaitSync().Values - .OrderBy(a => a.id).ToList(); - } - - [Obsolete] - public void GetMountStatus() - { - mavlink_mount_status_t req = new mavlink_mount_status_t(); - req.target_component = MAV.compid; - req.target_system = MAV.sysid; - - generatePacket((byte) MAVLINK_MSG_ID.MOUNT_STATUS, req); - } - - [Obsolete] - public async Task> GetLogEntry(ushort startno = 0, - ushort endno = ushort.MaxValue) - { - giveComport = true; - MAVLinkMessage buffer; - - Dictionary ans = new Dictionary(); - - mavlink_log_request_list_t req = new mavlink_log_request_list_t(); - - req.target_component = MAV.compid; - req.target_system = MAV.sysid; - req.start = startno; - req.end = endno; - - log.Info("GetLogEntry " + startno + "-" + endno); - - // request point - generatePacket((byte) MAVLINK_MSG_ID.LOG_REQUEST_LIST, req); - - DateTime start = DateTime.Now; - int retrys = 4; - - while (true) - { - if (!(start.AddMilliseconds(5000) > DateTime.Now)) - { - if (retrys > 0) - { - req.start = startno; - req.end = endno; - log.Info("GetLogEntry Retry " + retrys + " - giv com " + giveComport); - generatePacket((byte) MAVLINK_MSG_ID.LOG_REQUEST_LIST, req); - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - throw new TimeoutException("Timeout on read - GetLogEntry"); - } - - buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.LOG_ENTRY && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - var loge = buffer.ToStructure(); - - if (loge.id >= startno && loge.id <= endno) - { - if (loge.num_logs == 0 && loge.last_log_num == 0) - { - giveComport = false; - return ans; - } - - // reset timeout - start = DateTime.Now; - // add the log to our answer - ans[loge.id] = loge; - // set the startno to our new min - startno = (ushort) Math.Min(ans.Keys.Min() + 1, loge.id); - // set the end number to logmax - endno = loge.last_log_num; - if (ans.Count >= loge.num_logs) - { - giveComport = false; - return ans; - } - } - } - - if (buffer.msgid == (byte) MAVLINK_MSG_ID.LOG_DATA && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - giveComport = false; - throw new Exception("Existing log download already in progress."); - } - } - } - } - - [Obsolete] - public void EraseLog() - { - mavlink_log_erase_t req = new mavlink_log_erase_t(); - - req.target_component = MAV.compid; - req.target_system = MAV.sysid; - - // send twice - we have no feedback on this - generatePacket((byte) MAVLINK_MSG_ID.LOG_ERASE, req); - generatePacket((byte) MAVLINK_MSG_ID.LOG_ERASE, req); - } - - [Obsolete] - public async Task> getRallyPoints() - { - List points = new List(); - - if (!MAV.param.ContainsKey("RALLY_TOTAL")) - return points; - - int count = int.Parse(MAV.param["RALLY_TOTAL"].ToString()); - - for (int a = 0; a < (count - 1); a++) - { - try - { - var plla = await getRallyPoint(a).ConfigureAwait(false); - points.Add(plla.plla); - } - catch - { - return points; - } - } - - return points; - } - - [Obsolete] - public async Task<(PointLatLngAlt plla, int total)> getRallyPoint(int no) - { - MAVLinkMessage buffer; - - giveComport = true; - - PointLatLngAlt plla = new PointLatLngAlt(); - mavlink_rally_fetch_point_t req = new mavlink_rally_fetch_point_t(); - - req.idx = (byte) no; - req.target_component = MAV.compid; - req.target_system = MAV.sysid; - - // request point - generatePacket((byte) MAVLINK_MSG_ID.RALLY_FETCH_POINT, req); - - DateTime start = DateTime.Now; - int retrys = 3; - - while (true) - { - if (!(start.AddMilliseconds(700) > DateTime.Now)) - { - if (retrys > 0) - { - log.Info("getRallyPoint Retry " + retrys + " - giv com " + giveComport); - generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req); - start = DateTime.Now; - retrys--; - continue; - } - - giveComport = false; - throw new TimeoutException("Timeout on read - getRallyPoint"); - } - - buffer = await readPacketAsync().ConfigureAwait(false); - if (buffer.Length > 5) - { - if (buffer.msgid == (byte) MAVLINK_MSG_ID.RALLY_POINT && buffer.sysid == req.target_system && - buffer.compid == req.target_component) - { - mavlink_rally_point_t fp = buffer.ToStructure(); - - // check this gcs sent it - if (fp.target_system != gcssysid || - fp.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) - continue; - - if (req.idx != fp.idx) - { - generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req); - continue; - } - - plla.Lat = fp.lat / t7; - plla.Lng = fp.lng / t7; - plla.Tag = fp.idx.ToString(); - plla.Alt = fp.alt; - - giveComport = false; - - return (plla, fp.count); - } - } - } - } - - [Obsolete] - public bool setFencePoint(byte index, PointLatLngAlt plla, byte fencepointcount) - { - mavlink_fence_point_t fp = new mavlink_fence_point_t(); - - fp.idx = index; - fp.count = fencepointcount; - fp.lat = (float) plla.Lat; - fp.lng = (float) plla.Lng; - fp.target_component = MAV.compid; - fp.target_system = MAV.sysid; - - int retry = 3; - - while (retry > 0) - { - generatePacket((byte) MAVLINK_MSG_ID.FENCE_POINT, fp); - var newfp = getFencePoint(fp.idx).AwaitSync(); - - if (newfp.plla.GetDistance(plla) < 5) - return true; - retry--; - } - - throw new Exception("Could not verify GeoFence Point"); - } - - [Obsolete] - public bool setRallyPoint(byte index, PointLatLngAlt plla, short break_alt, UInt16 land_dir_cd, byte flags, - byte rallypointcount) - { - mavlink_rally_point_t rp = new mavlink_rally_point_t(); - - rp.idx = index; - rp.count = rallypointcount; - rp.lat = (int) (plla.Lat * t7); - rp.lng = (int) (plla.Lng * t7); - rp.alt = (short) plla.Alt; - rp.break_alt = break_alt; - rp.land_dir = land_dir_cd; - rp.flags = (byte) flags; - rp.target_component = MAV.compid; - rp.target_system = MAV.sysid; - - int retry = 3; - - while (retry > 0) - { - generatePacket((byte) MAVLINK_MSG_ID.RALLY_POINT, rp); - - var newfp = getRallyPoint(rp.idx).GetAwaiter().GetResult(); - - if (newfp.plla.Lat == plla.Lat && newfp.plla.Lng == rp.lng) - { - Console.WriteLine("Rally Set"); - return true; - } - - retry--; - } - - return false; - } - - public enum sensoroffsetsenum - { - gyro = 0, - accelerometer = 1, - magnetometer = 2, - barometer = 3, - optical_flow = 4, - second_magnetometer = 5 - } - - public bool SetSensorOffsets(byte sysid, byte compid, sensoroffsetsenum sensor, float x, float y, float z) - { - return doCommand(sysid, compid, MAV_CMD.PREFLIGHT_SET_SENSOR_OFFSETS, (int) sensor, x, y, z, 0, 0, 0); - } - - public void send_text(byte severity, string txt) - { - var temp = Encoding.ASCII.GetBytes(txt); - generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT, - new mavlink_statustext_t() {severity = (byte) severity, text = temp}); - } - - private Dictionary> streamfncache = new Dictionary>(); - private EventHandler _OnPacketReceived; - private EventHandler _OnPacketSent; - private static EventHandler _UpdateADSBPlanePosition; - private static EventHandler<(string id, MAV_COLLISION_THREAT_LEVEL threat_level)> _UpdateADSBCollision; - private EventHandler _ParamListChanged; - private EventHandler _MavChanged; - private EventHandler _CommsClose; - public bool printbps = true; - private bool _openComplete = true; - - private MAVLinkMessage readlogPacketMavlink() - { - byte[] datearray = new byte[8]; - - bool missingtimestamp = false; - - if (logplaybackfile.BaseStream is FileStream) - { - if (!streamfncache.ContainsKey(_logplaybackfile.BaseStream)) - streamfncache[_logplaybackfile.BaseStream] = new Tuple( - ((FileStream) _logplaybackfile.BaseStream).Name.ToLower(), logplaybackfile.BaseStream.Length); - - if (streamfncache[_logplaybackfile.BaseStream].Item1.EndsWith(".rlog")) - missingtimestamp = true; - } - else - { - if (!streamfncache.ContainsKey(_logplaybackfile.BaseStream)) - streamfncache[_logplaybackfile.BaseStream] = - new Tuple("", logplaybackfile.BaseStream.Length); - } - - if (!missingtimestamp) - { - int tem = logplaybackfile.BaseStream.Read(datearray, 0, datearray.Length); - - Array.Reverse(datearray); - - DateTime date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc); - - UInt64 dateint = BitConverter.ToUInt64(datearray, 0); - - try - { - // array is reversed above - if (datearray[7] == 254 || datearray[7] == 253) - { - //rewind 8bytes - logplaybackfile.BaseStream.Seek(-8, SeekOrigin.Current); - } - else - { - if ((dateint / 1000 / 1000 / 60 / 60) < 9999999) - { - date1 = date1.AddMilliseconds(dateint / 1000); - - lastlogread = date1.ToLocalTime(); - lastlogreadwall = DateTime.Now; - } - } - } - catch - { - } - } - - byte[] temp = new byte[0]; - - byte byte0 = 0; - byte byte1 = 0; - byte byte2 = 0; - - var filelength = streamfncache[_logplaybackfile.BaseStream].Item2; - var filepos = logplaybackfile.BaseStream.Position; - - if (filelength == filepos) - return MAVLinkMessage.Invalid; - - int length = 5; - int a = 0; - while (a < length) - { - if (filelength == filepos) - return MAVLinkMessage.Invalid; - - var tempb = (byte) logplaybackfile.ReadByte(); - filepos++; - - switch (a) - { - case 0: - byte0 = tempb; - if (byte0 != MAVLINK_STX_MAVLINK1 && byte0 != MAVLINK_STX) - { - log.DebugFormat("logread - lost sync byte {0} pos {1}", byte0, - logplaybackfile.BaseStream.Position); - // seek to next valid - do - { - byte0 = logplaybackfile.ReadByte(); - } while (byte0 != MAVLINK_STX_MAVLINK1 && byte0 != MAVLINK_STX); - - a = 1; - continue; - } - - break; - - case 1: - byte1 = tempb; - // handle length - { - int headerlength = byte0 == MAVLINK_STX ? 9 : 5; - int headerlengthstx = headerlength + 1; - - length = byte1 + headerlengthstx + 2; // header + 2 checksum - } - break; - - case 2: - byte2 = tempb; - // handle signing and mavlink2 - if (byte0 == MAVLINK_STX) - { - if ((byte2 & MAVLINK_IFLAG_SIGNED) > 0) - length += MAVLINK_SIGNATURE_BLOCK_LEN; - } - - // handle rest - { - temp = new byte[length]; - temp[0] = byte0; - temp[1] = byte1; - temp[2] = byte2; - - var readto = a + 1; - var readlength = length - (a + 1); - logplaybackfile.Read(temp, readto, readlength); - a = length; - } - break; - } - - a++; - } - - MAVLinkMessage tmp = new MAVLinkMessage(temp, lastlogread); - - MAVlist[tmp.sysid, tmp.compid].cs.datetime = lastlogread; - - return tmp; - } - - [Obsolete] - public bool translateMode(string modein, ref mavlink_set_mode_t mode) - { - return translateMode(MAV.sysid, MAV.compid, modein, ref mode); - } - - public bool translateMode(byte sysid, byte compid, string modein, ref mavlink_set_mode_t mode) - { - mode.target_system = sysid; - - if (modein == null || modein == "") - return false; - - try - { - List> modelist = Common.getModesList(MAVlist[sysid, compid].cs.firmware); - - foreach (KeyValuePair pair in modelist) - { - if (pair.Value.ToLower() == modein.ToLower()) - { - mode.base_mode = (byte) MAV_MODE_FLAG.CUSTOM_MODE_ENABLED; - mode.custom_mode = (uint) pair.Key; - } - } - - if (mode.base_mode == 0) - { - log.Error("No Mode Changed " + modein); - return false; - } - } - catch - { - log.Error("Failed to find Mode"); - return false; - } - - return true; - } - - public void setAPType(byte sysid, byte compid) - { - MAVlist[sysid, compid].sysid = sysid; - MAVlist[sysid, compid].compid = compid; - - switch (MAVlist[sysid, compid].apname) - { - case MAV_AUTOPILOT.ARDUPILOTMEGA: - // try and pull vehicle type from version string - if (MAVlist[sysid, compid].VersionString != null && MAVlist[sysid, compid].VersionString.Length > 1) - { - var firmware_lookup = new Dictionary() - { - {"ArduPlane V", Firmwares.ArduPlane}, - {"ArduCopter V", Firmwares.ArduCopter2}, - {"Blimp V", Firmwares.ArduCopter2}, - {"ArduRover V",Firmwares.ArduRover}, - {"ArduSub V",Firmwares.ArduSub}, - {"AntennaTracker V", Firmwares.ArduTracker} - }; - var match = firmware_lookup.Where(d => MAVlist[sysid, compid].VersionString.StartsWith(d.Key)).ToArray(); - if (match.Count() == 1) - { - MAVlist[sysid, compid].cs.firmware = match[0].Value; - break; - } - } - - switch (MAVlist[sysid, compid].aptype) - { - case MAV_TYPE.FIXED_WING: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduPlane; - break; - - case MAV_TYPE n when (n >= MAV_TYPE.VTOL_DUOROTOR && n <= MAV_TYPE.VTOL_RESERVED5): - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduPlane; - break; - - case MAV_TYPE.FLAPPING_WING: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduPlane; - break; - - case MAV_TYPE.QUADROTOR: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; - break; - - case MAV_TYPE.TRICOPTER: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; - break; - - case MAV_TYPE.HEXAROTOR: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; - break; - - case MAV_TYPE.OCTOROTOR: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; - break; - - case MAV_TYPE.HELICOPTER: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; - break; - - case MAV_TYPE.DODECAROTOR: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; - break; - - case MAV_TYPE.GROUND_ROVER: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduRover; - break; - - case MAV_TYPE.SURFACE_BOAT: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduRover; - break; - - case MAV_TYPE.SUBMARINE: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduSub; - break; - - case MAV_TYPE.ANTENNA_TRACKER: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduTracker; - break; - case MAV_TYPE.COAXIAL: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; - break; - default: - MAVlist[sysid, compid].cs.firmware = Firmwares.Other; - log.Error(MAVlist[sysid, compid].aptype + " not registered as valid type"); - break; - } - - break; - - case MAV_AUTOPILOT.UDB: - switch (MAVlist[sysid, compid].aptype) - { - case MAV_TYPE.FIXED_WING: - MAVlist[sysid, compid].cs.firmware = Firmwares.ArduPlane; - break; - } - - break; - - case MAV_AUTOPILOT.GENERIC: - switch (MAVlist[sysid, compid].aptype) - { - case MAV_TYPE.FIXED_WING: - MAVlist[sysid, compid].cs.firmware = Firmwares.Ateryx; - break; - } - - break; - - case MAV_AUTOPILOT.PX4: - MAVlist[sysid, compid].cs.firmware = Firmwares.PX4; - break; - - default: - switch (MAVlist[sysid, compid].aptype) - { - case MAV_TYPE.GIMBAL: // storm32 - name 83 - MAVlist[sysid, compid].cs.firmware = Firmwares.Gimbal; - break; - } - - break; - } - } - - public override string ToString() - { - if (BaseStream != null && BaseStream.IsOpen) - return "MAV " + MAV.sysid + " on " + BaseStream.PortName; - - if (logreadmode) - return "MAV " + MAV.sysid + " on LogFile"; - - return "MAV " + MAV.sysid + " on Ice"; - } - - public void Dispose() - { - if (_bytesReceivedSubj != null) - _bytesReceivedSubj.Dispose(); - if (_bytesSentSubj != null) - _bytesSentSubj.Dispose(); - if (MAVlist != null) - MAVlist.Dispose(); - - this.Close(); - - Terrain?.UnSub(); - Terrain = null; - - Mirrors.Clear(); - - logreadmode = false; - logplaybackfile = null; - } - - public void uAvionixADSBControl(int baroAltMSL,ushort squawk,/*UAVIONIX_ADSB_OUT_CONTROL_STATE*/byte state,/*UAVIONIX_ADSB_EMERGENCY_STATUS*/byte emergencyStatus,byte[] flight_id,byte x_bit) - { - mavlink_uavionix_adsb_out_control_t ctrl = new mavlink_uavionix_adsb_out_control_t(); - ctrl.state = state; - ctrl.baroAltMSL = baroAltMSL; - ctrl.squawk = squawk; - ctrl.emergencyStatus = emergencyStatus; - ctrl.flight_id = flight_id; - ctrl.x_bit = x_bit; - generatePacket(MAVLINK_MSG_ID.UAVIONIX_ADSB_OUT_CONTROL, ctrl); - } - - private class NoUIReporter : IProgressReporterDialogue - { - private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - - public ProgressWorkerEventArgs doWorkArgs { get; set; } = new ProgressWorkerEventArgs(); - - public event DoWorkEventHandler DoWork; - - public void BeginInvoke(Delegate method) - { - method?.DynamicInvoke(); - } - - public void Dispose() - { - - } - - public void RunBackgroundOperationAsync() - { - Task.Run(() => - { - try - { - log.Info("DoWork"); - if (this.DoWork != null) this.DoWork(this); - log.Info("DoWork Done"); - } - catch (Exception e) - { - } - }).Wait(); - } - - public void UpdateProgressAndStatus(int progress, string status) - { - log.Info(progress + "% " + status); - } - } - } -} +using log4net; +using MissionPlanner.ArduPilot; +using MissionPlanner.ArduPilot.Mavlink; +using MissionPlanner.Comms; +using MissionPlanner.Mavlink; +using MissionPlanner.Utilities; +using Newtonsoft.Json; +using System; +using System.Collections; +using System.Collections.Concurrent; +using System.Collections.Generic; +using System.Diagnostics; +using System.IO; +using System.Linq; +using System.Reactive.Subjects; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Security.Cryptography; +using System.Text; +using System.Text.RegularExpressions; +using System.Threading; +using System.Threading.Tasks; +using Timer = System.Timers.Timer; + +namespace MissionPlanner +{ + public class MAVLinkInterface : MAVLink, IDisposable, IMAVLinkInterface, IMAVLinkInterfaceLogRead + { + private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + private ICommsSerial _baseStream; + + public ICommsSerial BaseStream + { + get { return _baseStream; } + set + { + // This is called every time user changes the port selection, so we need to make sure we cleanup + // any previous objects so we don't leave the cleanup of system resources to the garbage collector. + if (_baseStream != null) + { + try + { + if (_baseStream.IsOpen) + { + _baseStream.Close(); + } + } + catch + { + } + + IDisposable dsp = _baseStream as IDisposable; + if (dsp != null) + { + try + { + dsp.Dispose(); + } + catch + { + } + } + } + + _baseStream = value; + } + } + + public event EventHandler OnPacketReceived + { + add + { + log.Debug("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value?.Method?.Name + + " " + value?.Target?.GetType()?.Name + + " " + value); + _OnPacketReceived += value; + } + + remove + { + log.Debug("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + + value?.Method?.Name + " " + value?.Target?.GetType()?.Name + + " " + value); + _OnPacketReceived -= value; + } + } + + public event EventHandler<(byte, byte)> MAVDetected + { + add + { + log.Debug("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value?.Method?.Name + + " " + value?.Target?.GetType()?.Name + + " " + value); + _MAVDetected += value; + } + + remove + { + log.Debug("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + + value?.Method?.Name + " " + value?.Target?.GetType()?.Name + + " " + value); + _MAVDetected -= value; + } + } + + private EventHandler<(byte, byte)> _MAVDetected; + + public void DoOnPacketSent(MAVLinkMessage pkt) + { + _OnPacketSent?.Invoke(this, pkt); + } + + public event EventHandler OnPacketSent + { + add + { + log.Debug("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _OnPacketSent += value; + } + remove + { + log.Debug("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _OnPacketSent -= value; + } + } + + public static event EventHandler UpdateADSBPlanePosition + { + add + { + log.Info("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _UpdateADSBPlanePosition += value; + } + remove + { + log.Info("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _UpdateADSBPlanePosition -= value; + } + } + + public static event EventHandler<(string id, MAV_COLLISION_THREAT_LEVEL threat_level)> UpdateADSBCollision + { + add + { + log.Info("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _UpdateADSBCollision += value; + } + remove + { + log.Info("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _UpdateADSBCollision -= value; + } + } + + public class Mirror + { + public ICommsSerial MirrorStream { get; set; } + public bool MirrorStreamWrite { get; set; } + } + + public List Mirrors { get; set; } = new List(); + + public event EventHandler ParamListChanged + { + add + { + log.Debug("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _ParamListChanged += value; + } + remove + { + log.Debug("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _ParamListChanged -= value; + } + } + + public event EventHandler MavChanged + { + add + { + log.Info("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _MavChanged += value; + } + remove + { + log.Info("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _MavChanged -= value; + } + } + + public event EventHandler CommsClose + { + add + { + log.Info("Subscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _CommsClose += value; + } + remove + { + log.Info("UnSubscribed " + new StackTrace(1, true)?.GetFrame(0)?.ToString() + " " + value.Method.Name + + " " + value.Target.GetType().Name + + " " + value); + _CommsClose -= value; + } + } + + public static byte gcssysid { get; set; } = 255; + + private string lastset = ""; + + /// + /// used to prevent comport access for exclusive use + /// + public bool giveComport + { + get + { + //Console.WriteLine("get giveComport {0} current {1}", Thread.CurrentThread.Name, _giveComport); + return _giveComport; + } + set + { + log.InfoFormat("set giveComport {0} current {1} new {2}", Thread.CurrentThread.Name, _giveComport, + value); + if (_giveComport && value) + { + // trying to set it true twice + log.Error(new System.Diagnostics.StackTrace().ToString()); + Debugger.Break(); + } + + if (value == true) + { + // wait for current read to end before continue + lock (readlock) + { + } + } + + lastset = Thread.CurrentThread.Name; + _giveComport = value; + } + } + + private volatile bool _giveComport = false; + + private DateTime lastparamset = DateTime.MinValue; + + /// + /// stores a list of strings during connect for possible debug + /// + internal List plaintxtlinebuffer = new List(30); + + /// + /// stores a single string during connect for possible debug + /// + internal string plaintxtline = ""; + private string buildplaintxtline = ""; + + public bool ReadOnly = false; + + public TerrainFollow Terrain; + + public event ProgressEventHandler Progress; + + private int _sysidcurrent = 0; + + public int sysidcurrent + { + get { return _sysidcurrent; } + set + { + if (_sysidcurrent == value) + return; + _sysidcurrent = value; + if (_MavChanged != null) _MavChanged(this, null); + } + } + + private int _compidcurrent = 0; + + public int compidcurrent + { + get { return _compidcurrent; } + set + { + if (_compidcurrent == value) + return; + _compidcurrent = value; + if (_MavChanged != null) _MavChanged(this, null); + } + } + + public MAVList MAVlist; + + public MAVState MAV + { + get { return MAVlist[sysidcurrent, compidcurrent]; } + set { MAVlist[sysidcurrent, compidcurrent] = value; } + } + + public double CONNECT_TIMEOUT_SECONDS = Settings.Instance.GetDouble("CONNECT_TIMEOUT_SECONDS", 30.0); + + /// + /// progress form to handle connect and param requests + /// + public IProgressReporterDialogue frmProgressReporter; + + /// + /// used for outbound packet sending + /// + public int packetcount { get; internal set; } = 0; + + private readonly Subject _bytesReceivedSubj = new Subject(); + private readonly Subject _bytesSentSubj = new Subject(); + + /// + /// Observable of the count of bytes received, notified when the bytes themselves are received + /// + public IObservable BytesReceived + { + get { return _bytesReceivedSubj; } + } + + /// + /// Observable of the count of bytes sent, notified when the bytes themselves are received + /// + public IObservable BytesSent + { + get { return _bytesSentSubj; } + } + + /// + /// Observable of the count of packets skipped (on reception), + /// calculated from periods where received packet sequence is not + /// contiguous + /// + public Subject WhenPacketLost { get; set; } + + public Subject WhenPacketReceived { get; set; } + + /// + /// used as a serial port write lock + /// + public volatile object writelock = new object(); + + /// + /// mavlink version + /// + private byte mavlinkversion = 0; + + /// + /// turns on console packet display + /// + public bool debugmavlink { get; set; } + + /// + /// enabled read from file mode + /// + public bool logreadmode + { + get { return _logreadmode; } + set { _logreadmode = value; } + } + + /// + /// used to disable all speech originating from this module + /// + public bool speechenabled { get; set; } = true; + + private bool _logreadmode = false; + + private BinaryReader _logplaybackfile; + + public DateTime lastlogread { get; set; } + public DateTime lastlogreadwall { get; set; } + + public BinaryReader logplaybackfile + { + get { return _logplaybackfile; } + set + { + _logplaybackfile = value; + if (_logplaybackfile != null && _logplaybackfile.BaseStream is FileStream) + log.Info("Logplaybackfile set " + ((FileStream)_logplaybackfile.BaseStream).Name); + MAVlist.Clear(); + } + } + + public BufferedStream logfile { get; set; } + public BufferedStream rawlogfile { get; set; } + + public int Mavlink1Count { get { return _mavlink1count; } } + public int Mavlink2Count { get { return _mavlink2count; } } + public int Mavlink2Signed { get { return _mavlink2signed; } } + + private int _mavlink1count = 0; + private int _mavlink2count = 0; + private int _mavlink2signed = 0; + private int _bps1 = 0; + private int _bps2 = 0; + private DateTime _bpstime { get; set; } + + public bool MirrorStreamWrite { + get { + if (Mirrors.Count > 0) + return Mirrors[0].MirrorStreamWrite; + + Mirrors.Add(new Mirror()); + return MirrorStreamWrite; + } + set + { + if (Mirrors.Count > 0) + Mirrors[0].MirrorStreamWrite = value; + else + Mirrors.Add(new Mirror() { MirrorStreamWrite = value }); + } + } + public ICommsSerial MirrorStream { + get + { + if (Mirrors.Count > 0) + return Mirrors[0].MirrorStream; + + Mirrors.Add(new Mirror()); + return MirrorStream; + } + set + { + if (Mirrors.Count > 0) + Mirrors[0].MirrorStream = value; + else + Mirrors.Add(new Mirror() { MirrorStream = value }); + } + } + + + public static ISpeech Speech; + + ~MAVLinkInterface() + { + this.Dispose(); + } + + public MAVLinkInterface() + { + // init fields + MAVlist = new MAVList(this); + this.packetcount = 0; + this._bytesReceivedSubj = new Subject(); + this._bytesSentSubj = new Subject(); + this.WhenPacketLost = new Subject(); + this.WhenPacketReceived = new Subject(); + + this.mavlinkversion = 0; + + this.debugmavlink = false; + this.logreadmode = false; + this.lastlogread = DateTime.MinValue; + this._logplaybackfile = null; + this.logfile = null; + this.rawlogfile = null; + this._bps1 = 0; + this._bps2 = 0; + this._bpstime = DateTime.MinValue; + _mavlink1count = 0; + _mavlink2count = 0; + _mavlink2signed = 0; + + AIS.Start(this); + + // new hearbeat detected + MAVDetected += OnMAVDetected; + } + + private void OnMAVDetected(object sender, (byte, byte) tuple) + { + + // check for a camera + if (tuple.Item2 == (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1 || + (tuple.Item2 >= (byte) MAVLink.MAV_COMPONENT.MAV_COMP_ID_CAMERA && + tuple.Item2 <= (byte) MAV_COMPONENT.MAV_COMP_ID_CAMERA6)) + { + MAVlist[tuple.Item1, tuple.Item2].Camera = new CameraProtocol(); + Task.Run(async () => + { + try + { + // Open holds this + while (!_openComplete) + { + await Task.Delay(1000); + } + + await Task.Delay(2000); + + if (MAVlist[tuple.Item1, tuple.Item2].Camera == null) + return; + + while(giveComport) + await Task.Delay(100); + + await MAVlist[tuple.Item1, tuple.Item2] + .Camera.StartID(MAVlist[tuple.Item1, tuple.Item2]) + .ConfigureAwait(false); + } + catch (Exception e) + { + log.Error(e); + } + }); + } + // gimbals + if (tuple.Item2 >= (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_GIMBAL && + tuple.Item2 <= (byte)MAV_COMPONENT.MAV_COMP_ID_GIMBAL6) + { + MAVlist[tuple.Item1, tuple.Item2].Gimbal = new GimbalProtocol(); + Task.Run(async () => + { + try + { + // Open holds this + while (!_openComplete) + { + await Task.Delay(1000); + } + + await Task.Delay(2000); + + MAVlist[tuple.Item1, tuple.Item2] + .Gimbal?.Discover(this); + } + catch (Exception e) + { + log.Error(e); + } + }); + } + + if (tuple.Item2 == (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1 || + (tuple.Item2 >= (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER && + tuple.Item2 <= (byte)MAV_COMPONENT.MAV_COMP_ID_ONBOARD_COMPUTER4)) + { + MAVlist[tuple.Item1, tuple.Item2].GimbalManager = new GimbalManagerProtocol(this, MAVlist[tuple.Item1, tuple.Item2].cs); + Task.Run(async () => + { + try + { + // Open holds this + while (!_openComplete) + { + await Task.Delay(1000); + } + + await Task.Delay(2000); + + MAVlist[tuple.Item1, tuple.Item2] + .GimbalManager?.Discover(); + } + catch (Exception e) + { + log.Error(e); + } + }); + } + } + + public MAVLinkInterface(Stream logfileStream) + : this() + { + logplaybackfile = new BinaryReader(logfileStream); + logreadmode = true; + } + + public void Close() + { + try + { + Terrain?.UnSub(); + Terrain = null; + } + catch + { + + } + + try + { + giveComport = false; + + if (logfile != null) + logfile.Close(); + } + catch + { + } + + try + { + if (rawlogfile != null) + rawlogfile.Close(); + } + catch + { + } + + try + { + if (logplaybackfile != null) + logplaybackfile.Close(); + } + catch + { + } + + try + { + if (BaseStream != null && BaseStream.IsOpen) + BaseStream.Close(); + } + catch + { + } + + try + { + if (_CommsClose != null) + _CommsClose(this, null); + } + catch + { + } + } + + public delegate IProgressReporterDialogue ProgressEventHandle(string title); + + public static event ProgressEventHandle CreateIProgressReporterDialogue; + + public void Open() + { + Open(false); + } + + public void Open(bool getparams, bool skipconnectedcheck = false, bool showui = true) + { + if (BaseStream == null || BaseStream.IsOpen && !skipconnectedcheck) + return; + + MAVlist.Clear(); + + if (showui) + frmProgressReporter = CreateIProgressReporterDialogue(Strings.ConnectingMavlink); + else + frmProgressReporter = new NoUIReporter(); + + if (getparams) + { + frmProgressReporter.DoWork += FrmProgressReporterDoWorkAndParams; + } + else + { + frmProgressReporter.DoWork += FrmProgressReporterDoWorkNOParams; + } + + frmProgressReporter.UpdateProgressAndStatus(-1, Strings.MavlinkConnecting); + + frmProgressReporter.RunBackgroundOperationAsync(); + + frmProgressReporter.Dispose(); + + if (_ParamListChanged != null) + { + _ParamListChanged(this, null); + } + } + + private void FrmProgressReporterDoWorkAndParams(IProgressReporterDialogue sender) + { + OpenBg(sender, true); + } + + private void FrmProgressReporterDoWorkNOParams(IProgressReporterDialogue sender) + { + OpenBg(sender, false); + } + + private void OpenBg(IProgressReporterDialogue PRsender, bool getparams) + { + _openComplete = false; + + frmProgressReporter.UpdateProgressAndStatus(-1, Strings.MavlinkConnecting); + + if (BaseStream is SerialPort) + { + // allow settings to settle - previous dtr + log.Debug("SerialPort Sleep 1"); + Thread.Sleep(1000); + } + + Terrain?.UnSub(); + Terrain = new TerrainFollow(this); + + bool hbseen = false; + + try + { + BaseStream.ReadBufferSize = 16 * 1024; + + lock (writelock) // so we dont have random traffic + { + log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate); + + if (BaseStream is UdpSerial) + { + PRsender.doWorkArgs.CancelRequestChanged += (o, e) => + { + ((UdpSerial) BaseStream).CancelConnect = true; + ((ProgressWorkerEventArgs) o) + .CancelAcknowledged = true; + }; + } + + BaseStream.Open(); + + BaseStream.DiscardInBuffer(); + + // other boards seem to have issues if there is no delay? posible bootloader timeout issue + if (BaseStream is SerialPort) + { + log.Debug("SerialPort Sleep 2"); + Thread.Sleep(1000); + } + } + + List hbhistory = new List(); + + DateTime start = DateTime.Now; + DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS); + + var countDown = new Timer {Interval = 1000, AutoReset = false}; + countDown.Elapsed += (sender, e) => + { + int secondsRemaining = Convert.ToInt32((deadline - e.SignalTime).TotalSeconds); + frmProgressReporter.UpdateProgressAndStatus(-1, string.Format(Strings.Trying, secondsRemaining)); + if (secondsRemaining > 0) countDown.Start(); + }; + countDown.Start(); + + int count = 0; + + plaintxtline = ""; + + while (true) + { + if (PRsender.doWorkArgs.CancelRequested) + { + PRsender.doWorkArgs.CancelAcknowledged = true; + countDown.Stop(); + if (BaseStream.IsOpen) + BaseStream.Close(); + giveComport = false; + return; + } + + log.Info(DateTime.Now.Millisecond + " Start connect loop "); + + if (DateTime.Now > deadline) + { + //if (Progress != null) + // Progress(-1, "No Heartbeat Packets"); + countDown.Stop(); + this.Close(); + + if (hbseen) + { + PRsender.doWorkArgs.ErrorMessage = Strings.Only1Hb; + throw new Exception(Strings.Only1HbD + plaintxtlinebuffer.Aggregate((a, b) => a + "\r\n" + b)); + } + else + { + PRsender.doWorkArgs.ErrorMessage = "No Heartbeat Packets Received"; + throw new TimeoutException(@"Can not establish a connection + +No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup +Mission Planner waits for 2 valid heartbeat packets before connecting +" + plaintxtlinebuffer.Aggregate((a, b) => a + "\r\n" + b)); + } + } + + Thread.Sleep(1); + + var buffer = getHeartBeat(); + if (buffer.Length > 0) + { + if (buffer.msgid == (ulong)MAVLINK_MSG_ID.HEARTBEAT) + { + mavlink_heartbeat_t hb = buffer.ToStructure(); + + // no GCS's and no broadcast compid's (ping adsb) + if (hb.type != (byte)MAV_TYPE.GCS && buffer.compid != 0) + { + hbhistory.Add(buffer); + } + } + else if (buffer.msgid == (ulong)MAVLINK_MSG_ID.HIGH_LATENCY2) + { + mavlink_high_latency2_t hl = buffer.ToStructure(); + // no GCS's and no broadcast compid's (ping adsb) + if (hl.type != (byte)MAV_TYPE.GCS && buffer.compid != 0) + { + hbhistory.Add(buffer); + } + } + else + { + continue; + } + } + + if (hbhistory.Count > 0) + hbseen = true; + + count++; + + // if we get no data, try enableing rts/cts + if (buffer.Length == 0 && BaseStream is SerialPort && start.AddSeconds(20) < DateTime.Now) + { + try + { + log.Debug("about to set RTS to " + !BaseStream.RtsEnable); + BaseStream.RtsEnable = !BaseStream.RtsEnable; + } + catch + { + } + } + + // SLCAN check + if (Regex.IsMatch(plaintxtline, @"\rT[0-9A-Z]{9,32}$", RegexOptions.Multiline)) + { + PRsender.doWorkArgs.ErrorMessage = + "This appears to be a CAN port, please use the DroneCAN screen"; + throw new Exception(@"This appears to be a CAN port, please use the DroneCAN screen"); + } + + // check we have hb's + if (hbhistory.Count > 0) + { + bool exit = false; + // get most seen hbs + var mostseenlist = hbhistory.GroupBy(s => MAVList.GetID(s.sysid, s.compid)) + .OrderByDescending(s => s.Count()).Where(s => s.Count() >= 2); + foreach (var mostseen in mostseenlist) + { + // get count on most seen + var seentimes = mostseen.Count(); + // get the most seen mavlinkmessage + var msg = mostseen.First(); + + // preference compid of 1, failover to anything that we have seen 4 times + if (seentimes >= 2 && msg.compid == 1 || seentimes >= 4) + { + if (msg.msgid == (ulong)MAVLINK_MSG_ID.HEARTBEAT) + SetupMavConnect(msg, (mavlink_heartbeat_t) msg.data); + if (msg.msgid == (ulong)MAVLINK_MSG_ID.HIGH_LATENCY2) + SetupMavConnect(msg, (mavlink_high_latency2_t)msg.data); + sysidcurrent = msg.sysid; + compidcurrent = msg.compid; + log.Info($"HB Selection {sysidcurrent}-{compidcurrent} seen {seentimes}"); + exit = true; + break; + } + } + + if (exit) + break; + } + } + + countDown.Stop(); + + char[] temp = ("Mission Planner " + getAppVersion() + "\0").ToCharArray(); + Array.Resize(ref temp, 50); + + // + generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT, + new mavlink_statustext_t() {severity = (byte) MAV_SEVERITY.INFO, text = temp.ToByteArray()}); + + // mavlink2 + generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT, + new mavlink_statustext_t() {severity = (byte) MAV_SEVERITY.INFO, text = temp.ToByteArray()}, + sysidcurrent, + compidcurrent, true, false); + + // mavlink2 - signed + generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT, + new mavlink_statustext_t() {severity = (byte) MAV_SEVERITY.INFO, text = temp.ToByteArray()}, + sysidcurrent, + compidcurrent, true, true); + + // this ensures a mavlink2 change has been noticed + getHeartBeat(); + + getVersion((byte) sysidcurrent, (byte) compidcurrent); + + doCommand((byte) sysidcurrent, (byte) compidcurrent, MAV_CMD.DO_SEND_BANNER, 0, 0, 0, 0, 0, 0, 0, + false); + + if (getparams) + { + frmProgressReporter.UpdateProgressAndStatus(0, + "Getting Params.. (sysid " + MAV.sysid + " compid " + MAV.compid + ") "); + + getParamListMavftp(MAV.sysid, MAV.compid); + } + + if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true) + { + giveComport = false; + if (BaseStream.IsOpen) + BaseStream.Close(); + return; + } + } + catch (Exception e) + { + try + { + BaseStream.Close(); + } + catch + { + } + + giveComport = false; + if (string.IsNullOrEmpty(PRsender.doWorkArgs.ErrorMessage)) + PRsender.doWorkArgs.ErrorMessage = Strings.ConnectFailed; + log.Error(e); + throw; + } + + //frmProgressReporter.Close(); + giveComport = false; + frmProgressReporter.UpdateProgressAndStatus(100, Strings.Done); + log.Info("Done open " + MAV.sysid + " " + MAV.compid); + MAV.packetslost = 0; + MAV.synclost = 0; + _openComplete = true; + } + + private string getAppVersion() + { + try + { + Assembly entryAssembly = Assembly.GetEntryAssembly(); + if (entryAssembly != null) + { + object[] customAttributes = + entryAssembly.GetCustomAttributes(typeof(AssemblyFileVersionAttribute), false); + if (customAttributes != null && customAttributes.Length != 0) + { + return ((AssemblyFileVersionAttribute) customAttributes[0]).Version; + } + } + } + catch + { + } + + return "0.0"; + } + + private uint request_id = 0; + + /// + /// devop read spi mpu6000 0 0 0xf5 1 + ///STABILIZE> Operation 1 OK: 1 bytes + ///0 1 2 3 4 5 6 7 8 9 a b c d e f + ///f0: -- -- -- -- -- 71 -- -- -- -- -- -- -- -- -- -- + /// + /// spi/i2c + /// spi + /// i2c + /// i2c + /// + /// + /// + public byte device_op(byte sysid, byte compid, out byte[] outputbuffer, MAVLink.DEVICE_OP_BUSTYPE bustype, + string name, byte bus, + byte address, byte regstart, byte count, byte[] writebytes = null) + { + var responce = false; + var buffer = new byte[0]; + byte result = 0; + + var sub = SubscribeToPacketType(MAVLINK_MSG_ID.DEVICE_OP_READ_REPLY, (m) => + { + var mtype = (MAVLINK_MSG_ID)m.msgid; + if (mtype == MAVLINK_MSG_ID.DEVICE_OP_READ_REPLY) + { + var msg = (mavlink_device_op_read_reply_t)m.data; + if (msg.result != 0) + log.InfoFormat(name + " Operation {0} failed: {1}", msg.request_id, msg.result); + else + log.InfoFormat(name + " Operation {0} OK: {1} bytes", msg.request_id, msg.count); + for (var i = 0; i < msg.count; i++) + { + var reg = i + msg.regstart; + log.InfoFormat("{0,2:X}:{1,2:X} ", reg, msg.data[i]); + if ((i + 1) % 16 == 0) + Console.WriteLine(); + if (msg.count % 16 != 0) + Console.WriteLine(); + } + + buffer = msg.data.Take(msg.count).ToArray(); + responce = true; + result = msg.result; + } + + return true; + }, sysid, compid); + + var sub2 = SubscribeToPacketType(MAVLINK_MSG_ID.DEVICE_OP_WRITE_REPLY, (m) => + { + var mtype = (MAVLINK_MSG_ID) m.msgid; + if (mtype == MAVLINK_MSG_ID.DEVICE_OP_WRITE_REPLY) + { + var msg = (mavlink_device_op_write_reply_t) m.data; + if (msg.result != 0) + log.InfoFormat(name + " Operation {0} failed: {1}", msg.request_id, msg.result); + else + log.InfoFormat(name + " Operation {0} OK", msg.request_id); + + responce = true; + result = msg.result; + } + + return true; + }, sysid, compid); + + if (writebytes != null) + { + var write = new mavlink_device_op_write_t() + { + target_system = (byte) sysid, + target_component = (byte) compid, + request_id = request_id++, + bustype = (byte) bustype, + bus = bus, + busname = name.MakeBytesSize(40), + address = address, + regstart = regstart, + count = (byte) writebytes.Length, + data = writebytes.MakeSize(128) + }; + + generatePacket(MAVLINK_MSG_ID.DEVICE_OP_WRITE, write); + } + else + { + var read = new MAVLink.mavlink_device_op_read_t() + { + target_system = (byte) sysid, + target_component = (byte) compid, + request_id = request_id++, + bustype = (byte) bustype, + bus = bus, + busname = name.MakeBytesSize(40), + address = address, + regstart = regstart, + count = count + }; + generatePacket(MAVLINK_MSG_ID.DEVICE_OP_READ, read); + } + + log.InfoFormat("bustype {0} name {1} bus {2} address {3}", bustype, name, bus, address); + var start = DateTime.Now; + while (!responce && start.AddSeconds(1) > DateTime.Now) + { + Thread.Sleep(1); + } + + UnSubscribeToPacketType(sub); + UnSubscribeToPacketType(sub2); + + outputbuffer = buffer; + return result; + } + + private void SetupMavConnect(MAVLinkMessage message, mavlink_heartbeat_t hb) + { + mavlinkversion = hb.mavlink_version; + MAVlist[message.sysid, message.compid].aptype = (MAV_TYPE) hb.type; + MAVlist[message.sysid, message.compid].apname = (MAV_AUTOPILOT) hb.autopilot; + + setAPType(message.sysid, message.compid); + + MAVlist[message.sysid, message.compid].sysid = message.sysid; + MAVlist[message.sysid, message.compid].compid = message.compid; + MAVlist[message.sysid, message.compid].recvpacketcount = message.seq; + log.InfoFormat("ID sys {0} comp {1} ver{2} type {3} name {4}", message.sysid, message.compid, + mavlinkversion, + MAVlist[message.sysid, message.compid].aptype.ToString(), MAVlist[message.sysid, message.compid].apname.ToString()); + } + + private void SetupMavConnect(MAVLinkMessage message, mavlink_high_latency2_t hl) + { + mavlinkversion = 0; + MAVlist[message.sysid, message.compid].aptype = (MAV_TYPE)hl.type; + MAVlist[message.sysid, message.compid].apname = (MAV_AUTOPILOT)hl.autopilot; + MAVlist[message.sysid, message.compid].isHighLatency = true; + + setAPType(message.sysid, message.compid); + + MAVlist[message.sysid, message.compid].sysid = message.sysid; + MAVlist[message.sysid, message.compid].compid = message.compid; + MAVlist[message.sysid, message.compid].recvpacketcount = message.seq; + log.InfoFormat("ID HL sys {0} comp {1} ver{2} type {3} name {4}", message.sysid, message.compid, + mavlinkversion, + MAVlist[message.sysid, message.compid].aptype.ToString(), MAVlist[message.sysid, message.compid].apname.ToString()); + } + + public MAVLinkMessage getHeartBeat() + { + return getHeartBeatAsync().AwaitSync(); + } + + public async Task getHeartBeatAsync() + { + giveComport = true; + + sendPacket(new MAVLink.mavlink_heartbeat_t() + { + type = (byte) MAVLink.MAV_TYPE.GCS, + autopilot = (byte) MAVLink.MAV_AUTOPILOT.INVALID, + mavlink_version = 3 // MAVLink.MAVLINK_VERSION + }, gcssysid, 190); + + DateTime start = DateTime.Now; + int readcount = 0; + while (true) + { + MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); + readcount++; + if (buffer.Length > 5) + { + //log.Info("getHB packet received: " + buffer.Length + " btr " + BaseStream.BytesToRead + " type " + buffer.msgid ); + if (buffer.msgid == (byte) MAVLINK_MSG_ID.HEARTBEAT) + { + mavlink_heartbeat_t hb = buffer.ToStructure(); + + if (hb.type != (byte) MAV_TYPE.GCS) + { + SetupMavConnect(buffer, hb); + + giveComport = false; + return buffer; + } + } + if (buffer.msgid == (byte)MAVLINK_MSG_ID.HIGH_LATENCY2) + { + mavlink_high_latency2_t hl = buffer.ToStructure(); + + if (hl.type != (byte)MAV_TYPE.GCS) + { + SetupMavConnect(buffer, hl); + + giveComport = false; + return buffer; + } + } + } + + if (DateTime.Now > start.AddMilliseconds(2200) || readcount > 200) // was 1200 , now 2.2 sec + { + giveComport = false; + return MAVLinkMessage.Invalid; + } + } + } + + /// + /// Send a mavlink packet + /// + /// data + /// target sysid + /// target compid + public void sendPacket(object indata, int sysid, int compid) + { + bool validPacket = false; + + if (BaseStream == null || !BaseStream.IsOpen) + return; + + foreach (var ty in MAVLINK_MESSAGE_INFOS) + { + if (ty.type == indata.GetType()) + { + validPacket = true; + generatePacket((int) ty.msgid, indata, sysid, compid); + return; + } + } + + if (!validPacket) + { + log.Info("Mavlink : NOT VALID PACKET sendPacket() " + indata.GetType().ToString()); + } + } + + private void generatePacket(MAVLINK_MSG_ID messageType, object indata) + { + generatePacket((int) messageType, indata); + } + + private void generatePacket(int messageType, object indata) + { + //uses currently targeted mavs sysid and compid + generatePacket(messageType, indata, MAV.sysid, MAV.compid); + } + + public void generatePacket(MAVLINK_MSG_ID messageType, object indata, int sysid, int compid, + bool forcemavlink2 = false, + bool forcesigning = false) + { + generatePacket((int) messageType, indata, sysid, compid, forcemavlink2, forcesigning); + } + + /// + /// Generate a Mavlink Packet and write to serial + /// + /// type number = MAVLINK_MSG_ID + /// struct of data + public void generatePacket(int messageType, object indata, int sysid, int compid, bool forcemavlink2 = false, + bool forcesigning = false) + { + if (BaseStream == null || !BaseStream.IsOpen) + { + return; + } + + if (ReadOnly) + { + // allow these messages + if (messageType == (byte) MAVLINK_MSG_ID.MISSION_REQUEST_LIST || + messageType == (byte) MAVLINK_MSG_ID.MISSION_REQUEST_PARTIAL_LIST || + messageType == (byte) MAVLINK_MSG_ID.MISSION_REQUEST || + messageType == (byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT || + messageType == (byte) MAVLINK_MSG_ID.PARAM_REQUEST_LIST || + messageType == (byte) MAVLINK_MSG_ID.PARAM_REQUEST_READ || + messageType == (byte) MAVLINK_MSG_ID.RALLY_FETCH_POINT || + messageType == (byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT + ) + { + } + else + { + return; + } + } + + byte[] packet = new byte[0]; + lock (writelock) + { + byte[] data = MavlinkUtil.StructureToByteArray(indata); + + int i = 0; + + // are we mavlink2 enabled for this sysid/compid + if (!MAVlist[sysid, compid].mavlinkv2 && messageType < 256 && !forcemavlink2) + { + var info = MAVLINK_MESSAGE_INFOS.SingleOrDefault(p => p.msgid == messageType); + if (data.Length != info.minlength) + { + Array.Resize(ref data, (int) info.minlength); + } + + //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); + packet = new byte[data.Length + 6 + 2]; + + packet[0] = MAVLINK_STX_MAVLINK1; + packet[1] = (byte) data.Length; + packet[2] = (byte) packetcount; + + packetcount++; + + packet[3] = gcssysid; + packet[4] = (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; + packet[5] = (byte) messageType; + + i = 6; + foreach (byte b in data) + { + packet[i] = b; + i++; + } + + ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6); + + checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint) messageType).crc, + checksum); + + byte ck_a = (byte) (checksum & 0xFF); ///< High byte + byte ck_b = (byte) (checksum >> 8); ///< Low byte + + packet[i] = ck_a; + i += 1; + packet[i] = ck_b; + i += 1; + } + else + { + // trim packet for mavlink2 + MavlinkUtil.trim_payload(ref data); + + // space for signing if needed + if (MAVlist[sysid, compid].signing || forcesigning) + packet = new byte[data.Length + MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES + + MAVLINK_SIGNATURE_BLOCK_LEN]; + else + packet = new byte[data.Length + MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES]; + + packet[0] = MAVLINK_STX; + packet[1] = (byte) data.Length; + packet[2] = 0; // incompat + if (MAVlist[sysid, compid].signing || forcesigning) // current mav + packet[2] |= MAVLINK_IFLAG_SIGNED; + packet[3] = 0; // compat + packet[4] = (byte) packetcount; + + packetcount++; + + packet[5] = gcssysid; + packet[6] = (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; + packet[7] = (byte) (messageType & 0xff); + packet[8] = (byte) ((messageType >> 8) & 0xff); + packet[9] = (byte) ((messageType >> 16) & 0xff); + + i = 10; + foreach (byte b in data) + { + packet[i] = b; + i++; + } + + ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + MAVLINK_NUM_HEADER_BYTES); + + checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint) messageType).crc, + checksum); + + byte ck_a = (byte) (checksum & 0xFF); ///< High byte + byte ck_b = (byte) (checksum >> 8); ///< Low byte + + packet[i] = ck_a; + i += 1; + packet[i] = ck_b; + i += 1; + + if (MAVlist[sysid, compid].signing || forcesigning) + { + //https://docs.google.com/document/d/1ETle6qQRcaNWAmpG2wz0oOpFKSF_bcTmYMQvtTGI8ns/edit + + /* + 8 bits of link ID + 48 bits of timestamp + 48 bits of signature + */ + + // signature = sha256_48(secret_key + header + payload + CRC + link-ID + timestamp) + + var timestamp = (UInt64) ((DateTime.UtcNow - new DateTime(2015, 1, 1)).TotalMilliseconds * 100); + + if (timestamp == MAVlist[sysid, compid].timestamp) + timestamp++; + + MAVlist[sysid, compid].timestamp = timestamp; + + var timebytes = BitConverter.GetBytes(timestamp); + + var sig = new byte[7]; // 13 includes the outgoing hash + sig[0] = MAVlist[sysid, compid].sendlinkid; + Array.Copy(timebytes, 0, sig, 1, 6); // timestamp + + //Console.WriteLine("gen linkid {0}, time {1} {2} {3} {4} {5} {6} {7}", sig[0], sig[1], sig[2], sig[3], sig[4], sig[5], sig[6], timestamp); + + var signingKey = MAVlist[sysid, compid].signingKey; + + if (signingKey == null || signingKey.Length != 32) + { + signingKey = new byte[32]; + } + + using (SHA256CryptoServiceProvider signit = new SHA256CryptoServiceProvider()) + { + signit.TransformBlock(signingKey, 0, signingKey.Length, null, 0); + signit.TransformBlock(packet, 0, i, null, 0); + signit.TransformFinalBlock(sig, 0, sig.Length); + var ctx = signit.Hash; + // trim to 48 + Array.Resize(ref ctx, 6); + + foreach (byte b in sig) + { + packet[i] = b; + i++; + } + + foreach (byte b in ctx) + { + packet[i] = b; + i++; + } + } + } + } + + if (BaseStream.IsOpen) + { + BaseStream.Write(packet, 0, i); + _bytesSentSubj.OnNext(i); + } + + try + { + SaveToTlog(new Span(packet, 0, i)); + } + catch + { + } + } + try + { + // callback outside of lock + if(packet.Length > 0) + _OnPacketSent?.Invoke(this, new MAVLinkMessage(packet)); + } + catch + { + } + } + + public void SaveToTlog(Span packet) + { + try + { + if (logfile != null && logfile.CanWrite && !logreadmode) + { + lock (logfile) + { + byte[] datearray = + BitConverter.GetBytes( + (UInt64) ((DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds * 1000)); + Array.Reverse(datearray); + logfile.Write(datearray, 0, datearray.Length); + logfile.Write(packet.ToArray(), 0, packet.Length); + } + } + } + catch + { + } + } + + public bool Write(byte[] packet) + { + lock (writelock) + { + if (BaseStream.IsOpen) + { + BaseStream.Write(packet, 0, packet.Length); + _bytesSentSubj.OnNext(packet.Length); + } + + try + { + SaveToTlog(new Span(packet, 0, packet.Length)); + } + catch + { + } + } + try + { + // callback outside of lock + _OnPacketSent?.Invoke(this, new MAVLinkMessage(packet)); + } + catch + { + } + return true; + } + + public bool Write(string line) + { + lock (writelock) + { + BaseStream.Write(line); + } + + _bytesSentSubj.OnNext(line.Length); + return true; + } + + public bool setupSigning(byte sysid, byte compid, string userseed, byte[] key = null) + { + byte[] shauser; + bool clearkey = false; + + if (key == null) + { + clearkey = String.IsNullOrEmpty(userseed); + + // sha the user input string + using (SHA256CryptoServiceProvider signit = new SHA256CryptoServiceProvider()) + { + shauser = signit.ComputeHash(Encoding.UTF8.GetBytes(userseed)); + Array.Resize(ref shauser, 32); + } + } + else + { + shauser = key; + Array.Resize(ref shauser, 32); + } + + mavlink_setup_signing_t sign = new mavlink_setup_signing_t(); + if (!clearkey) + { + sign.initial_timestamp = + (UInt64) ((DateTime.UtcNow - new DateTime(2015, 1, 1)).TotalMilliseconds * 100); + sign.secret_key = shauser; + } + else + { + sign.initial_timestamp = 0; + sign.secret_key = new byte[32]; + } + + sign.target_component = (byte) compid; + sign.target_system = (byte) sysid; + + generatePacket((int) MAVLINK_MSG_ID.SETUP_SIGNING, sign, sysid, compid); + + generatePacket((int) MAVLINK_MSG_ID.SETUP_SIGNING, sign, sysid, compid); + + if (!clearkey) + { + // we will auto adapt to this key - so no need to assign it + //MAVlist[sysid, compid].signingKey = shauser; + } + else + { + // clear the key + MAVlist[sysid, compid].signingKey = null; + return disableSigning(sysid, compid); + } + + return enableSigning(sysid, compid); + } + + public bool enableSigning(int sysid, int compid) + { + MAVlist[sysid, compid].signing = true; + MAVlist[sysid, compid].mavlinkv2 = true; + + return MAVlist[sysid, compid].signing; + } + + public bool disableSigning(int sysid, int compid) + { + MAVlist[sysid, compid].signing = false; + MAVlist[sysid, compid].mavlinkv2 = false; + + return MAVlist[sysid, compid].signing; + } + + [Obsolete] + /// + /// set param on apm, used for param rename + /// + /// + /// + /// + public bool setParam(string[] paramnames, double value) + { + foreach (string paramname in paramnames) + { + if (setParam(paramname, value)) + { + return true; + } + } + + return false; + } + + [Obsolete] + public bool setParam(string paramname, double value, bool force = false) + { + return setParam((byte) sysidcurrent, (byte) compidcurrent, paramname, value, force); + } + + public bool setParam(byte sysid, byte compid, string paramname, double value, bool force = false) + { + return setParamAsync(sysid, compid, paramname, value, force).AwaitSync(); + } + + /// + /// Set parameter on apm + /// + /// name as a string + /// + public async Task setParamAsync(byte sysid, byte compid, string paramname, double value, + bool force = false) + { + if (!MAVlist[sysid, compid].param.ContainsKey(paramname)) + { + log.Warn("Trying to set Param that doesnt exist " + paramname + "=" + value); + return false; + } + + MAVLinkParam param = MAVlist[sysid, compid].param[paramname]; + if (param.Value == value && !force) + { + log.Warn("setParam " + paramname + " not modified as same"); + return true; + } + + giveComport = true; + + // param type is set here, however it is always sent over the air as a float 100int = 100f. + var req = new mavlink_param_set_t + { + target_system = sysid, + target_component = compid, + param_type = (byte) MAVlist[sysid, compid].param_types[paramname] + }; + + char[] temp = paramname.ToCharArray(); + + Array.Resize(ref temp, 16); + req.param_id = temp.ToByteArray(); + if ((MAVlist[sysid, compid].cs.capabilities & (uint) MAV_PROTOCOL_CAPABILITY.PARAM_FLOAT) > 0 || + MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA) + { + req.param_value = new MAVLinkParam(paramname, value, (MAV_PARAM_TYPE.REAL32)).float_value; + } + else + { + req.param_value = new MAVLinkParam(paramname, value, + (MAV_PARAM_TYPE) MAVlist[sysid, compid].param_types[paramname]).float_value; + } + + int currentparamcount = MAVlist[sysid, compid].param.Count; + + bool complete = false; + + var sub1 = SubscribeToPacketType(MAVLINK_MSG_ID.PARAM_VALUE, buffer => + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + mavlink_param_value_t par = buffer.ToStructure(); + + string st = Encoding.UTF8.GetString(par.param_id); + + int pos = st.IndexOf('\0'); + + if (pos != -1) + { + st = st.Substring(0, pos); + } + + if (st != paramname) + { + log.InfoFormat("MAVLINK bad param response - {0} vs {1}", paramname, st); + return true; + } + + if (MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA) + { + var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); + MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, BitConverter.GetBytes(par.param_value), + MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE) par.param_type, param.default_value); + } + else + { + var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); + MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, BitConverter.GetBytes(par.param_value), + (MAV_PARAM_TYPE) par.param_type, (MAV_PARAM_TYPE) par.param_type); + } + + log.Info("setParam gotback " + st + " : " + MAVlist[sysid, compid].param[st]); + + lastparamset = DateTime.UtcNow; + + // check if enabeling this param has added subparams, queue on gui thread + if (currentparamcount < par.param_count) + { + /* + MainV2.instance.BeginInvoke((Action) delegate + { + Loading.ShowLoading(String.Format(Strings.ParamRefreshRequired, currentparamcount, + par.param_count)); + }); + */ + } + + complete = true; + } + + return true; + }, sysid, compid); + + try + { + generatePacket((byte) MAVLINK_MSG_ID.PARAM_SET, req, sysid, compid); + + log.InfoFormat("setParam '{0}' = '{1}' sysid {2} compid {3}", paramname, value, sysid, + compid); + + DateTime start = DateTime.Now; + int retrys = 3; + while (true) + { + if (complete) + return true; + + if (!(start.AddMilliseconds(700) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("setParam Retry " + retrys); + generatePacket((byte) MAVLINK_MSG_ID.PARAM_SET, req, sysid, compid); + start = DateTime.Now; + retrys--; + continue; + } + + throw new TimeoutException("Timeout on read - setParam " + paramname); + } + + await readPacketAsync().ConfigureAwait(false); + } + } + finally + { + giveComport = false; + UnSubscribeToPacketType(sub1); + } + } + + /// + /// With GUI + /// + public void getParamList() + { + log.InfoFormat("getParamList {0} {1}", sysidcurrent, compidcurrent); + + frmProgressReporter = CreateIProgressReporterDialogue(Strings.GettingParams + " " + sysidcurrent); + + frmProgressReporter.DoWork += FrmProgressReporterGetParams; + frmProgressReporter.UpdateProgressAndStatus(-1, Strings.GettingParamsD); + + frmProgressReporter.RunBackgroundOperationAsync(); + + frmProgressReporter.Dispose(); + + _ParamListChanged?.Invoke(this, null); + } + + private void FrmProgressReporterGetParams(IProgressReporterDialogue sender) + { + getParamListMavftp(MAV.sysid, MAV.compid); + } + + /// + /// Get param list via mavftp with old style fallback + /// + /// + /// + /// MAVLinkParamList + public MAVLinkParamList getParamListMavftp(byte sysid, byte compid) + { + return getParamListMavftpAsync(sysid, compid).AwaitSync(); + } + + public async Task getParamListMavftpAsync(byte sysid, byte compid) + { + var sub2 = SubscribeToPacketType(MAVLINK_MSG_ID.STATUSTEXT, buffer => + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.STATUSTEXT) + { + var msg = buffer.ToStructure(); + + string logdata = Encoding.UTF8.GetString(msg.text); + + int ind = logdata.IndexOf('\0'); + if (ind != -1) + logdata = logdata.Substring(0, ind); + + if (logdata.ToLower().Contains("copter") || logdata.ToLower().Contains("rover") || + logdata.ToLower().Contains("plane")) + { + MAVlist[sysid, compid].VersionString = logdata; + // Version string may allow to better identify type + setAPType(sysid, compid); + } + else if (logdata.ToLower().Contains("nuttx") || logdata.ToLower().Contains("chibios")) + { + MAVlist[sysid, compid].SoftwareVersions = logdata; + } + else if (logdata.ToLower().Contains("px4v2") || + (Regex.IsMatch(logdata, @"\s[0-9A-F]+\s[0-9A-F]+\s[0-9A-F]+") && !logdata.Contains("IOMCU"))) + { + MAVlist[sysid, compid].SerialString = logdata; + } + else if (logdata.ToLower().Contains("frame")) + { + MAVlist[sysid, compid].FrameString = logdata; + } + else + { + } + } + + return true; + }, (byte)sysid, (byte)compid); + + // get the banner + doCommand((byte) sysid, (byte) compid, MAV_CMD.DO_SEND_BANNER, 0, 0, 0, 0, 0, 0, 0, + false); + + try + { + if (((MAVlist[sysid,compid].cs.capabilities & (int) MAV_PROTOCOL_CAPABILITY.FTP) > 0) && Settings.Instance.GetBoolean("UseMavFtpParams", true)) + { + if (frmProgressReporter != null) + frmProgressReporter.UpdateProgressAndStatus(-1, $"Checking for Param MAVFTP {sysid}-{compid}"); + var cancel = new CancellationTokenSource(); + var paramfileTask = Task.Run(() => + { + var ftp = new MAVFtp(this, sysid, compid); + ftp.Progress += (s,i) => + { + if (frmProgressReporter != null) + frmProgressReporter.UpdateProgressAndStatus(i, + $"Getting Param MAVFTP {sysid}-{compid} : {s}"); + }; + return ftp.GetFile( + // always try to get defualts, AP will send orginal format if not avalable + "@PARAM/param.pck?withdefaults=1", cancel, true, 110); + }); + while (!paramfileTask.IsCompleted) + { + if (frmProgressReporter != null) + if (frmProgressReporter.doWorkArgs.CancelRequested) + { + cancel.Cancel(); + frmProgressReporter.doWorkArgs.CancelAcknowledged = true; + return MAVlist[sysid, compid].param; + } + + await Task.Delay(10).ConfigureAwait(false); + } + + if (frmProgressReporter != null) + if (frmProgressReporter.doWorkArgs.CancelRequested) + return MAVlist[sysid, compid].param; + + var paramfile = await paramfileTask; + if (paramfile != null && paramfile.Length > 0) + { + var mavlist = parampck.unpack(paramfile.ToArray()); + if (mavlist != null) + { + MAVlist[sysid, compid].param.Clear(); + MAVlist[sysid, compid].param.TotalReported = + mavlist.Count; + MAVlist[sysid, compid].param.AddRange(mavlist); + var gen = new MavlinkParse(); + mavlist.ForEach(a => + { + MAVlist[sysid, compid].param_types[a.Name] = + a.Type; + SaveToTlog(gen.GenerateMAVLinkPacket10( + MAVLINK_MSG_ID.PARAM_VALUE, + new mavlink_param_value_t((float) a.Value, (ushort) mavlist.Count, + 0, + a.Name.MakeBytesSize(16), (byte) a.Type), sysid, compid)); + }); + + UnSubscribeToPacketType(sub2); + return MAVlist[sysid, compid].param; + } + } + } + } + catch (Exception e) + { + log.Error(e); + } + + UnSubscribeToPacketType(sub2); + return await getParamListAsync(sysid, compid); + } + + /// + /// Without GUI + /// + /// + /// + /// + public MAVLinkParamList getParamList(byte sysid, byte compid) + { + return getParamListAsync(sysid, compid).AwaitSync(); + } + + /// + /// Get param list from apm + /// + /// + public async Task getParamListAsync(byte sysid, byte compid) + { + List indexsreceived = new List(); + + // create new list so if canceled we use the old list + MAVLinkParamList newparamlist = new MAVLinkParamList(); + + int param_total = 1; + + mavlink_param_request_list_t req = new mavlink_param_request_list_t + { + target_system = sysid, + target_component = compid + }; + + DateTime start = DateTime.Now; + + DateTime lastmessage = DateTime.MinValue; + + //hires.Stopwatch stopwatch = new hires.Stopwatch(); + int retry = 0; + bool onebyone = false; + bool missing_params = false; + DateTime lastonebyone = DateTime.MinValue; + + var sub2 = SubscribeToPacketType(MAVLINK_MSG_ID.STATUSTEXT, buffer => + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.STATUSTEXT) + { + var msg = buffer.ToStructure(); + + string logdata = Encoding.UTF8.GetString(msg.text); + + int ind = logdata.IndexOf('\0'); + if (ind != -1) + logdata = logdata.Substring(0, ind); + + if (logdata.ToLower().Contains("copter") || logdata.ToLower().Contains("rover") || + logdata.ToLower().Contains("plane")) + { + MAVlist[sysid, compid].VersionString = logdata; + } + else if (logdata.ToLower().Contains("nuttx") || logdata.ToLower().Contains("chibios")) + { + MAVlist[sysid, compid].SoftwareVersions = logdata; + } + else if (logdata.ToLower().Contains("px4v2") || + (Regex.IsMatch(logdata, @"\s[0-9A-F]+\s[0-9A-F]+\s[0-9A-F]+") && !logdata.Contains("IOMCU"))) + { + MAVlist[sysid, compid].SerialString = logdata; + } + else if (logdata.ToLower().Contains("frame")) + { + MAVlist[sysid, compid].FrameString = logdata; + } + else + { + } + } + + return true; + }, (byte)sysid, (byte)compid); + + int seenvalid = 0; + + var sub1 = SubscribeToPacketType(MAVLINK_MSG_ID.PARAM_VALUE, buffer => + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + // if we are doing one by one dont update start time + if (!onebyone) + start = DateTime.Now; + + mavlink_param_value_t par = buffer.ToStructure(); + + // set new target + param_total = (par.param_count); + newparamlist.TotalReported = param_total; + + if (param_total == 0) + return true; + + string paramID = Encoding.UTF8.GetString(par.param_id); + + int pos = paramID.IndexOf('\0'); + if (pos != -1) + { + paramID = paramID.Substring(0, pos); + } + + // check if we already have it + if (indexsreceived.Contains(par.param_index)) + { + log.Info("Already got " + (par.param_index) + " '" + paramID + "' " + + (indexsreceived.Count * 100) / param_total); + if (frmProgressReporter != null) + this.frmProgressReporter.UpdateProgressAndStatus( + (indexsreceived.Count * 100) / param_total, "Already Got param " + paramID); + return true; + } + + seenvalid++; + //Console.WriteLine(DateTime.Now.Millisecond + " gp2 "); + + log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + + (par.param_count) + " name: " + paramID + " " + par.param_value); + + //Console.WriteLine(DateTime.Now.Millisecond + " gp2a "); + + // item uses float based param system + if ((MAVlist[sysid, compid].cs.capabilities & (uint) MAV_PROTOCOL_CAPABILITY.PARAM_FLOAT) > 0 || + MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA) + { + var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); + newparamlist[paramID] = new MAVLinkParam(paramID, BitConverter.GetBytes(par.param_value), + MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE) par.param_type); + } + else + { + var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); + newparamlist[paramID] = new MAVLinkParam(paramID, BitConverter.GetBytes(par.param_value), + (MAV_PARAM_TYPE) par.param_type, (MAV_PARAM_TYPE) par.param_type); + } + + //Console.WriteLine(DateTime.Now.Millisecond + " gp2b "); + + // exclude index of 65535 + if (par.param_index != 65535) + indexsreceived.Add(par.param_index); + + lock(MAVlist[sysid, compid].param_types) + MAVlist[sysid, compid].param_types[paramID] = (MAV_PARAM_TYPE) par.param_type; + + //Console.WriteLine(DateTime.Now.Millisecond + " gp3 "); + + if (frmProgressReporter != null) + this.frmProgressReporter.UpdateProgressAndStatus((indexsreceived.Count * 100) / param_total, + Strings.Gotparam + paramID); + + // we hit the last param, jump striaght to retry if params missing + if (par.param_index == (param_total - 1)) + missing_params |= indexsreceived.Count < param_total; + } + + return true; + }, (byte)sysid, (byte)compid); + + + generatePacket((byte)MAVLINK_MSG_ID.PARAM_REQUEST_LIST, req); + + short tenbytenindex = 0; + + do + { + if (frmProgressReporter != null && frmProgressReporter.doWorkArgs.CancelRequested) + { + frmProgressReporter.doWorkArgs.CancelAcknowledged = true; + + UnSubscribeToPacketType(sub1); + UnSubscribeToPacketType(sub2); + frmProgressReporter.doWorkArgs.ErrorMessage = "User Canceled"; + return MAVlist[sysid, compid].param; + } + + // 4 seconds between valid packets or got to last param and have some missing + if ((missing_params || !(start.AddMilliseconds(4000) > DateTime.Now)) && !logreadmode) + { + // if we have less than 75% of the total use full list pull + if (!onebyone && (retry < 2) && ((param_total <= 1) || (indexsreceived.Count < ((param_total / 4) * 3)))) + { + if (param_total > 1) + { + // only increment retry counter if we have a valid total + // otherwise keep requesting full list + retry++; + } + log.InfoFormat("Get Param whole list retry {0} got {1} 75%={2} count {3}", retry, + indexsreceived.Count, ((param_total / 4) * 3), param_total); + generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_LIST, req); + start = DateTime.Now; + missing_params = false; + continue; + } + + onebyone = true; + + if (lastonebyone.AddMilliseconds(1000) < DateTime.Now) + { + int queued = 0; + short startindex = 0; + var backupseenvalid = seenvalid; + if (param_total > 1) + startindex = (short) (tenbytenindex % (param_total - 1)); + + seenvalid = 0; + + // seeing more than what we requested... skip this round - maybe full param list is still inflight + if(backupseenvalid > 10) + continue; + + log.Info("Get param 10 by 10 - got " + indexsreceived.Count + " of " + param_total + + " request from startindex " + startindex + " seen since last requests " + backupseenvalid); + + // try getting individual params + bool found_missing_param = false; + for (short i = startindex; i <= (param_total - 1); i++) + { + if (!indexsreceived.Contains(i)) + { + found_missing_param = true; + + if (frmProgressReporter != null && frmProgressReporter.doWorkArgs.CancelRequested) + { + frmProgressReporter.doWorkArgs.CancelAcknowledged = true; + UnSubscribeToPacketType(sub1); + UnSubscribeToPacketType(sub2); + frmProgressReporter.doWorkArgs.ErrorMessage = "User Canceled"; + return MAVlist[sysid, compid].param; + } + + // prevent dropping out of this get params loop + try + { + queued++; + tenbytenindex = (short)(i + 1); + + mavlink_param_request_read_t req2 = new mavlink_param_request_read_t + { + target_system = sysid, + target_component = compid, + param_index = i, + param_id = new byte[] {0} + }; + + Array.Resize(ref req2.param_id, 16); + + generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_READ, req2); + + if (queued >= 10) + { + break; + } + } + catch (Exception excp) + { + log.Info("GetParam Failed index: " + i + " " + excp); + UnSubscribeToPacketType(sub1); + UnSubscribeToPacketType(sub2); + throw excp; + } + } + } + if (!found_missing_param) + { + // no missing param after start index, start from the begining + tenbytenindex = 0; + } + + lastonebyone = DateTime.Now; + } + } + + await readPacketAsync(); + + if (logreadmode && logplaybackfile.BaseStream.Position >= logplaybackfile.BaseStream.Length) + { + break; + } + + if (!logreadmode && !BaseStream.IsOpen) + { + var exp = new Exception("Not Connected"); + frmProgressReporter.doWorkArgs.ErrorMessage = exp.Message; + UnSubscribeToPacketType(sub1); + UnSubscribeToPacketType(sub2); + throw exp; + } + } while (indexsreceived.Count < param_total); + + if (indexsreceived.Count != param_total) + { + var exp = new Exception("Missing Params " + indexsreceived.Count + " vs " + param_total); + frmProgressReporter.doWorkArgs.ErrorMessage = exp.Message; + UnSubscribeToPacketType(sub1); + UnSubscribeToPacketType(sub2); + throw exp; + } + + UnSubscribeToPacketType(sub1); + UnSubscribeToPacketType(sub2); + + log.InfoFormat("list count {0}, rcvd total {1} indexseen count {2}", newparamlist.Count, param_total, + indexsreceived.Count); + + MAVlist[sysid, compid].param.Clear(); + MAVlist[sysid, compid].param.TotalReported = param_total; + MAVlist[sysid, compid].param.AddRange(newparamlist); + return MAVlist[sysid, compid].param; + } + + private int _parampoll = 0; + + public void getParamPoll() + { + // check if we have all + if (MAV.param.TotalReceived >= MAV.param.TotalReported) + { + return; + } + + // if we are connected as primary to a vehicle where we dont have all the params, poll for them + short i = (short) (_parampoll % MAV.param.TotalReported); + + GetParam((byte) sysidcurrent, (byte) compidcurrent, "", i, false); + + _parampoll++; + } + + [Obsolete] + public float GetParam(string name) + { + return GetParam(name, -1); + } + + [Obsolete] + public float GetParam(short index) + { + return GetParam("", index); + } + + [Obsolete] + public float GetParam(string name = "", short index = -1, bool requireresponce = true) + { + return GetParam(MAV.sysid, MAV.compid, name, index, requireresponce); + } + + public float GetParam(byte sysid, byte compid, string name = "", short index = -1, bool requireresponce = true) + { + return GetParamAsync(sysid, compid, name, index, requireresponce).AwaitSync(); + } + + /// + /// Get param by either index or name + /// + /// + /// + /// + public async Task GetParamAsync(byte sysid, byte compid, string name = "", short index = -1, + bool requireresponce = true) + { + if (name == "" && index == -1) + return 0; + + log.Info("GetParam name: '" + name + "' or index: " + index + " " + sysid + ":" + compid); + + MAVLinkMessage buffer; + giveComport = true; + mavlink_param_request_read_t req = new mavlink_param_request_read_t + { + target_system = sysid, + target_component = compid, + param_index = index, + param_id = new byte[] {0} + }; + if (index == -1) + { + req.param_id = name.MakeBytes(); + } + + Array.Resize(ref req.param_id, 16); + + generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_READ, req, sysid, compid); + + if (!requireresponce) + { + giveComport = false; + return 0f; + } + + DateTime start = DateTime.Now; + int retrys = 3; + + while (true) + { + if (!(start.AddMilliseconds(700) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("GetParam Retry " + retrys); + generatePacket((byte) MAVLINK_MSG_ID.PARAM_REQUEST_READ, req, sysid, compid); + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - GetParam"); + } + + buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + mavlink_param_value_t par = buffer.ToStructure(); + + string st = Encoding.UTF8.GetString(par.param_id); + + int pos = st.IndexOf('\0'); + + if (pos != -1) + { + st = st.Substring(0, pos); + } + + // not the correct id + if (!(par.param_index == index || st == name)) + { + log.ErrorFormat("Wrong Answer {0} - {1} - {2} --- '{3}' vs '{4}'", par.param_index, + Encoding.UTF8.GetString(par.param_id), par.param_value, + Encoding.UTF8.GetString(req.param_id).TrimEnd(), st); + continue; + } + + // update table + if (MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA) + { + var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); + MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, + BitConverter.GetBytes(par.param_value), MAV_PARAM_TYPE.REAL32, + (MAV_PARAM_TYPE) par.param_type); + } + else + { + var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); + MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, + BitConverter.GetBytes(par.param_value), (MAV_PARAM_TYPE) par.param_type, + (MAV_PARAM_TYPE) par.param_type); + } + + lock(MAVlist[sysid, compid].param_types) + MAVlist[sysid, compid].param_types[st] = (MAV_PARAM_TYPE) par.param_type; + + log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + + (par.param_count) + " name: " + st); + + giveComport = false; + + return par.param_value; + } + } + } + } + + /// + /// Stops all requested data packets. + /// + public void stopall(bool forget) + { + mavlink_request_data_stream_t req = new mavlink_request_data_stream_t + { + target_system = MAV.sysid, + target_component = MAV.compid, + req_message_rate = 10, + start_stop = 0, + req_stream_id = 0 + }; + + // no error on bad + try + { + generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); + Thread.Sleep(20); + generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); + Thread.Sleep(20); + generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); + log.Info("Stopall Done"); + } + catch + { + } + } + + [Obsolete] + public void setWPACK(MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + setWPACK(MAV.sysid, MAV.compid, type); + } + + public void setWPACK(byte sysid, byte compid, MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + mavlink_mission_ack_t req = new mavlink_mission_ack_t + { + target_system = sysid, + target_component = compid, + type = 0, + mission_type = (byte) type + }; + + generatePacket((byte) MAVLINK_MSG_ID.MISSION_ACK, req); + } + + public bool setWPCurrent(byte sysid, byte compid, ushort index) + { + return setWPCurrentAsync(sysid, compid, index).AwaitSync(); + } + + public async Task setWPCurrentAsync(byte sysid, byte compid, ushort index) + { + giveComport = true; + MAVLinkMessage buffer; + + mavlink_mission_set_current_t req = new mavlink_mission_set_current_t + { + target_system = sysid, + target_component = compid, + seq = index + }; + + generatePacket((byte) MAVLINK_MSG_ID.MISSION_SET_CURRENT, req); + + DateTime start = DateTime.Now; + int retrys = 5; + + while (true) + { + if (!(start.AddMilliseconds(2000) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("setWPCurrent Retry " + retrys); + generatePacket((byte) MAVLINK_MSG_ID.MISSION_SET_CURRENT, req); + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - setWPCurrent"); + } + + buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_CURRENT && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + giveComport = false; + return true; + } + } + } + } + + [Obsolete("Mavlink 09 - use doCommand", true)] + public bool doAction(object actionid) + { + // mavlink 09 + throw new NotImplementedException(); + } + + public bool doDFUBoot(byte sysid, byte compid) + { + doCommand(sysid, compid, MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, + 42, 24, 71, 99, 0, 0, 0, false); + + return true; + } + + /// + /// Control high latency mode + /// + /// true to enable, false to disable + /// + public bool doHighLatency(bool onoff= false) + { + int param1 = onoff ? 1 : 0; + + var ans1 = doCommand((byte)sysidcurrent, (byte)compidcurrent, MAV_CMD.CONTROL_HIGH_LATENCY, + param1, 0, 0, 0, 0, 0, 0); + + return ans1; + } + + /// + /// Control ICE Engine + /// + /// true to enable, false to disable + /// + public bool doEngineControl(byte sysid, byte compid, bool onoff = false) + { + int param1 = onoff ? 1 : 0; + + return doCommand(sysid, compid, MAV_CMD.DO_ENGINE_CONTROL, + param1, 0, 0, 0, 0, 0, 0); + } + + /// + /// reboot the vehicle + /// + /// reboot into bootloader mode? + /// use current sysid/compid or scan for it + /// + public bool doReboot(bool bootloadermode = false, bool currentvehicle = true) + { + int param1 = 1; + if (bootloadermode) + { + param1 = 3; + } + + // reboot the current selected mav + if (currentvehicle) + { + var ans1 = doCommand((byte) sysidcurrent, (byte) compidcurrent, MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, + param1, 0, 0, 0, 0, 0, 0); + var ans2 = false; + if (!ans1) + { + ans2 = doCommand((byte)sysidcurrent, (byte)compidcurrent, MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 0, + 0, 0, 0, 0, 0); + } + // Successful reboot + if (ans1 || ans2) + { + // Not going into bootloader and on a serial connection + if (!bootloadermode && (BaseStream is SerialPort)) + { + // Direct USB will disconnect after a reboot, wait and see if we should re-connect + Thread.Sleep(500); + if (!BaseStream.IsOpen) + { + Open(true); + } + } + return true; + } + + giveComport = false; + return false; + } + else + { + // scan for hb on unknown mav + MAVLinkMessage buffer = getHeartBeat(); + + if (buffer.Length > 5) + { + mavlink_heartbeat_t hb = buffer.ToStructure(); + + mavlinkversion = hb.mavlink_version; + MAV.aptype = (MAV_TYPE) hb.type; + MAV.apname = (MAV_AUTOPILOT) hb.autopilot; + MAV.sysid = buffer.sysid; + MAV.compid = buffer.compid; + } + + // reboot if we have seen hb + if (MAV.sysid != 0 && MAV.compid != 0) + { + doCommand((byte) sysidcurrent, (byte) compidcurrent, MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, param1, 0, + 0, 0, 0, 0, 0); + doCommand((byte) sysidcurrent, (byte) compidcurrent, MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 0, 0, 0, + 0, 0, 0); + } + } + + giveComport = false; + return true; + } + + [Obsolete] + public bool doARM(bool armit, bool force = false) + { + return doARM(MAV.sysid, MAV.compid, armit, force); + } + + public bool doARM(byte sysid, byte compid, bool armit, bool force = false) + { + return doARMAsync(sysid, compid, armit, force).AwaitSync(); + } + + public async Task doARMAsync(byte sysid, byte compid, bool armit, bool force = false) + { + const float magic_force_arm_value = 2989.0f; + const float magic_force_disarm_value = 21196.0f; + + if (force) + { + if (armit) + return await doCommandAsync(sysid, compid, MAV_CMD.COMPONENT_ARM_DISARM, 1, magic_force_arm_value, + 0, 0, 0, 0, + 0).ConfigureAwait(false); + else + return await doCommandAsync(sysid, compid, MAV_CMD.COMPONENT_ARM_DISARM, 0, + magic_force_disarm_value, 0, 0, 0, + 0, 0).ConfigureAwait(false); + } + else + { + if (armit) + return await doCommandAsync(sysid, compid, MAV_CMD.COMPONENT_ARM_DISARM, 1, 0, 0, 0, 0, 0, 0) + .ConfigureAwait(false); + else + return await doCommandAsync(sysid, compid, MAV_CMD.COMPONENT_ARM_DISARM, 0, 0, 0, 0, 0, + 0, 0).ConfigureAwait(false); + } + } + + [Obsolete] + public bool doAbortLand() + { + return doCommand(MAV_CMD.DO_GO_AROUND, 0, 0, 0, 0, 0, 0, 0); + } + + [Obsolete] + public bool doMotorTest(int motor, MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout, + int motorcount = 0) + { + return doCommand(MAV_CMD.DO_MOTOR_TEST, (float) motor, (float) (byte) thr_type, + (float) throttle, (float) timeout, (float) motorcount, 0, 0); + } + + [Obsolete] + public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7, + bool requireack = true) + { + return doCommand(MAV.sysid, MAV.compid, actionid, p1, p2, p3, p4, p5, p6, p7, requireack, null); + } + + public bool doCommand(byte sysid, byte compid, MAV_CMD actionid, float p1, float p2, float p3, + float p4, + float p5, float p6, float p7, bool requireack = true, Action uicallback = null) + { + return doCommandAsync(sysid, compid, actionid, p1, p2, p3, p4, p5, p6, p7, requireack, uicallback) + .AwaitSync(); + } + + public async Task doCommandAsync(byte sysid, byte compid, MAV_CMD actionid, float p1, float p2, float p3, + float p4, + float p5, float p6, float p7, bool requireack = true, Action uicallback = null) + { + if (BaseStream == null || BaseStream.IsOpen == false) + return false; + + MAVLinkMessage buffer; + + mavlink_command_long_t req = new mavlink_command_long_t + { + target_system = sysid, + target_component = compid, + command = (ushort) actionid, + confirmation = 0, + param1 = p1, + param2 = p2, + param3 = p3, + param4 = p4, + param5 = p5, + param6 = p6, + param7 = p7 + }; + + log.InfoFormat("doCommand cmd {0} {1} {2} {3} {4} {5} {6} {7}", actionid.ToString(), p1, p2, p3, p4, p5, p6, + p7); + + if (requireack) + giveComport = true; + + generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid); + + if (!requireack) + { + giveComport = false; + return true; + } + + DateTime GUI = DateTime.Now; + + DateTime start = DateTime.Now; + int retrys = 3; + + int timeout = 2000; + + // imu calib take a little while + if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1) + { + // this is for advanced accel offsets, and blocks execution + giveComport = false; + return true; + } + else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p6 == 1) + { + // compassmot + // send again just incase + generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid); + giveComport = false; + return true; + } + else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION) + { + retrys = 1; + timeout = 25000; + } + else if (actionid == MAV_CMD.FLASH_BOOTLOADER) + { + retrys = 1; + timeout = 25000; + } + else if (actionid == MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN) + { + generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid); + giveComport = false; + return true; + } + else if (actionid == MAV_CMD.COMPONENT_ARM_DISARM) + { + // 10 seconds as may need an imu calib + timeout = 10000; + } + else if (actionid == MAV_CMD.GET_HOME_POSITION) + { + giveComport = false; + return true; + } + + while (true) + { + if (DateTime.Now > GUI.AddMilliseconds(100)) + { + GUI = DateTime.Now; + + uicallback?.Invoke(); + } + + if (!(start.AddMilliseconds(timeout) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("doCommand Retry " + retrys); + req.confirmation++; + generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid); + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - doCommand"); + } + + buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.COMMAND_ACK && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + var ack = buffer.ToStructure(); + + if (ack.command != req.command) + { + log.InfoFormat("doCommand cmd resp {0} - {1} - Commands dont match", (MAV_CMD) ack.command, + (MAV_RESULT) ack.result); + continue; + } + + log.InfoFormat("doCommand cmd resp {0} - {1}", (MAV_CMD) ack.command, (MAV_RESULT) ack.result); + + + if (ack.result == (byte)MAV_RESULT.IN_PROGRESS) + { + start = DateTime.Now; + retrys = 0; + continue; + } + else if (ack.result == (byte) MAV_RESULT.ACCEPTED) + { + giveComport = false; + return true; + } + else + { + giveComport = false; + return false; + } + } + } + } + } + + public bool doCommandInt(byte sysid, byte compid, MAV_CMD actionid, float p1, float p2, float p3, float p4, + int p5, int p6, float p7, bool requireack = true, Action uicallback = null, + MAV_FRAME frame = MAV_FRAME.GLOBAL) + { + return doCommandIntAsync(sysid, compid, actionid, p1, p2, p3, p4, p5, p6, p7, requireack, uicallback, frame) + .AwaitSync(); + } + + public async Task doCommandIntAsync(byte sysid, byte compid, MAV_CMD actionid, float p1, float p2, + float p3, float p4, + int p5, int p6, float p7, bool requireack = true, Action uicallback = null, + MAV_FRAME frame = MAV_FRAME.GLOBAL) + { + if (BaseStream == null || BaseStream.IsOpen == false) + return false; + + MAVLinkMessage buffer; + + mavlink_command_int_t req = new mavlink_command_int_t() + { + target_system = sysid, + target_component = compid, + + command = (ushort) actionid, + + param1 = p1, + param2 = p2, + param3 = p3, + param4 = p4, + x = p5, + y = p6, + z = p7, + autocontinue = 0, + current = 0, + frame = (byte) frame + }; + + log.InfoFormat("doCommandIntAsync cmd {0} {1} {2} {3} {4} {5} {6} {7}", actionid.ToString(), p1, p2, p3, p4, + p5, p6, + p7); + + if (requireack) + giveComport = true; + + generatePacket((byte) MAVLINK_MSG_ID.COMMAND_INT, req, sysid, compid); + + if (!requireack) + { + giveComport = false; + return true; + } + + DateTime GUI = DateTime.Now; + + DateTime start = DateTime.Now; + int retrys = 3; + + int timeout = 2000; + + while (true) + { + if (DateTime.Now > GUI.AddMilliseconds(100)) + { + GUI = DateTime.Now; + + uicallback?.Invoke(); + } + + if (!(start.AddMilliseconds(timeout) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("doCommandIntAsync Retry " + retrys); + generatePacket((byte) MAVLINK_MSG_ID.COMMAND_INT, req, sysid, compid); + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - doCommand"); + } + + buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.COMMAND_ACK && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + var ack = buffer.ToStructure(); + + if (ack.command != req.command) + { + log.InfoFormat("doCommandIntAsync cmd resp {0} - {1} - Commands dont match", + (MAV_CMD) ack.command, + (MAV_RESULT) ack.result); + continue; + } + + log.InfoFormat("doCommandIntAsync cmd resp {0} - {1}", (MAV_CMD) ack.command, + (MAV_RESULT) ack.result); + + if (ack.result == (byte) MAV_RESULT.ACCEPTED) + { + giveComport = false; + return true; + } + else + { + giveComport = false; + return false; + } + } + } + } + } + + public void SendAck() + { + mavlink_command_ack_t ack = new mavlink_command_ack_t(); + ack.command = (ushort) MAV_CMD.PREFLIGHT_CALIBRATION; + ack.result = 0; + + // send twice + generatePacket(MAVLINK_MSG_ID.COMMAND_ACK, ack); + Thread.Sleep(20); + generatePacket(MAVLINK_MSG_ID.COMMAND_ACK, ack); + } + + public void SendSerialControl(SERIAL_CONTROL_DEV port, ushort timeoutms, byte[] data, uint baudrate = 0, + bool close = false) + { + mavlink_serial_control_t ctl = new mavlink_serial_control_t + { + baudrate = baudrate, + device = (byte) port, + timeout = timeoutms, + data = new byte[70], + count = 0 + }; + + if (close) + { + ctl.flags = 0; + } + else + { + ctl.flags = (byte) SERIAL_CONTROL_FLAG.EXCLUSIVE; // | SERIAL_CONTROL_FLAG.MULTI); + } + + if (data != null && data.Length != 0) + { + int packets = (data.Length / 70) + 1; + int len = data.Length; + while (len > 0) + { + if (packets == 1) + ctl.flags |= (byte) SERIAL_CONTROL_FLAG.RESPOND; + + byte n = (byte) Math.Min(70, len); + + ctl.count = n; + Array.Copy(data, data.Length - len, ctl.data, 0, n); + + // dont flood the port + Thread.Sleep(10); + + generatePacket((byte) MAVLINK_MSG_ID.SERIAL_CONTROL, ctl); + + len -= n; + packets--; + } + } + else + { + if (!close) + ctl.flags |= (byte) SERIAL_CONTROL_FLAG.RESPOND | (byte) SERIAL_CONTROL_FLAG.MULTI; + + generatePacket((byte) MAVLINK_MSG_ID.SERIAL_CONTROL, ctl); + } + } + + public void SendRCOverride(byte sysid, byte compid, UInt16 rc1, UInt16 rc2, UInt16 rc3, UInt16 rc4, UInt16 rc5, + UInt16 rc6, UInt16 rc7, UInt16 rc8) + { + mavlink_rc_channels_override_t rc = new mavlink_rc_channels_override_t + { + target_component = compid, + target_system = sysid, + chan1_raw = rc1, + chan2_raw = rc2, + chan3_raw = rc3, + chan4_raw = rc4, + chan5_raw = rc5, + chan6_raw = rc6, + chan7_raw = rc7, + chan8_raw = rc8 + }; + + sendPacket(rc, rc.target_system, rc.target_component); + } + + public void SendManualControl(byte sysid, byte compid, Int16 x, Int16 y, Int16 z, Int16 r, UInt16 buttons) + { + mavlink_manual_control_t mc = new mavlink_manual_control_t + { + target = sysid, + x = x, + y = y, + z = z, + r = r, + buttons = buttons + }; + + sendPacket(mc, sysid, compid); + } + + public void requestDatastream(MAV_DATA_STREAM id, byte hzrate, int sysid = -1, int compid = -1) + { + requestDatastream(id, (int) hzrate, sysid, compid); + } + + public void requestDatastream(MAV_DATA_STREAM id, int hzrate, int sysid = -1, int compid = -1) + { + if (sysid == -1) + sysid = sysidcurrent; + + if (compid == -1) + compid = compidcurrent; + + if (hzrate == -1) + return; + + double pps = 0; + + switch (id) + { + case MAV_DATA_STREAM.ALL: + + break; + + case MAV_DATA_STREAM.EXTENDED_STATUS: + if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.SYS_STATUS)) + { + if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.SYS_STATUS] < + DateTime.UtcNow.AddSeconds(-2)) + break; + pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.SYS_STATUS]; + } + + if (hzratecheck(pps, hzrate)) + { + return; + } + + break; + + case MAV_DATA_STREAM.EXTRA1: + if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.ATTITUDE)) + { + if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.ATTITUDE] < + DateTime.UtcNow.AddSeconds(-2)) + break; + pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.ATTITUDE]; + } + + if (hzratecheck(pps, hzrate)) + { + return; + } + + break; + + case MAV_DATA_STREAM.EXTRA2: + if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.VFR_HUD)) + { + if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.VFR_HUD] < + DateTime.UtcNow.AddSeconds(-2)) + break; + pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.VFR_HUD]; + } + + if (hzratecheck(pps, hzrate)) + { + return; + } + + break; + + case MAV_DATA_STREAM.EXTRA3: + if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.AHRS)) + { + if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.AHRS] < + DateTime.UtcNow.AddSeconds(-2)) + break; + pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.AHRS]; + } + + if (hzratecheck(pps, hzrate)) + { + return; + } + + break; + + case MAV_DATA_STREAM.POSITION: + if (MAVlist[sysid, compid] + .packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.GLOBAL_POSITION_INT)) + { + if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.GLOBAL_POSITION_INT] < + DateTime.UtcNow.AddSeconds(-2)) + break; + pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.GLOBAL_POSITION_INT]; + } + + if (hzratecheck(pps, hzrate)) + { + return; + } + + break; + + case MAV_DATA_STREAM.RAW_CONTROLLER: + if (MAVlist[sysid, compid] + .packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.RC_CHANNELS_SCALED)) + { + if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RC_CHANNELS_SCALED] < + DateTime.UtcNow.AddSeconds(-2)) + break; + pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RC_CHANNELS_SCALED]; + } + + if (hzratecheck(pps, hzrate)) + { + return; + } + + break; + + case MAV_DATA_STREAM.RAW_SENSORS: + if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.RAW_IMU)) + { + if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RAW_IMU] < + DateTime.UtcNow.AddSeconds(-2)) + break; + pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RAW_IMU]; + } + + if (hzratecheck(pps, hzrate)) + { + return; + } + + break; + + case MAV_DATA_STREAM.RC_CHANNELS: + if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.RC_CHANNELS_RAW)) + { + if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RC_CHANNELS_RAW] < + DateTime.UtcNow.AddSeconds(-2)) + break; + pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RC_CHANNELS_RAW]; + } + + if (hzratecheck(pps, hzrate)) + { + return; + } + + break; + } + + if (pps == 0 && hzrate == 0) + { + return; + } + + log.InfoFormat("Request stream {0} at {1} hz for {2}:{3}", + Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate, sysid, compid); + getDatastream((byte) sysid, (byte) compid, id, hzrate); + } + + // returns true for ok + private bool hzratecheck(double pps, int hzrate) + { + if (double.IsInfinity(pps)) + { + return false; + } + // 0 request + else if (hzrate == 0 && pps == 0) + { + return true; + } + + // range check pps, include packetloss + if (pps > hzrate - 1 && pps < hzrate + 0.1) + return true; + + return false; + } + + [Obsolete] + private void getDatastream(MAV_DATA_STREAM id, int hzrate) + { + getDatastream(MAV.sysid, MAV.compid, id, hzrate); + } + + private void getDatastream(byte sysid, byte compid, MAV_DATA_STREAM id, int hzrate) + { + if (hzrate == -1) + return; + + mavlink_request_data_stream_t req = new mavlink_request_data_stream_t + { + target_system = sysid, + target_component = compid, + req_message_rate = (byte) hzrate, + start_stop = 1, + req_stream_id = (byte) id + }; + + // send each one twice. + generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req, sysid, compid); + generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req, sysid, compid); + } + + [Obsolete] + public ushort getWPCount(MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + return getWPCountAsync(MAV.sysid, MAV.compid, type).AwaitSync(); + } + + public ushort getWPCount(byte sysid, byte compid, MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + return getWPCountAsync(sysid, compid, type).AwaitSync(); + } + + /// + /// Returns WP count + /// + /// + public async Task getWPCountAsync(byte sysid, byte compid, + MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + giveComport = true; + MAVLinkMessage buffer; + mavlink_mission_request_list_t req = new mavlink_mission_request_list_t + { + target_system = sysid, + target_component = compid, + mission_type = (byte) type + }; + + // request list + generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req); + + DateTime start = DateTime.Now; + int retrys = 6; + + while (true) + { + if (!(start.AddMilliseconds(700) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("getWPCount Retry " + retrys + " - giv com " + giveComport); + generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req); + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + //return (byte)int.Parse(param["WP_TOTAL"].ToString()); + throw new TimeoutException("Timeout on read - getWPCount"); + } + + buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_COUNT && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + var count = buffer.ToStructure(); + // check this gcs sent it + if (count.target_system != gcssysid || + count.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + log.Info("wpcount: " + count.count); + giveComport = false; + return count.count; // should be ushort, but apm has limited wp count < byte + } + } + } + } + + [Obsolete] + public Locationwp getHomePosition() + { + return getHomePositionAsync((byte) sysidcurrent, (byte) compidcurrent).AwaitSync(); + } + + public async Task getHomePositionAsync(byte sysid, byte compid) + { + Locationwp? ans = null; + + var sub1 = SubscribeToPacketType(MAVLINK_MSG_ID.HOME_POSITION, buffer => + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.HOME_POSITION) + { + var home = buffer.ToStructure(); + + ans = new Locationwp().Set(home.latitude / 1.0e7, home.longitude / 1.0e7, home.altitude / 1000.0, + (byte) MAV_CMD.WAYPOINT); + } + + return true; + }, (byte)sysid, (byte)compid); + + try + { + doCommand(sysid, compid, MAV_CMD.GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, false); + + giveComport = true; + DateTime start = DateTime.Now; + int retrys = 3; + + while (true) + { + if (ans != null) + return ans.Value; + + if (!(start.AddMilliseconds(700) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("getHomePosition Retry " + retrys + " - giv com " + giveComport); + doCommand(sysid, compid, MAV_CMD.GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, false); + start = DateTime.Now; + retrys--; + continue; + } + + throw new TimeoutException("Timeout on read - getHomePosition"); + } + + await readPacketAsync().ConfigureAwait(false); + } + } + finally + { + giveComport = false; + UnSubscribeToPacketType(sub1); + } + } + + [Obsolete] + public Locationwp getWP(ushort index, MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + return getWP(MAV.sysid, MAV.compid, index, type); + } + + public Locationwp getWP(byte sysid, byte compid, ushort index, + MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + return getWPAsync(sysid, compid, index, type).AwaitSync(); + } + + /// + /// Gets specfied WP + /// + /// + /// WP + public async Task getWPAsync(byte sysid, byte compid, ushort index, + MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + while (giveComport == true) + Thread.Sleep(10); + + bool use_int = (MAVlist[sysid, compid].cs.capabilities & (uint) MAV_PROTOCOL_CAPABILITY.MISSION_INT) > 0; + + object req; + + if (use_int) + { + mavlink_mission_request_int_t reqi = new mavlink_mission_request_int_t + { + target_system = sysid, + target_component = compid, + mission_type = (byte) type, + seq = index + }; + + // request + generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT, reqi); + + req = reqi; + } + else + { + mavlink_mission_request_t reqf = new mavlink_mission_request_t + { + target_system = sysid, + target_component = compid, + mission_type = (byte) type, + seq = index + }; + + // request + generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST, reqf); + + req = reqf; + } + + giveComport = true; + Locationwp loc = new Locationwp(); + + DateTime start = DateTime.Now; + int retrys = 5; + + while (true) + { + if (!(start.AddMilliseconds(2500) > DateTime.Now)) // apm times out after 5000ms + { + if (retrys > 0) + { + log.Info("getWP Retry " + retrys); + if (use_int) + generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT, req); + else + generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST, req); + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - getWP"); + } + + //Console.WriteLine("getwp read " + DateTime.Now.Millisecond); + MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); + //Console.WriteLine("getwp readend " + DateTime.Now.Millisecond); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM && buffer.sysid == sysid && + buffer.compid == compid) + { + //Console.WriteLine("getwp ans " + DateTime.Now.Millisecond); + + var wp = buffer.ToStructure(); + if (wp.target_system != gcssysid || + wp.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + // received a packet, but not what we requested + if (index != wp.seq) + { + generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST, req); + continue; + } + + loc.frame = wp.frame; + loc.id = (ushort) (wp.command); + loc.p1 = (wp.param1); + loc.p2 = (wp.param2); + loc.p3 = (wp.param3); + loc.p4 = (wp.param4); + + loc.alt = ((wp.z)); + loc.lat = ((wp.x)); + loc.lng = ((wp.y)); + + log.InfoFormat("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng, + loc.frame); + + break; + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM_INT && buffer.sysid == sysid && + buffer.compid == compid) + { + //Console.WriteLine("getwp ans " + DateTime.Now.Millisecond); + + var wp = buffer.ToStructure(); + // check this gcs sent it + if (wp.target_system != gcssysid || + wp.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + // received a packet, but not what we requested + if (index != wp.seq) + { + generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT, req); + continue; + } + + loc.frame = wp.frame; + loc.id = (ushort) (wp.command); + loc.p1 = (wp.param1); + loc.p2 = (wp.param2); + loc.p3 = (wp.param3); + loc.p4 = (wp.param4); + + loc.alt = ((wp.z)); + loc.lat = ((wp.x / 1.0e7)); + loc.lng = ((wp.y / 1.0e7)); + + if (!Locationwp.isLocationCommand(loc.id)) + { + loc.lat = wp.x; + loc.lng = wp.y; + } + + log.InfoFormat("getWPint {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, + loc.lng, + loc.frame); + + break; + } + else + { + //log.Info(DateTime.Now + " PC getwp " + buffer.msgid); + } + } + } + + giveComport = false; + return loc; + } + + public object DebugPacket(MAVLinkMessage datin) + { + string text = ""; + return DebugPacket(datin, ref text, true); + } + + public object DebugPacket(MAVLinkMessage datin, bool PrintToConsole) + { + string text = ""; + return DebugPacket(datin, ref text, PrintToConsole); + } + + public object DebugPacket(MAVLinkMessage datin, ref string text) + { + return DebugPacket(datin, ref text, true); + } + + /// + /// Print entire decoded packet to console + /// + /// packet byte array + /// struct of data + public object DebugPacket(MAVLinkMessage datin, ref string text, bool PrintToConsole, string delimeter = " ") + { + string textoutput = ""; + try + { + if (datin.Length > 5) + { + textoutput = + string.Format( + "{0,2:X}{8}{1,2:X}{8}{2,2:X}{8}{3,2:X}{8}{4,2:X}{8}{5,2:X}{8}{6,2:X}{8}{7,6:X}{8}", + datin.header, + datin.payloadlength, datin.incompat_flags, datin.compat_flags, datin.seq, datin.sysid, + datin.compid, datin.msgid, delimeter); + + object data = datin.data; + + Type test = data.GetType(); + + { + textoutput = textoutput + test.Name + delimeter; + + foreach (var field in test.GetFields()) + { + // field.Name has the field's name. + + object fieldValue = field.GetValue(data); // Get value + + if (field.FieldType.IsArray) + { + textoutput = textoutput + field.Name + delimeter; + if (fieldValue.GetType() == typeof(byte[])) + { + try + { + byte[] crap = (byte[]) fieldValue; + + foreach (byte fiel in crap) + { + if (fiel == 0) + { + break; + } + else + { + textoutput = textoutput + (char) fiel; + } + } + } + catch + { + } + } + + if (fieldValue.GetType() == typeof(char[])) + { + try + { + char[] crap = (char[]) fieldValue; + + foreach (char fiel in crap) + { + if (fiel == 0) + { + break; + } + else + { + textoutput = textoutput + fiel; + } + } + } + catch + { + } + } + + if (fieldValue.GetType() == typeof(short[])) + { + try + { + short[] crap = (short[]) fieldValue; + + foreach (short fiel in crap) + { + if (fiel == 0) + { + break; + } + else + { + textoutput = textoutput + Convert.ToString(fiel, 16) + "|"; + } + } + } + catch + { + } + } + + textoutput = textoutput + delimeter; + } + else + { + textoutput = textoutput + field.Name + delimeter + fieldValue.ToString() + delimeter; + } + } + + var sig = ""; + if (datin.sig != null) + sig = Convert.ToBase64String(datin.sig); + + textoutput = textoutput + delimeter + "sig " + sig + delimeter + "Len" + delimeter + + datin.Length + delimeter + "crc16" + delimeter + datin.crc16 + "\r\n"; + if (PrintToConsole) + Console.Write(textoutput); + + if (text != null) + text = textoutput; + } + + return data; + } + } + catch + { + textoutput = textoutput + "\r\n"; + } + + return null; + } + + [Obsolete] + public void setWPPartialUpdate(ushort startwp, ushort endwp, + MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + setWPPartialUpdateAsync(MAV.sysid, MAV.compid, startwp, endwp, type).ConfigureAwait(false).GetAwaiter() + .GetResult(); + } + + /// + /// Set start and finish for partial wp upload. + /// + /// + /// + public async Task setWPPartialUpdateAsync(byte sysid, byte compid, ushort startwp, ushort endwp, + MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + mavlink_mission_write_partial_list_t req = new mavlink_mission_write_partial_list_t + { + target_system = sysid, + target_component = compid, + start_index = (short) startwp, + end_index = (short) endwp, + mission_type = (byte) type + }; + + generatePacket((byte) MAVLINK_MSG_ID.MISSION_WRITE_PARTIAL_LIST, req); + } + + [Obsolete] + public void setWPTotal(ushort wp_total, MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + setWPTotalAsync(MAV.sysid, MAV.compid, wp_total, type).ConfigureAwait(false).GetAwaiter().GetResult(); + return; + } + + /// + /// Sets wp total count + /// + /// + public async Task setWPTotalAsync(byte sysid, byte compid, ushort wp_total, + MAVLink.MAV_MISSION_TYPE type = MAV_MISSION_TYPE.MISSION) + { + giveComport = true; + mavlink_mission_count_t req = new mavlink_mission_count_t + { + target_system = sysid, + target_component = compid, + count = wp_total, + mission_type = (byte) type + }; + + log.Info("setWPTotal req MISSION_COUNT " + req.ToJSON(Formatting.None)); + generatePacket((byte) MAVLINK_MSG_ID.MISSION_COUNT, req); + + DateTime start = DateTime.Now; + int retrys = 3; + + while (true) + { + if (!(start.AddMilliseconds(700) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("setWPTotal Retry " + retrys); + generatePacket((byte) MAVLINK_MSG_ID.MISSION_COUNT, req); + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - setWPTotal"); + } + + MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 9) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + var request = buffer.ToStructure(); + // check this gcs sent it + if (request.target_system != gcssysid || + request.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + if (request.seq == 0 || request.seq == 1) + { + if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) + { + if (MAV.param["WP_TOTAL"] != null) + MAV.param["WP_TOTAL"].Value = wp_total - 1; + if (MAV.param["CMD_TOTAL"] != null) + MAV.param["CMD_TOTAL"].Value = wp_total - 1; + if (MAV.param["MIS_TOTAL"] != null) + MAV.param["MIS_TOTAL"].Value = wp_total - 1; + MAVlist[req.target_system, req.target_component].wps.Clear(); + } + + if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) + MAVlist[req.target_system, req.target_component].fencepoints.Clear(); + if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) + MAVlist[req.target_system, req.target_component].rallypoints.Clear(); + + giveComport = false; + return; + } + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT && + buffer.sysid == req.target_system && buffer.compid == req.target_component) + { + var request = buffer.ToStructure(); + // check this gcs sent it + if (request.target_system != gcssysid || + request.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + if (request.seq == 0 || request.seq == 1) + { + if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) + { + if (MAV.param["WP_TOTAL"] != null) + MAV.param["WP_TOTAL"].Value = wp_total - 1; + if (MAV.param["CMD_TOTAL"] != null) + MAV.param["CMD_TOTAL"].Value = wp_total - 1; + if (MAV.param["MIS_TOTAL"] != null) + MAV.param["MIS_TOTAL"].Value = wp_total - 1; + MAVlist[req.target_system, req.target_component].wps.Clear(); + } + + if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) + MAVlist[req.target_system, req.target_component].fencepoints.Clear(); + if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) + MAVlist[req.target_system, req.target_component].rallypoints.Clear(); + + giveComport = false; + return; + } + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ACK && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + var ans = buffer.ToStructure(); + log.Info("setWPTotal ACK 47 : " + buffer.msgid + " ans " + + Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString())); + // check this gcs sent it + if (ans.target_system != gcssysid || + ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + giveComport = false; + return; // (MAV_MISSION_RESULT)ans.type; + } + else + { + //Console.WriteLine(DateTime.Now + " PC getwp " + buffer.msgid); + } + } + } + } + + [Obsolete] + public void InjectGpsData(byte[] data, ushort length) + { + InjectGpsData(MAV.sysid, MAV.compid, data, length); + } + + private int inject_seq_no = 0; + + /// + /// used to inject data into the gps ie rtcm/sbp/ubx + /// + /// + public void InjectGpsData(byte sysid, byte compid, byte[] data, ushort length, bool rtcm_message = true) + { + if (data == null) throw new ArgumentNullException(nameof(data)); + // new message + if (rtcm_message) + { + mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t(); + var msglen = 180; + + if (length > msglen * 4) + { + log.Error("Message too large " + length); + return; + } + + // number of packets we need, including a termination packet if needed + var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1; + + if (nopackets >= 4) + nopackets = 4; + + // flags = isfrag(1)/frag(2)/seq(5) + + for (int a = 0; a < nopackets; a++) + { + // check if its a fragment + if (nopackets > 1) + gps.flags = 1; + else + gps.flags = 0; + + // add fragment number + gps.flags += (byte) ((a & 0x3) << 1); + + // add seq number + gps.flags += (byte) ((inject_seq_no & 0x1f) << 3); + + // create the empty buffer + gps.data = new byte[msglen]; + + // calc how much data we are copying + int copy = Math.Min(length - a * msglen, msglen); + + // copy the data + Array.Copy(data, a * msglen, gps.data, 0, copy); + + // set the length + gps.len = (byte) copy; + + generatePacket((byte) MAVLINK_MSG_ID.GPS_RTCM_DATA, gps, sysid, compid); + } + + inject_seq_no++; + } + else + { + mavlink_gps_inject_data_t gps = new mavlink_gps_inject_data_t(); + var msglen = 110; + + var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1; + + for (int a = 0; a < len; a++) + { + gps.data = new byte[msglen]; + + int copy = Math.Min(length - a * msglen, msglen); + + Array.Copy(data, a * msglen, gps.data, 0, copy); + gps.len = (byte) copy; + gps.target_component = compid; + gps.target_system = sysid; + + generatePacket((byte) MAVLINK_MSG_ID.GPS_INJECT_DATA, gps, sysid, compid); + } + } + } + + [Obsolete] + public MAV_MISSION_RESULT setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current = 0, + byte autocontinue = 1, bool use_int = false, MAV_MISSION_TYPE mission_type = MAV_MISSION_TYPE.MISSION) + { + return setWP(MAV.sysid, MAV.compid, loc, index, frame, current, autocontinue, use_int, mission_type); + } + + public MAV_MISSION_RESULT setWP(byte sysid, byte compid, Locationwp loc, ushort index, MAV_FRAME frame, + byte current = 0, + byte autocontinue = 1, bool use_int = false, MAV_MISSION_TYPE mission_type = MAV_MISSION_TYPE.MISSION) + { + return setWPAsync(sysid, compid, loc, index, frame, current, autocontinue, use_int, mission_type) + .AwaitSync(); + } + + /// + /// Save wp to eeprom + /// + /// location struct + /// wp no + /// global or relative + /// 0 = no , 2 = guided mode + public async Task setWPAsync(byte sysid, byte compid, Locationwp loc, ushort index, + MAV_FRAME frame, byte current = 0, + byte autocontinue = 1, bool use_int = false, MAV_MISSION_TYPE mission_type = MAV_MISSION_TYPE.MISSION) + { + if (use_int) + { + mavlink_mission_item_int_t req = new mavlink_mission_item_int_t + { + target_system = sysid, + target_component = compid, + command = loc.id, + current = current, + autocontinue = autocontinue, + mission_type = (byte) mission_type, + frame = (byte) frame + }; + + if (!Locationwp.isLocationCommand(loc.id)) + { + req.y = (int) (loc.lng); + req.x = (int) (loc.lat); + } + else + { + req.y = (int) (loc.lng * 1.0e7); + req.x = (int) (loc.lat * 1.0e7); + } + + req.z = (float) (loc.alt); + + req.param1 = loc.p1; + req.param2 = loc.p2; + req.param3 = loc.p3; + req.param4 = loc.p4; + + req.seq = index; + + return await setWPAsync(req).ConfigureAwait(false); + } + else + { + mavlink_mission_item_t req = new mavlink_mission_item_t + { + target_system = sysid, + target_component = compid, + command = loc.id, + current = current, + autocontinue = autocontinue, + mission_type = (byte) mission_type, + frame = (byte) frame, + y = (float) (loc.lng), + x = (float) (loc.lat), + z = (float) (loc.alt), + param1 = loc.p1, + param2 = loc.p2, + param3 = loc.p3, + param4 = loc.p4, + seq = index + }; + + return await setWPAsync(req).ConfigureAwait(false); + } + } + + [Obsolete] + public async Task setWPAsync(mavlink_mission_item_t req) + { + giveComport = true; + + ushort index = req.seq; + + log.InfoFormat("setWP {7}:{8} {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, + req.param1, + req.x, req.y, req.z, index, req.target_system, req.target_component); + + // request + generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM, req); + + DateTime start = DateTime.Now; + int retrys = 10; + + while (true) + { + if (!(start.AddMilliseconds(450) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("setWP Retry " + retrys); + generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM, req); + + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - setWP"); + } + + MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ACK && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + var ans = buffer.ToStructure(); + log.Info("set wp " + index + " ACK 47 : " + buffer.msgid + " ans " + + Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString())); + // check this gcs sent it + if (ans.target_system != gcssysid || + ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + if (req.current == 2) + { + MAVlist[req.target_system, req.target_component].GuidedMode = (Locationwp) req; + } + else if (req.current == 3) + { + } + else + { + if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) + MAVlist[req.target_system, req.target_component].wps[req.seq] = (Locationwp) req; + if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) + MAVlist[req.target_system, req.target_component].fencepoints[req.seq] = + (Locationwp) req; + if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) + MAVlist[req.target_system, req.target_component].rallypoints[req.seq] = + (Locationwp) req; + } + + //if (ans.target_system == req.target_system && ans.target_component == req.target_component) + { + giveComport = false; + return (MAV_MISSION_RESULT) ans.type; + } + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST && + buffer.sysid == req.target_system && buffer.compid == req.target_component) + { + var ans = buffer.ToStructure(); + + if (ans.target_system != gcssysid || + ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + if (ans.seq == (index + 1)) + { + log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer.msgid); + giveComport = false; + + if (req.current == 2) + { + MAVlist[req.target_system, req.target_component].GuidedMode = (Locationwp) req; + } + else if (req.current == 3) + { + } + else + { + if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) + MAVlist[req.target_system, req.target_component].wps[req.seq] = (Locationwp) req; + if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) + MAVlist[req.target_system, req.target_component].fencepoints[req.seq] = + (Locationwp) req; + if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) + MAVlist[req.target_system, req.target_component].rallypoints[req.seq] = + (Locationwp) req; + } + + //if (ans.target_system == req.target_system && ans.target_component == req.target_component) + { + giveComport = false; + return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED; + } + } + else + { + log.InfoFormat( + "set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + + buffer.msgid + + " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component); + // resend point now + start = DateTime.MinValue; + } + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST_INT && + buffer.sysid == req.target_system && buffer.compid == req.target_component) + { + var ans = buffer.ToStructure(); + + if (ans.target_system != gcssysid || + ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + if (ans.seq == (index + 1)) + { + log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer.msgid); + giveComport = false; + + if (req.current == 2) + { + MAVlist[req.target_system, req.target_component].GuidedMode = (Locationwp) req; + } + else if (req.current == 3) + { + } + else + { + MAVlist[req.target_system, req.target_component].wps[req.seq] = (Locationwp) req; + } + + //if (ans.target_system == req.target_system && ans.target_component == req.target_component) + { + giveComport = false; + return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED; + } + } + else + { + log.InfoFormat( + "set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + + buffer.msgid + + " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component); + // resend point now + start = DateTime.MinValue; + } + } + else + { + //Console.WriteLine(DateTime.Now + " PC setwp " + buffer.msgid); + } + } + } + + // return MAV_MISSION_RESULT.MAV_MISSION_INVALID; + } + + [Obsolete] + public async Task setWPAsync(mavlink_mission_item_int_t req) + { + giveComport = true; + + ushort index = req.seq; + + log.Info("setWPint req MISSION_ITEM_INT " + req.ToJSON(Formatting.None)); + //log.InfoFormat("setWPint {7}:{8} {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, + // req.x / 1.0e7, req.y /1.0e7 , req.z, index, req.target_system, req.target_component); + + // request + generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM_INT, req); + + DateTime start = DateTime.Now; + int retrys = 10; + + while (true) + { + if (!(start.AddMilliseconds(450) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("setWP Retry " + retrys); + generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM_INT, req); + + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - setWP"); + } + + MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ACK && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + var ans = buffer.ToStructure(); + log.Info("setWPint resp MISSION_ACK " + buffer.ToJSON(Formatting.None)); + //log.Info("set wp " + index + " ACK 47 : " + buffer.msgid + " ans " + + // Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString())); + // check this gcs sent it + if (ans.target_system != gcssysid || + ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + if (req.current == 2) + { + MAVlist[req.target_system, req.target_component].GuidedMode = (Locationwp) req; + } + else if (req.current == 3) + { + } + else + { + if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) + MAVlist[req.target_system, req.target_component].wps[req.seq] = (Locationwp) req; + if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) + MAVlist[req.target_system, req.target_component].fencepoints[req.seq] = + (Locationwp) req; + if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) + MAVlist[req.target_system, req.target_component].rallypoints[req.seq] = + (Locationwp) req; + } + + //if (ans.target_system == req.target_system && ans.target_component == req.target_component) + { + giveComport = false; + return (MAV_MISSION_RESULT) ans.type; + } + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST && + buffer.sysid == req.target_system && buffer.compid == req.target_component) + { + var ans = buffer.ToStructure(); + if (ans.target_system != gcssysid || + ans.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + if (ans.seq == (index + 1)) + { + log.Info("setWPint resp MISSION_REQUEST" + buffer.ToJSON(Formatting.None)); + //log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer.msgid); + + if (req.current == 2) + { + MAVlist[req.target_system, req.target_component].GuidedMode = (Locationwp) req; + } + else if (req.current == 3) + { + } + else + { + if (req.mission_type == (byte) MAV_MISSION_TYPE.MISSION) + MAVlist[req.target_system, req.target_component].wps[req.seq] = (Locationwp) req; + if (req.mission_type == (byte) MAV_MISSION_TYPE.FENCE) + MAVlist[req.target_system, req.target_component].fencepoints[req.seq] = + (Locationwp) req; + if (req.mission_type == (byte) MAV_MISSION_TYPE.RALLY) + MAVlist[req.target_system, req.target_component].rallypoints[req.seq] = + (Locationwp) req; + } + + //if (ans.target_system == req.target_system && ans.target_component == req.target_component) + { + giveComport = false; + return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED; + } + } + else + { + log.InfoFormat( + "set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + + buffer.msgid + + " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component); + // resend point now + start = DateTime.MinValue; + } + } + else + { + //Console.WriteLine(DateTime.Now + " PC setwp " + buffer.msgid); + } + } + } + + // return MAV_MISSION_RESULT.MAV_MISSION_INVALID; + } + + public int getRequestedWPNo(byte sysid, byte compid) + { + return getRequestedWPNoAsync(sysid, compid).AwaitSync(); + } + + public async Task getRequestedWPNoAsync(byte sysid, byte compid) + { + giveComport = true; + DateTime start = DateTime.Now; + + while (true) + { + if (!(start.AddMilliseconds(1500) > DateTime.Now)) + { + giveComport = false; + throw new TimeoutException("Timeout on read - getRequestedWPNo"); + } + + MAVLinkMessage buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte)MAVLINK_MSG_ID.MISSION_REQUEST && buffer.sysid == sysid && + buffer.compid == compid) + { + var ans = buffer.ToStructure(); + + log.InfoFormat("getRequestedWPNo seq {0} ts {1} tc {2}", ans.seq, ans.target_system, + ans.target_component); + + giveComport = false; + + return ans.seq; + } + if (buffer.msgid == (byte)MAVLINK_MSG_ID.MISSION_REQUEST_INT && buffer.sysid == sysid && + buffer.compid == compid) + { + var ans = buffer.ToStructure(); + + log.InfoFormat("getRequestedWPNo INT seq {0} ts {1} tc {2}", ans.seq, ans.target_system, + ans.target_component); + + giveComport = false; + + return ans.seq; + } + } + } + } + + [Obsolete] + public void setGuidedModeWP(Locationwp gotohere, bool setguidedmode = true) + { + setGuidedModeWP(MAV.sysid, MAV.compid, gotohere, setguidedmode); + } + + public void setGuidedModeWP(byte sysid, byte compid, Locationwp gotohere, bool setguidedmode = true) + { + if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0) + return; + + try + { + gotohere.id = (ushort) MAV_CMD.WAYPOINT; + + if (setguidedmode) + { + // fix for followme change + if (MAVlist[sysid, compid].cs.mode.ToUpper() != "GUIDED") + setMode(sysid, compid, "GUIDED"); + } + + log.InfoFormat("setGuidedModeWP {0}:{1} lat {2} lng {3} alt {4}", sysid, compid, gotohere.lat, + gotohere.lng, gotohere.alt); + + if (MAVlist[sysid, compid].cs.firmware == Firmwares.ArduPlane) + { + MAV_MISSION_RESULT ans = setWP(sysid, compid, gotohere, 0, (MAV_FRAME)gotohere.frame, (byte) 2); + + if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED) + { + throw new Exception("Guided Mode Failed"); + } + } + else + { + setPositionTargetGlobalInt((byte) sysid, (byte) compid, + true, false, false, false, (MAV_FRAME)gotohere.frame, + gotohere.lat, gotohere.lng, gotohere.alt, 0, 0, 0, 0, 0); + } + } + catch (Exception ex) + { + log.Error(ex); + } + } + + [Obsolete] + public void setNewWPAlt(Locationwp gotohere) + { + setNewWPAlt((byte) sysidcurrent, (byte) compidcurrent, gotohere); + } + + public void setNewWPAlt(byte sysid, byte compid, Locationwp gotohere) + { + try + { + gotohere.id = (ushort) MAV_CMD.WAYPOINT; + + log.InfoFormat("setNewWPAlt {0}:{1} lat {2} lng {3} alt {4}", sysid, compid, gotohere.lat, gotohere.lng, + gotohere.alt); + + MAV_MISSION_RESULT ans = setWP(sysid, compid, gotohere, 0, MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte) 3); + + if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED) + { + throw new Exception("Alt Change Failed"); + } + + /* + // currently plane supports just an alt change, copter requires all lat/lng/alt + setPositionTargetGlobalInt((byte)sysid, (byte)compid, + true, false, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, + gotohere.lat, gotohere.lng, gotohere.alt, 0, 0, 0, 0, 0); + */ + } + catch (Exception ex) + { + log.Error(ex); + throw; + } + } + + public void setPositionTargetGlobalInt(byte sysid, byte compid, bool pos, bool vel, bool acc, bool yaw, + MAV_FRAME frame, double lat, double lng, double alt, double vx, double vy, double vz, double yawangle, + double yawrate) + { + // for mavlink SET_POSITION_TARGET messages + const ushort MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2)); + const ushort MAVLINK_SET_POS_TYPE_MASK_ALT_IGNORE = ((0 << 0) | (0 << 1) | (1 << 2)); + const ushort MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = ((1 << 3) | (1 << 4) | (1 << 5)); + const ushort MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = ((1 << 6) | (1 << 7) | (1 << 8)); + const ushort MAVLINK_SET_POS_TYPE_MASK_FORCE = ((1 << 9)); + const ushort MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE = ((1 << 10) | (1 << 11)); + + mavlink_set_position_target_global_int_t target = new mavlink_set_position_target_global_int_t() + { + target_system = sysid, + target_component = compid, + alt = (float) alt, + lat_int = (int) (lat * 1e7), + lon_int = (int) (lng * 1e7), + coordinate_frame = (byte) frame, + vx = (float) vx, + vy = (float) vy, + vz = (float) vz, + yaw = (float) yawangle, + yaw_rate = (float) yawrate + }; + + target.type_mask = ushort.MaxValue; + target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_FORCE; + + if (pos && lat != 0 && lng != 0) + target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; + if (pos && lat == 0 && lng == 0) + target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_ALT_IGNORE; + if (vel) + target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; + if (acc) + target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; + if (yaw) + target.type_mask -= MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; + + if (pos) + { + if (lat != 0) + MAVlist[sysid, compid].GuidedMode.x = (int) (lat * 1e7); + if (lng != 0) + MAVlist[sysid, compid].GuidedMode.y = (int) (lng * 1e7); + MAVlist[sysid, compid].GuidedMode.z = (float) alt; + } + + bool pos_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE) > 0; + bool vel_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE) > 0; + bool acc_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE) > 0; + + generatePacket((byte) MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT, target, sysid, compid); + } + + [Obsolete] + public void setDigicamControl(bool shot) + { + mavlink_digicam_control_t req = new mavlink_digicam_control_t(); + + req.target_system = MAV.sysid; + req.target_component = MAV.compid; + req.shot = (shot == true) ? (byte) 1 : (byte) 0; + + if (!doCommand((byte) sysidcurrent, (byte) compidcurrent, MAV_CMD.DO_DIGICAM_CONTROL, 0, 0, 0, 0, 1, 0, 0)) + { + generatePacket((byte) MAVLINK_MSG_ID.DIGICAM_CONTROL, req); + } + } + + [Obsolete] + public void setMountConfigure(MAV_MOUNT_MODE mountmode, bool stabroll, bool stabpitch, bool stabyaw) + { + byte stab_roll = (stabroll == true) ? (byte) 1 : (byte) 0; + byte stab_pitch = (stabpitch == true) ? (byte) 1 : (byte) 0; + byte stab_yaw = (stabyaw == true) ? (byte) 1 : (byte) 0; + + // p1 : mount mode + // p2, p3, p4 : stabilize roll, pitch, yaw + // p5, p6, p7 : empty + // no ack required + doCommand(MAV.sysid, MAV.compid, MAV_CMD.DO_MOUNT_CONFIGURE, (byte)mountmode, stab_roll, stab_pitch, stab_yaw, 0, 0, 0, false); + } + + [Obsolete] + public void setMountControl(double pa, double pb, double pc, bool islatlng) + { + setMountControl(MAV.sysid, MAV.compid, pa, pb, pc, islatlng); + } + + public void setMountControl(byte sysid, byte compid, double pa, double pb, double pc, bool islatlng) + { + if (!islatlng) + { + // pa is pitch, pb is roll, pc is yaw in centi-degrees. convert to degrees + doCommand(sysid, compid, MAV_CMD.DO_MOUNT_CONTROL, (float)(pa * 0.01), (float)(pb * 0.01), (float)(pc * 0.01), 0, 0, 0, (float)MAV_MOUNT_MODE.MAVLINK_TARGETING, false); + } + else + { + // pa is lat in degrees, placed in param5 in deg * 10e7 + // pb is lon in degrees, placed in param6 in deg * 10e7 + // pc is alt in meters, placed in param5 in meters + doCommand(sysid, compid, MAV_CMD.DO_MOUNT_CONTROL, 0, 0, 0, (float)pc, (float)(pa * 10000000.0f), (float)(pb * 10000000.0f), (float)MAV_MOUNT_MODE.MAVLINK_TARGETING, false); + } + } + + [Obsolete] + public void setMode(string modein) + { + setMode(MAV.sysid, MAV.compid, modein); + } + + public void setMode(byte sysid, byte compid, string modein) + { + mavlink_set_mode_t mode = new mavlink_set_mode_t(); + + if (translateMode(sysid, compid, modein, ref mode)) + { + log.Info("setMode " + modein + " (" + mode.custom_mode + ")"); + setMode(sysid, compid, mode); + } + } + + [Obsolete] + public void setMode(mavlink_set_mode_t mode, MAV_MODE_FLAG base_mode = 0) + { + setMode(MAV.sysid, MAV.compid, mode, base_mode); + } + + public void setMode(byte sysid, byte compid, mavlink_set_mode_t mode, MAV_MODE_FLAG base_mode = 0) + { + mode.base_mode |= (byte) base_mode; + + // new + doCommand(sysid, compid, MAV_CMD.DO_SET_MODE, mode.base_mode, mode.custom_mode, 0, 0, 0, 0, 0, false); + + // old + generatePacket((byte) (byte) MAVLINK_MSG_ID.SET_MODE, mode, sysid, compid); + Thread.Sleep(10); + generatePacket((byte) (byte) MAVLINK_MSG_ID.SET_MODE, mode, sysid, compid); + } + + private double t7 = 1.0e7; + + public bool debug = false; + + private static bool ByteArrayCompare(ReadOnlySpan a1, ReadOnlySpan a2) + { + return a1.SequenceEqual(a2); + } + + /// + /// used for a readlock on readpacket + /// + private SemaphoreSlim readlock = new SemaphoreSlim(1, 1); + + /// + /// Serial Reader to read mavlink packets. POLL method + /// + /// + public MAVLinkMessage readPacket() + { + return readPacketAsync().AwaitSync(); + } + + public async Task readPacketAsync() + { + byte[] buffer = new byte[MAVLINK_MAX_PACKET_LEN + 25]; + int count = 0; + int length = 0; + int readcount = 0; + MAVLinkMessage message = null; + bool skipheader = false; + + //These need to be function wide, because of the extension of ReadLock try/catch + byte sysid; + byte compid; + byte packetSeqNo; + uint msgid; + bool packetSeemValid = false; + + DateTime start = DateTime.Now; + + if (debug) + Console.WriteLine(DateTime.Now.Millisecond + " SR0 " + BaseStream?.BytesToRead); + + await readlock.WaitAsync().ConfigureAwait(false); + + try + { + if (debug) + Console.WriteLine(DateTime.Now.Millisecond + " SR1 " + BaseStream?.BytesToRead); + + readloop: + + while ((BaseStream != null && BaseStream.IsOpen) || logreadmode) + { + try + { + if (readcount > 300) + { + break; + } + + readcount++; + if (logreadmode) + { + message = readlogPacketMavlink(); + buffer = message.buffer; + if (buffer == null || buffer.Length == 0) + return MAVLinkMessage.Invalid; + } + else + { + if (BaseStream.ReadTimeout != 1200) + BaseStream.ReadTimeout = + 1200; // 1200 ms between chars - the gps detection requires this. + + // time updated for internal reference + MAV.cs.datetime = DateTime.Now; + + if (!skipheader) + { + DateTime to = DateTime.UtcNow.AddMilliseconds(BaseStream.ReadTimeout); + + if (debug) + Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream?.BytesToRead); + + while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0) + { + if (DateTime.UtcNow > to) + { + log.InfoFormat("MAVLINK: 1 wait time out btr {0} len {1}", + BaseStream?.BytesToRead, + length); + throw new TimeoutException("Timeout"); + } + + await Task.Delay(1).ConfigureAwait(false); + if (debug) + Console.WriteLine(DateTime.Now.Millisecond + " SR0b " + + BaseStream?.BytesToRead); + } + + if (debug) + Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream?.BytesToRead); + if (BaseStream.IsOpen) + { + BaseStream.Read(buffer, count, 1); + if (rawlogfile != null && rawlogfile.CanWrite) + rawlogfile.WriteByte(buffer[count]); + } + + if (debug) + Console.WriteLine(DateTime.Now.Millisecond + " SR1b " + BaseStream?.BytesToRead); + } + + skipheader = false; + } + } + catch (Exception e) + { + skipheader = false; + log.Info("MAVLink readpacket read error: " + e.ToString()); + break; + } + + // check if looks like a mavlink packet and check for exclusions and write to console + if (buffer[0] != MAVLINK_STX_MAVLINK1 && buffer[0] != MAVLINK_STX) + { + if (buffer[0] >= 0x20 && buffer[0] <= 127 || buffer[0] == '\n' || buffer[0] == '\r') + { + // Write printable characters outside of mavlink packets to console + Console.Write((char)buffer[0]); + + // check for line termination and log it + if (buffer[0] == '\r' || buffer[0] == '\n') + { + // check new line is valid + if (buildplaintxtline.Length > 3) + { + plaintxtline = buildplaintxtline; + plaintxtlinebuffer.Insert(0, plaintxtline); + while (plaintxtlinebuffer.Count >= 52) + plaintxtlinebuffer.RemoveAt(plaintxtlinebuffer.Count - 1); + } + + log.Info(plaintxtline); + // reset for next line + buildplaintxtline = ""; + } + else + { + buildplaintxtline += (char)buffer[0]; + } + } + + _bytesReceivedSubj.OnNext(1); + count = 0; + buffer[1] = 0; + continue; + } + + // reset count on valid packet + readcount = 0; + + if (debug) + Console.WriteLine(DateTime.Now.Millisecond + " SR2 " + BaseStream?.BytesToRead); + + // check for a header + if (buffer[0] == MAVLINK_STX_MAVLINK1 || buffer[0] == MAVLINK_STX) + { + var mavlinkv2 = buffer[0] == MAVLINK_STX ? true : false; + + int headerlength = mavlinkv2 ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN; + int headerlengthstx = headerlength + 1; + + // if we have the header, and no other chars, get the length and packet identifiers + if (count < headerlength && !logreadmode) + { + DateTime to = DateTime.UtcNow.AddMilliseconds(BaseStream.ReadTimeout); + + while (BaseStream.IsOpen && BaseStream.BytesToRead < headerlength - count) + { + if (DateTime.UtcNow > to) + { + log.InfoFormat("MAVLINK: 2 wait time out btr {0} len {1}", BaseStream.BytesToRead, + length); + throw new TimeoutException("Timeout"); + } + + await Task.Delay(1).ConfigureAwait(false); + } + + if (debug) + Console.WriteLine(DateTime.Now.Millisecond + " SR2a " + BaseStream?.BytesToRead); + //var start1 = DateTime.Now; + int read = BaseStream.Read(buffer, count + 1, headerlength - count); + //var end = DateTime.Now - start1; + //var lapse = end.TotalMilliseconds; + //Console.WriteLine("read: " + lapse); + if (debug) + Console.WriteLine(DateTime.Now.Millisecond + " SR2b " + BaseStream?.BytesToRead); + if (rawlogfile != null && rawlogfile.CanWrite) + rawlogfile.Write(buffer, count + 1, read); + count += read; + } + + // packet length + if (buffer[0] == MAVLINK_STX) + { + length = buffer[1] + headerlengthstx + + MAVLINK_NUM_CHECKSUM_BYTES; // data + header + checksum - magic - length + if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0) + { + length += MAVLINK_SIGNATURE_BLOCK_LEN; + } + } + else + { + length = buffer[1] + headerlengthstx + + MAVLINK_NUM_CHECKSUM_BYTES; // data + header + checksum - U - length + } + + if (debug) + Console.WriteLine(DateTime.Now.Millisecond + " SR3 " + BaseStream?.BytesToRead); + + if (count >= headerlength || logreadmode) + { + try + { + if (logreadmode) + { + } + else + { + DateTime to = DateTime.UtcNow.AddMilliseconds(BaseStream.ReadTimeout); + + while (BaseStream.IsOpen && BaseStream.BytesToRead < (length - (headerlengthstx))) + { + if (DateTime.UtcNow > to) + { + log.InfoFormat("MAVLINK: 3 wait time out btr {0} len {1}", + BaseStream.BytesToRead, length); + break; + } + + await Task.Delay(1).ConfigureAwait(false); + } + + if (BaseStream.IsOpen) + { + //var start1 = DateTime.UtcNow; + int read = BaseStream.Read(buffer, headerlengthstx, length - (headerlengthstx)); + //var end = DateTime.UtcNow - start1; + //var lapse = end.TotalMilliseconds; + //Console.WriteLine("read: " + lapse); + + if (read != (length - headerlengthstx)) + log.InfoFormat("MAVLINK: bad read {0}, {1}, {2}", headerlengthstx, length, + count); + if (rawlogfile != null && rawlogfile.CanWrite) + { + // write only what we read, temp is the whole packet, so 6-end + rawlogfile.Write(buffer, headerlengthstx, read); + } + } + } + + count = length; + } + catch + { + break; + } + + break; + } + } + + count++; + if (count == 299) + break; + } + + if (debug) + Console.WriteLine(DateTime.Now.Millisecond + " SR4 " + BaseStream?.BytesToRead); + //ReadLock ended here + + // resize the packet to the correct length + Array.Resize(ref buffer, count); + + // add byte count + _bytesReceivedSubj.OnNext(buffer.Length); + + // update bps statistics + if (_bpstime.Second != DateTime.UtcNow.Second) + { + long btr = 0; + if (BaseStream != null && BaseStream.IsOpen) + { + btr = BaseStream.BytesToRead; + } + else if (logreadmode) + { + btr = logplaybackfile.BaseStream.Length - logplaybackfile.BaseStream.Position; + } + + if (printbps) + Console.Write( + "bps {0} loss {1} left {2} mem {3} mav2 {4} sign {5} mav1 {6} mav2 {7} signed {8} \n", + _bps1, + MAV.synclost, btr, + GC.GetTotalMemory(false) / 1024 / 1024.0, MAV.mavlinkv2, MAV.signing, + _mavlink1count, + _mavlink2count, _mavlink2signed); + _bps2 = _bps1; // prev sec + _bps1 = 0; // current sec + _bpstime = DateTime.UtcNow; + _mavlink1count = 0; + _mavlink2count = 0; + _mavlink2signed = 0; + } + + _bps1 += buffer.Length; + + if (buffer.Length == 0) + return MAVLinkMessage.Invalid; + + if (message == null) + message = new MAVLinkMessage(buffer, DateTime.UtcNow); + + msgid = message.msgid; + + message_info msginfo = MAVLINK_MESSAGE_INFOS.GetMessageInfo(msgid); + + // calc crc + var sigsize = (message.sig != null) ? MAVLINK_SIGNATURE_BLOCK_LEN : 0; + ushort crc = MavlinkCRC.crc_calculate(buffer, + message.Length - sigsize - MAVLINK_NUM_CHECKSUM_BYTES); + + // calc extra bit of crc for mavlink 1.0/2.0 + if (message.header == MAVLINK_STX_MAVLINK1 || message.header == MAVLINK_STX) + { + crc = MavlinkCRC.crc_accumulate(msginfo.crc, crc); + } + + // check message length size vs table (mavlink1 explicit size check | mavlink2 allow all, undersize 0 trimmed, and oversize unknown extension) + if (!message.ismavlink2 && message.payloadlength != msginfo.minlength) + { + if (msginfo.length == 0) // pass for unknown packets + { + log.InfoFormat("unknown packet type {0}", message.msgid); + } + else + { + log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", buffer.Length, + message.msgid); + return MAVLinkMessage.Invalid; + } + } + + // check crc + if ((message.crc16 >> 8) != (crc >> 8) || + (message.crc16 & 0xff) != (crc & 0xff)) + { + if (buffer.Length > 5 && msginfo.name != null) + log.InfoFormat("Mavlink Bad Packet (crc fail) len {0} crc {1} vs {4} pkno {2} {3}", + buffer.Length, + crc, message.msgid, msginfo.name.ToString(), + message.crc16); + if (logreadmode) + log.InfoFormat("bad packet pos {0} ", logplaybackfile.BaseStream.Position); + // unknown packet + if (buffer.Length > 5 && String.IsNullOrEmpty(msginfo.name)) + ProcessMirrorStream(buffer); + + // look within the last 20 bytes + int offset = buffer.Search(new byte[] { MAVLINK_STX }, Math.Max(1, buffer.Length - 20)); + if (offset > 0) + { + var newbuffer = new byte[MAVLINK_MAX_PACKET_LEN + 25]; + Array.ConstrainedCopy(buffer, offset, newbuffer, 0, buffer.Length - offset); + count = buffer.Length - offset - 1; + log.InfoFormat($"rewind oldlen {buffer.Length} offset {offset} cnt {count}"); + buffer = newbuffer; + message = null; + skipheader = true; + goto readloop; + } + + return MAVLinkMessage.Invalid; + } + + sysid = message.sysid; + compid = message.compid; + packetSeqNo = message.seq; + + // create a state for any sysid/compid includes gcs on log playback + if (!MAVlist.Contains(sysid, compid)) + { + // create an item - hidden + MAVlist.AddHiddenList(sysid, compid); + // prevent packetloss counter on connect + MAVlist[sysid, compid].recvpacketcount = unchecked(packetSeqNo - (byte)1); + } + + // once set it cannot be reverted + if (!MAVlist[sysid, compid].mavlinkv2) + MAVlist[sysid, compid].mavlinkv2 = message.buffer[0] == MAVLINK_STX ? true : false; + + // stat count + if (message.buffer[0] == MAVLINK_STX) + _mavlink2count++; + else if (message.buffer[0] == MAVLINK_STX_MAVLINK1) + _mavlink1count++; + + //check if sig was included in packet, and we are not ignoring the signature (signing isnt checked else we wont enable signing) + //logreadmode we always ignore signing as they would not be in the log if they failed the signature + if (message.sig != null && !MAVlist[sysid, compid].signingignore && !logreadmode) + { + _mavlink2signed++; + + bool valid = false; + + // check existing key + if (MAVlist[sysid, compid].signingKey != null) + valid = CheckSignature(MAVlist[sysid, compid].signingKey, message, sysid, compid); + + if (!valid) + { + foreach (var AuthKey in MAVAuthKeys.Keys.Values) + { + valid = CheckSignature(AuthKey.Key, message, sysid, compid); + if (valid) + break; + } + + if (!valid) + { + log.InfoFormat("Packet failed signature but passed crc"); + return MAVLinkMessage.Invalid; + } + } + + var timestamp = new DateTime(2015, 1, 1).AddMilliseconds(message.sigTimestamp / 100.0); + var delta = DateTime.UtcNow - timestamp; + } + + // packet is now verified + + // log before any processing that could send response packets (e.g. timesync) + try + { + SaveToTlog(new Span(buffer)); + + if (logfile != null) + { + lock (logfile) + { + if (msgid == 0) + { + if (logfile != null && logfile.CanWrite) + logfile.Flush(); + if (rawlogfile != null && rawlogfile.CanWrite) + rawlogfile.Flush(); + } + } + } + } + catch (Exception ex) + { + log.Error(ex); + } + + // extract wp's/rally/fence/camera feedback/params from stream, including gcs packets on playback. + // may also send responses (e.g. timesync reply) + if (buffer.Length >= 5) + { + processInfoFromStream(ref message, sysid, compid); + } + + // if its a gcs packet - dont process further + if (buffer.Length >= 5 && (sysid == gcssysid || sysid == 253) && logreadmode) // gcs packet + { + return message; + } + + // update packet loss statistics + if (!logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < DateTime.UtcNow) + { + MAVlist[sysid, compid].packetlosttimer = DateTime.UtcNow; + MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost * 0.8f); + MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost * 0.8f); + } + else if (logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < lastlogread) + { + MAVlist[sysid, compid].packetlosttimer = lastlogread.ToUniversalTime(); + MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost * 0.8f); + MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost * 0.8f); + } + + packetSeemValid = false; + //The following IF had to be splitted to accomodate the new place of Readlock end. + //packetSeemValid helps it. + if ((message.header == MAVLINK_STX_MAVLINK1 || + message.header == MAVLINK_STX) && + buffer.Length >= message.payloadlength) + { + packetSeemValid = true; + // check if we lost pacakets based on seqno + int expectedPacketSeqNo = ((MAVlist[sysid, compid].recvpacketcount + 1) % 0x100); + + { + // the second part is to work around a 3dr radio bug sending dup seqno's + if (packetSeqNo != expectedPacketSeqNo && + packetSeqNo != MAVlist[sysid, compid].recvpacketcount) + { + MAVlist[sysid, compid].synclost++; // actual sync loss's + int numLost = 0; + + if (packetSeqNo < ((MAVlist[sysid, compid].recvpacketcount + 1))) + // recvpacketcount = 255 then 10 < 256 = true if was % 0x100 this would fail + { + numLost = 0x100 - expectedPacketSeqNo + packetSeqNo; + } + else + { + numLost = packetSeqNo - expectedPacketSeqNo; + } + + MAVlist[sysid, compid].packetslost += numLost; + WhenPacketLost.OnNext(numLost); + + if (!logreadmode) + log.InfoFormat("mav {2}-{4} seqno {0} exp {3} pkts lost {1}", packetSeqNo, + numLost, + sysid, + expectedPacketSeqNo, compid); + } + + MAVlist[sysid, compid].packetsnotlost++; + + //Console.WriteLine("{0} {1}", sysid, packetSeqNo); + + MAVlist[sysid, compid].recvpacketcount = packetSeqNo; + } + WhenPacketReceived.OnNext(1); + + // packet stats per mav + if (!MAVlist[sysid, compid].packetspersecond.ContainsKey(msgid) || + double.IsInfinity(MAVlist[sysid, compid].packetspersecond[msgid])) + MAVlist[sysid, compid].packetspersecond[msgid] = 0; + if (!MAVlist[sysid, compid].packetspersecondbuild.ContainsKey(msgid)) + MAVlist[sysid, compid].packetspersecondbuild[msgid] = DateTime.UtcNow; + + MAVlist[sysid, compid].packetspersecond[msgid] = (((1000 / + ((DateTime.UtcNow - + MAVlist[sysid, compid] + .packetspersecondbuild[msgid]) + .TotalMilliseconds) + + MAVlist[sysid, compid].packetspersecond[ + msgid]) / 2)); + + MAVlist[sysid, compid].packetspersecondbuild[msgid] = DateTime.UtcNow; + } + //Console.WriteLine("Packet {0}",temp[5]); + // store packet history + + + //ReadLock Release moved here to include packet seqence checking. + //Releasing the lock earlier casued packet sequence errors, when readPacketAsync() was called repeatedly. + //Especially during retries. + + } // end readlock + finally + { + readlock.Release(); + } + + try + { + //The packet header checking IF had to be splitted to allow moving the Readlock try/catch. + if (packetSeemValid) + { + MAVlist[sysid, compid].addPacket(message); + + // adsb packets are forwarded and can be from any sysid/compid + if (msgid == (byte)MAVLINK_MSG_ID.ADSB_VEHICLE) + { + var adsb = message.ToStructure(); + + var id = adsb.ICAO_address.ToString("X5"); + + if (_UpdateADSBPlanePosition != null) + _UpdateADSBPlanePosition(this, new adsb.PointLatLngAltHdg( + adsb.lat / 1e7, + adsb.lon / 1e7, + adsb.altitude / 1000, + adsb.heading * 0.01f, + adsb.hor_velocity, + id, + DateTime.Now + ) + { + CallSign = Encoding.UTF8.GetString(adsb.callsign), + Squawk = adsb.squawk, + Raw = adsb, + VerticalSpeed = adsb.ver_velocity, + } + ); + } + + if (msgid == (byte)MAVLINK_MSG_ID.COLLISION) + { + var coll = message.ToStructure(); + + var id = coll.id.ToString("X5"); + + var coll_type = (MAV_COLLISION_SRC)coll.src; + + var action = (MAV_COLLISION_ACTION)coll.action; + + if (action > MAV_COLLISION_ACTION.REPORT) + { + // we are reacting to a threat + } + + var threat_level = (MAV_COLLISION_THREAT_LEVEL)coll.threat_level; + + _UpdateADSBCollision?.Invoke(this, (id, threat_level)); + } + + // set seens sysid's based on hb packet - this will hide 3dr radio packets + if (msgid == (uint)MAVLINK_MSG_ID.UAVCAN_NODE_STATUS) + { + var cannode = message.ToStructure(); + + // add a seen sysid + if (!MAVlist.Contains(sysid, compid, false)) + { + // ensure its set from connect or log playback + MAVlist.Create(sysid, compid); + MAVlist[sysid, compid].aptype = MAV_TYPE.ONBOARD_CONTROLLER; + MAVlist[sysid, compid].apname = MAV_AUTOPILOT.INVALID; + MAVlist[sysid, compid].CANNode = true; + setAPType(sysid, compid); + + // new device, so request node info + doCommand(sysid, compid, MAV_CMD.UAVCAN_GET_NODE_INFO, 0, 0, 0, 0, 0, 0, 0, false); + } + } + + if (msgid == (uint)MAVLINK_MSG_ID.UAVCAN_NODE_INFO) + { + var cannode = message.ToStructure(); + + var name = Encoding.UTF8.GetString(cannode.name); + + MAVlist[sysid, compid].VersionString = name; + + MAVlist[sysid, compid].SoftwareVersions = + cannode.sw_version_major + "." + cannode.sw_version_minor; + } + + // set seens sysid's based on hb packet - this will hide 3dr radio packets ( which send a RADIO_STATUS, but not a HEARTBEAT ) + if (msgid == (byte)MAVLINK_MSG_ID.HEARTBEAT) + { + mavlink_heartbeat_t hb = message.ToStructure(); + + // not a gcs + //if (hb.type != (byte)MAV_TYPE.GCS) + { + // add a seen sysid + if (!MAVlist.Contains(sysid, compid, false)) + { + // ensure its set from connect or log playback + MAVlist.Create(sysid, compid); + MAVlist[sysid, compid].aptype = (MAV_TYPE)hb.type; + MAVlist[sysid, compid].apname = (MAV_AUTOPILOT)hb.autopilot; + setAPType(sysid, compid); + _MAVDetected?.Invoke(this, (sysid, compid)); + } + + // attach to the only remote device. / default to first device seen + if (MAVlist.Count == 1) + { + // set it private as compidset will trigger new mavstate + _sysidcurrent = sysid; + compidcurrent = compid; + } + } + } + + //treat a high latency packet as a HB + if (msgid == (byte)MAVLINK_MSG_ID.HIGH_LATENCY2) + { + mavlink_high_latency2_t hl = message.ToStructure(); + + // not a gcs + //if (hb.type != (byte)MAV_TYPE.GCS) + { + // add a seen sysid + if (!MAVlist.Contains(sysid, compid, false)) + { + // ensure its set from connect or log playback + MAVlist.Create(sysid, compid); + MAVlist[sysid, compid].aptype = (MAV_TYPE)hl.type; + MAVlist[sysid, compid].apname = (MAV_AUTOPILOT)hl.autopilot; + MAVlist[sysid, compid].isHighLatency = true; + setAPType(sysid, compid); + _MAVDetected?.Invoke(this, (sysid, compid)); + } + + // attach to the only remote device. / default to first device seen + if (MAVlist.Count == 1) + { + // set it private as compidset will trigger new mavstate + _sysidcurrent = sysid; + compidcurrent = compid; + } + } + } + + // process for all mavs, filtering done inside - subscription handler + PacketReceived(message); + + _OnPacketReceived?.Invoke(this, message); + + if (debugmavlink) + DebugPacket(message); + + if (msgid == (byte)MAVLINK_MSG_ID.STATUSTEXT) // status text + { + var msg = message.ToStructure(); + + byte sev = msg.severity; + + string logdata = Encoding.UTF8.GetString(msg.text); + int ind = logdata.IndexOf('\0'); + if (ind != -1) + logdata = logdata.Substring(0, ind); + log.Info(DateTime.Now + " " + sev + " " + logdata); + + MAVlist[sysid, compid].cs.messages.Add((DateTime.Now, logdata)); + + // cap list at 1000 element + while (MAVlist[sysid, compid].cs.messages.Count > 1000) + MAVlist[sysid, compid].cs.messages.RemoveAt(0); + + // gymbals etc are a child/slave to the main sysid, this displays the children messages under the current displayed vehicle + if (sysid == sysidcurrent && compid != compidcurrent) + MAVlist[sysidcurrent, compidcurrent].cs.messages + .Add((DateTime.Now, compid + " : " + logdata)); + + bool printit = false; + + MAV_SEVERITY mavsev = MAV_SEVERITY.EMERGENCY; + if (sev <= (byte)Settings.Instance.GetInt32("severity", 4)) + { + printit = true; + mavsev = (MAV_SEVERITY)sev; + } + + if (logdata.StartsWith("Tuning:") || logdata.StartsWith("PreArm:") || + logdata.StartsWith("Arm:")) + printit = true; + + if (printit) + { + // high severity msg publish to current selected mav + if (sysid == sysidcurrent && compid != compidcurrent) + { + MAVlist[sysid, compidcurrent].cs.messageHigh = compid + " : " + logdata; + MAVlist[sysid, compidcurrent].cs.messageHighSeverity = mavsev; + } + + MAVlist[sysid, compid].cs.messageHigh = logdata; + MAVlist[sysid, compid].cs.messageHighSeverity = mavsev; + + if (Speech != null && + Speech.IsReady && + Settings.Instance["speechenable"] != null && + Settings.Instance["speechenable"].ToString() == "True") + { + if (speechenabled && !logdata.StartsWith("PreArm:")) // Suppress audible PreArm messages + Speech.SpeakAsync(logdata); + } + } + } + + if (Settings.Instance["autoParamCommit"] == null || + Settings.Instance.GetBoolean("autoParamCommit") == true) + { + if (lastparamset != DateTime.MinValue && lastparamset.AddSeconds(10) < DateTime.UtcNow) + { + lastparamset = DateTime.MinValue; + + if (BaseStream.IsOpen) + { + doCommand((byte)sysidcurrent, (byte)compidcurrent, MAV_CMD.PREFLIGHT_STORAGE, 1, 0, 0, + 0, 0, 0, 0, + false); + } + } + } + + ProcessMirrorStream(buffer); + } + } + catch (Exception ex) + { + log.Error(ex); + } + + // update last valid packet receive time + MAVlist[sysid, compid].lastvalidpacket = DateTime.UtcNow; + + return message; + } + + private void ProcessMirrorStream(byte[] buffer) + { + foreach (var Mirror in Mirrors) + { + var MirrorStream = Mirror.MirrorStream; + try + { + // full rw from mirror stream + if (MirrorStream != null && MirrorStream.IsOpen) + { + MirrorStream.Write(buffer, 0, buffer.Length); + + while (MirrorStream.BytesToRead > 0) + { + var len = MirrorStream.BytesToRead; + + byte[] buf = new byte[len]; + + len = MirrorStream.Read(buf, 0, len); + + if (Mirror.MirrorStreamWrite) + lock (writelock) + { + BaseStream.Write(buf, 0, len); + + if (rawlogfile != null && rawlogfile.CanWrite) + rawlogfile.Write(buf, 0, len); + } + } + } + + } + catch + { + } + } + } + + private bool CheckSignature(byte[] AuthKey, MAVLinkMessage message, byte sysid, byte compid) + { + bool valid; + using (SHA256CryptoServiceProvider signit = new SHA256CryptoServiceProvider()) + { + signit.TransformBlock(AuthKey, 0, AuthKey.Length, null, 0); + signit.TransformFinalBlock(message.buffer, 0, message.Length - MAVLINK_SIGNATURE_BLOCK_LEN + 7); + var ctx = signit.Hash; + + //Console.WriteLine("rec linkid {0}, time {1} {2} {3} {4} {5} {6} {7}", message.sig[0], message.sig[1], message.sig[2], message.sig[3], message.sig[4], message.sig[5], message.sig[6], message.sigTimestamp); + + valid = ByteArrayCompare(new ReadOnlySpan(ctx, 0, 6), new ReadOnlySpan(message.sig, 7, 6)); + + if (!valid) + return valid; + + // got valid key + MAVlist[sysid, compid].linkid = message.sig[0]; + + MAVlist[sysid, compid].signingKey = AuthKey; + + enableSigning(sysid, compid); + + return valid; + } + } + + private void PacketReceived(MAVLinkMessage buffer) + { + MAVLINK_MSG_ID type = (MAVLINK_MSG_ID) buffer.msgid; + (MAVLINK_MSG_ID msgId, Func function, bool exclusive, byte sysid, byte compid)[] list; + + // lock to grab the list, because this is public, to prevent a recursion + lock (Subscriptions) + { + list = Subscriptions.ToArray(); + } + + foreach (var item in list) + { + //exact match, and 0,0 = current primary + if (item.msgId == type && buffer.sysid == item.sysid && buffer.compid == item.compid || + item.msgId == type && item.sysid == 0 && item.compid == 0 && buffer.sysid == sysidcurrent && buffer.compid == compidcurrent) + { + try + { + item.function(buffer); + } + catch (Exception ex) + { + log.Error(ex); + } + } + } + } + + readonly private List<(MAVLINK_MSG_ID msgId, Func function, bool exclusive, byte sysid, byte compid)> Subscriptions = + new List<(MAVLINK_MSG_ID, Func, bool, byte, byte)>(); + + /// + /// Subscribe to a packet on the current target MAV. use OnPacketReceived to get all MAVs + /// + /// + /// + /// + /// + public int SubscribeToPacketType(MAVLINK_MSG_ID msgid, + Func function, byte sysid, byte compid, bool exclusive = false) + { + log.Info($"SubscribeToPacketType {msgid} {function} {exclusive} {sysid} {compid}"); + + var item = (msgid, function, exclusive, sysid, compid); + + lock (Subscriptions) + { + if (exclusive) + { + foreach (var subitem in Subscriptions) + { + if (subitem.msgId == item.msgid) + { + Subscriptions.Remove(subitem); + break; + } + } + } + + log.Debug("SubscribeToPacketType " + item.msgid + " " + item.function); + + Subscriptions.Add(item); + } + + return item.GetHashCode(); + } + + public void UnSubscribeToPacketType(int id) + { + lock (Subscriptions) + { + log.Debug("UnSubscribeToPacketType " + id); + var sub = Subscriptions.ToArray(); + + foreach (var valueTuple in sub) + { + if (id == valueTuple.GetHashCode()) + { + Subscriptions.Remove(valueTuple); + break; + } + } + } + } + + + + /// + /// Processes a MAVLink packet received from the stream and updates cached state for the + /// addressed vehicle/component. This includes tracking mission/fence/rally and parameter + /// data from sent or received traffic, and may also trigger protocol side effects such as + /// sending responses for messages that require them (for example, TIMESYNC handling). + /// + /// The MAVLink packet to process. Its contents may be used to update local mission/parameter state and may cause protocol responses to be sent. + /// The system id associated with the packet when resolving vehicle state updates. + /// The component id associated with the packet when resolving vehicle state updates. + private void processInfoFromStream(ref MAVLinkMessage buffer, byte sysid, byte compid) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_COUNT) + { + // clear old + mavlink_mission_count_t wp = buffer.ToStructure(); + + if (wp.target_system == gcssysid) + { + wp.target_system = sysid; + wp.target_component = compid; + } + + if (wp.mission_type == (byte) MAV_MISSION_TYPE.MISSION) + MAVlist[wp.target_system, wp.target_component].wps.Clear(); + if (wp.mission_type == (byte) MAV_MISSION_TYPE.FENCE) + MAVlist[wp.target_system, wp.target_component].fencepoints.Clear(); + if (wp.mission_type == (byte) MAV_MISSION_TYPE.RALLY) + MAVlist[wp.target_system, wp.target_component].rallypoints.Clear(); + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM) + { + mavlink_mission_item_t wp = buffer.ToStructure(); + + if (wp.target_system == gcssysid) + { + wp.target_system = sysid; + wp.target_component = compid; + } + + if (wp.current == 2) + { + // guide mode wp + if (wp.mission_type == (byte) MAV_MISSION_TYPE.MISSION) + MAVlist[wp.target_system, wp.target_component].GuidedMode = (Locationwp) wp; + } + else + { + if (wp.mission_type == (byte) MAV_MISSION_TYPE.MISSION) + MAVlist[wp.target_system, wp.target_component].wps[wp.seq] = (Locationwp) wp; + if (wp.mission_type == (byte) MAV_MISSION_TYPE.FENCE) + MAVlist[wp.target_system, wp.target_component].fencepoints[wp.seq] = (Locationwp) wp; + if (wp.mission_type == (byte) MAV_MISSION_TYPE.RALLY) + MAVlist[wp.target_system, wp.target_component].rallypoints[wp.seq] = (Locationwp) wp; + } + + //Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command); + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM_INT) + { + mavlink_mission_item_int_t wp = buffer.ToStructure(); + + if (wp.target_system == gcssysid) + { + wp.target_system = sysid; + wp.target_component = compid; + } + + if (wp.current == 2) + { + // guide mode wp + if (wp.mission_type == (byte) MAV_MISSION_TYPE.MISSION) + MAVlist[wp.target_system, wp.target_component].GuidedMode = (Locationwp) wp; + } + else + { + if (wp.mission_type == (byte) MAV_MISSION_TYPE.MISSION) + MAVlist[wp.target_system, wp.target_component].wps[wp.seq] = wp; + if (wp.mission_type == (byte) MAV_MISSION_TYPE.FENCE) + MAVlist[wp.target_system, wp.target_component].fencepoints[wp.seq] = wp; + if (wp.mission_type == (byte) MAV_MISSION_TYPE.RALLY) + MAVlist[wp.target_system, wp.target_component].rallypoints[wp.seq] = wp; + } + + //Console.WriteLine("WP INT # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command); + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.HOME_POSITION) + { + var home = buffer.ToStructure(); + + var loc = new PointLatLngAlt(home.latitude / 1.0e7, home.longitude / 1.0e7, home.altitude / 1000.0); + + MAVlist[buffer.sysid, buffer.compid].cs.HomeLocation = loc; + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT) + { + mavlink_set_position_target_global_int_t setpos = + buffer.ToStructure(); + + MAVlist[setpos.target_system, setpos.target_component].GuidedMode = (Locationwp) setpos; + + //Console.WriteLine("SET_POSITION_TARGET_GLOBAL_INT x {0} y {1} z {2} frame {3}", setpos.lat_int/1e7, setpos.lon_int/1e7, setpos.alt, setpos.coordinate_frame); + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.RALLY_POINT) + { + mavlink_rally_point_t rallypt = buffer.ToStructure(); + + if (rallypt.target_system == gcssysid) + { + rallypt.target_system = sysid; + rallypt.target_component = compid; + } + + MAVlist[rallypt.target_system, rallypt.target_component].rallypoints[rallypt.idx] = + new mavlink_mission_item_int_t(0, 0, 0, 0, rallypt.lat, rallypt.lng, rallypt.alt, rallypt.idx, + (ushort) MAV_CMD.RALLY_POINT, rallypt.target_system, rallypt.target_component, + (byte) MAV_FRAME.GLOBAL_RELATIVE_ALT, 0, 0, (byte) MAV_MISSION_TYPE.RALLY); + + //Console.WriteLine("RP # {0} {1} {2} {3} {4}", rallypt.idx, rallypt.lat, rallypt.lng, rallypt.alt, rallypt.break_alt); + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.CAMERA_FEEDBACK) + { + mavlink_camera_feedback_t camerapt = buffer.ToStructure(); + + if (MAVlist[sysid, compid].camerapoints.Count == 0 || + MAVlist[sysid, compid].camerapoints.Last().time_usec != camerapt.time_usec) + { + MAVlist[sysid, compid].camerapoints.RemoveAll(a => + a.cam_idx * 256 + a.img_idx == camerapt.cam_idx * 256 + camerapt.img_idx); + MAVlist[sysid, compid].camerapoints.Add(camerapt); + } + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.FENCE_POINT) + { + mavlink_fence_point_t fencept = buffer.ToStructure(); + + if (fencept.target_system == gcssysid) + { + fencept.target_system = sysid; + fencept.target_component = compid; + } + + MAVlist[fencept.target_system, fencept.target_component].fencepoints[fencept.idx] = + new mavlink_mission_item_int_t(fencept.count - 1, 0, 0, 0, (int) (fencept.lat * 1e7), + (int) (fencept.lng * 1e7), 0, fencept.idx, + fencept.idx == 0 + ? (ushort) MAV_CMD.FENCE_RETURN_POINT + : (ushort) MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION, + fencept.target_system, fencept.target_component, (byte) MAV_FRAME.GLOBAL_RELATIVE_ALT, 0, 0, + (byte) MAV_MISSION_TYPE.FENCE); + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE) + { + mavlink_param_value_t value = buffer.ToStructure(); + + string st = Encoding.UTF8.GetString(value.param_id); + + int pos = st.IndexOf('\0'); + + if (pos != -1) + { + st = st.Substring(0, pos); + } + + lock(MAVlist[sysid, compid].param_types) + MAVlist[sysid, compid].param_types[st] = (MAV_PARAM_TYPE) value.param_type; + + if (MAVlist[sysid, compid].apname == MAV_AUTOPILOT.ARDUPILOTMEGA && + buffer.compid != (byte) MAV_COMPONENT.MAV_COMP_ID_UDP_BRIDGE) + { + var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); + MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, BitConverter.GetBytes(value.param_value), + MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE) value.param_type); + } + else + { + var offset = Marshal.OffsetOf(typeof(mavlink_param_value_t), "param_value"); + MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, BitConverter.GetBytes(value.param_value), + (MAV_PARAM_TYPE) value.param_type, (MAV_PARAM_TYPE) value.param_type); + } + + if (value.param_count != ushort.MaxValue) + MAVlist[sysid, compid].param.TotalReported = value.param_count; + } + else if (buffer.msgid == (byte) MAVLINK_MSG_ID.TIMESYNC) + { + Int64 now_ns = + (Int64) ((DateTime.Now - new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc)).TotalMilliseconds * + 1000000); + + mavlink_timesync_t tsync = buffer.ToStructure(); + // tc1 - client + // ts1 - server + + // system does not know the time + if (tsync.tc1 == 0) + { + tsync.tc1 = now_ns; + if (BaseStream != null && BaseStream.IsOpen) + sendPacket(tsync, buffer.sysid, buffer.compid); + } // system knows the time + else if (tsync.tc1 > 0) + { + Int64 offset_ns = (tsync.ts1 + now_ns - tsync.tc1 * 2) / 2; + Int64 dt = MAVlist[buffer.sysid, buffer.compid].time_offset_ns - offset_ns; + + if (Math.Abs(dt) > 10000000) // 10 millisecond skew + { + MAVlist[buffer.sysid, buffer.compid].time_offset_ns = offset_ns; // hard-set it. + } + else + { + var offset_avg_alpha = 0.6; + var avg = (offset_avg_alpha * offset_ns) + + (1.0 - offset_avg_alpha) * MAVlist[buffer.sysid, buffer.compid].time_offset_ns; + MAVlist[buffer.sysid, buffer.compid].time_offset_ns = (long) avg; + } + } + } + } + + [Obsolete] + public bool getVersion(bool responcerequired = true) + { + return getVersion(MAV.sysid, MAV.compid, responcerequired); + } + + public bool getVersion(byte sysid, byte compid, bool responcerequired = true) + { + return getVersionAsync(sysid, compid, responcerequired).AwaitSync(); + } + + public async Task getVersionAsync(byte sysid, byte compid, bool responcerequired = true) + { + mavlink_autopilot_version_request_t req = new mavlink_autopilot_version_request_t(); + + req.target_component = compid; + req.target_system = sysid; + + // use *all three* methods + doCommand(MAVLink.MAV_CMD.REQUEST_MESSAGE, + (float)MAVLink.MAVLINK_MSG_ID.AUTOPILOT_VERSION, + 0, 0, 0, 0, 0, 0, false); + + // MAV_CMD.REQUEST_AUTOPILOT_CAPABILITIES is deprecated + doCommand(MAV_CMD.REQUEST_AUTOPILOT_CAPABILITIES, 0, 0, 0, 0, 0, 0, 0, false); + + // AUTOPILOT_VERSION_REQUEST is deprecated + generatePacket((byte) MAVLINK_MSG_ID.AUTOPILOT_VERSION_REQUEST, req); + + if (!responcerequired) + return true; + + var responce = false; + + var sub = SubscribeToPacketType(MAVLINK_MSG_ID.AUTOPILOT_VERSION, (buffer) => + { + if (buffer.msgid == (byte)MAVLINK_MSG_ID.AUTOPILOT_VERSION && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + responce = true; + return true; + } + return true; + }, (byte)sysid, (byte)compid); + + DateTime start = DateTime.Now; + int retrys = 3; + + while (!responce) + { + if (!(start.AddMilliseconds(500) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("getVersion Retry " + retrys); + generatePacket((byte)MAVLINK_MSG_ID.AUTOPILOT_VERSION_REQUEST, req); + start = DateTime.Now; + retrys--; + continue; + } + + break; + } + + await Task.Delay((100)); + } + + UnSubscribeToPacketType(sub); + + return responce; + } + + [Obsolete] + public async Task<(PointLatLngAlt plla, int total)> getFencePoint(int no) + { + MAVLinkMessage buffer; + + giveComport = true; + + PointLatLngAlt plla = new PointLatLngAlt(); + mavlink_fence_fetch_point_t req = new mavlink_fence_fetch_point_t(); + + req.idx = (byte) no; + req.target_component = MAV.compid; + req.target_system = MAV.sysid; + + // request point + generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req); + + DateTime start = DateTime.Now; + int retrys = 3; + + while (true) + { + if (!(start.AddMilliseconds(700) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("getFencePoint Retry " + retrys + " - giv com " + giveComport); + generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req); + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - getFencePoint"); + } + + buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.FENCE_POINT && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + mavlink_fence_point_t fp = buffer.ToStructure(); + + // check this gcs sent it + if (fp.target_system != gcssysid || + fp.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER || + fp.idx != no) + continue; + + giveComport = false; + + plla.Lat = fp.lat; + plla.Lng = fp.lng; + plla.Tag = fp.idx.ToString(); + + return (plla, fp.count); + } + } + } + } + + [Obsolete] + public async Task GetLog(ushort no) + { + return new FileStream(await GetLog(MAV.sysid, MAV.compid, no), FileMode.Open, FileAccess.ReadWrite); + } + + /// GetLog: ms of LOG_DATA silence before the request is retried + internal int LogDataTimeoutMs { get; set; } = 3000; + + /// GetLog: ms of LOG_DATA silence before missing blocks are re-requested + internal int LogDataRefetchMs { get; set; } = 500; + + public Task GetLog(byte sysid, byte compid, ushort no, CancellationToken cancel = default) + { + return GetLogInternal(sysid, compid, no, Path.GetTempFileName(), cancel); + } + + /// + /// Download log into . The file is + /// created (or truncated), and deleted again on failure or cancellation. + /// + internal async Task GetLogInternal(byte sysid, byte compid, ushort no, string filename, CancellationToken cancel) + { + ConcurrentQueue queue = new ConcurrentQueue(); + SemaphoreSlim queueSignal = new SemaphoreSlim(0); + EventHandler handler = (sender, msg) => + { + // filter on the header fields here so unrelated traffic - e.g. + // another vehicle's log stream on the same link - cannot grow + // the queue or reset the silence timers; the log id needs a + // payload parse and stays checked on the consumer side + if (msg.msgid == (byte) MAVLINK_MSG_ID.LOG_DATA && msg.sysid == sysid && + msg.compid == compid) + { + queue.Enqueue(msg); + try + { + queueSignal.Release(); + } + catch (ObjectDisposedException) + { + // packet arrived after this download already finished + } + } + }; + OnPacketReceived += handler; + + // formally end the log session; best effort - the link may already be gone + void SendLogRequestEnd() + { + try + { + var end = new mavlink_log_request_end_t + { + target_system = sysid, + target_component = compid + }; + generatePacket((byte) MAVLINK_MSG_ID.LOG_REQUEST_END, end); + } + catch + { + } + } + + try + { + using (FileStream ms = new FileStream(filename, FileMode.Create, FileAccess.ReadWrite)) + { + // received 90-byte block numbers; every block below lowestMissing is known received + HashSet set = new HashSet(); + uint lowestMissing = 0; + // how far past the contiguous frontier a packet may sit and still raise + // the end-of-log bar; genuine streams are near-contiguous + const uint endDetectionSlack = 90 * 100; + + giveComport = false; + MAVLinkMessage buffer = MAVLinkMessage.Invalid; + + if (Progress != null) + { + Progress((int) 0, ""); + } + + uint totallength = 0; + uint maxEnd = 0; + // end-of-log candidate from a short packet past the trusted window, + // kept only while nothing arrives after it + uint pendingEnd = 0; + uint ofs = 0; + uint bps = 0; + DateTime bpstimer = DateTime.Now; + + mavlink_log_request_data_t req = new mavlink_log_request_data_t(); + + req.target_component = compid; + req.target_system = sysid; + req.id = no; + req.ofs = ofs; + // entire log + req.count = 0xFFFFFFFF; + + // request point + generatePacket((byte) MAVLINK_MSG_ID.LOG_REQUEST_DATA, req); + + int retrys = 3; + + while (true) + { + // park until the handler queues a LOG_DATA packet + // false means LogDataTimeoutMs of silence - resend the request + if (!await queueSignal.WaitAsync(LogDataTimeoutMs, cancel).ConfigureAwait(false)) + { + // the stream went quiet right after an end candidate from past the + // trusted window - that silence is the missing evidence: a corrupt + // packet is followed by more stream, the real end of log is not. + // accept it and let the fill-in phase repair the gaps + if (pendingEnd > 0) + { + totallength = Math.Max(pendingEnd, maxEnd); + log.Info("start fillin len " + totallength + " count " + set.Count + + " (end past stalled frontier)"); + break; + } + + if (retrys > 0) + { + log.Info("GetLog Retry " + retrys + " - giv com " + giveComport); + generatePacket((byte) MAVLINK_MSG_ID.LOG_REQUEST_DATA, req); + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - GetLog"); + } + + if (!queue.TryDequeue(out buffer)) + continue; + + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.LOG_DATA && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + var data = buffer.ToStructure(); + + if (data.id != no) + continue; + + // a corrupt count would overrun the fixed 90-byte payload + if (data.count > data.data.Length) + continue; + + bps += data.count; + + // record what we have received + bool newBlock = set.Add(data.ofs / 90); + while (set.Contains(lowestMissing)) + lowestMissing++; + // only fresh data earns more patience - a packet resent every + // retry window must not keep the streaming phase alive forever + if (newBlock) + retrys = 3; + // only packets near the contiguous frontier are trusted with + // protocol state - a corrupt far offset must not raise the + // end-of-log bar, move the retry offset or end the download + bool nearFrontier = data.ofs <= (ulong) lowestMissing * 90 + endDetectionSlack; + if (nearFrontier) + maxEnd = Math.Max(maxEnd, data.ofs + data.count); + + if (ms.Position != data.ofs) + ms.Seek((long) data.ofs, SeekOrigin.Begin); + ms.Write(data.data, 0, data.count); + + // update new start point + if (nearFrontier) + req.ofs = data.ofs + data.count; + + if (bpstimer.Second != DateTime.Now.Second) + { + if (Progress != null) + { + Progress((int) req.ofs, ""); + } + + //Console.WriteLine("log dl bps: " + bps.ToString()); + bpstimer = DateTime.Now; + bps = 0; + } + + // a short or empty packet ends the log, but only trust it at the + // highest offset seen - a reordered short packet must not truncate + // the download + if (data.count < 90 && data.ofs + data.count >= maxEnd) + { + if (nearFrontier) + { + totallength = data.ofs + data.count; + log.Info("start fillin len " + totallength + " count " + set.Count + + " datalen " + data.count); + break; + } + + // past the trusted window it clears the bar trivially, so it is + // only a candidate: packet loss can stall the frontier far behind + // a genuine end, but a corrupt packet short by chance looks the + // same. defer to the silence check - the largest of a final run + // of candidates, so a smaller corrupt one cannot truncate the log + pendingEnd = Math.Max(pendingEnd, data.ofs + data.count); + } + else + { + // the stream continued, so any prior end candidate was corrupt + pendingEnd = 0; + } + } + } + } + + // blocks 0..totalBlocks-1 must all be present before the download is complete + uint totalBlocks = (totallength + 89) / 90; + + log.Info("set count " + set.Count); + log.Info("count total " + totalBlocks); + log.Info("totallength " + totallength); + log.Info("current length " + ms.Length); + + // a corrupt far-offset packet in the streaming phase may have grown the + // file past the log - the returned file must not be longer than the log + if (ms.Length > totallength) + ms.SetLength(totallength); + + // request the first run of still-missing blocks + void RequestFirstMissing() + { + if (lowestMissing >= totalBlocks) + return; + + // request large chunk if they are back to back + uint bytereq = 90; + uint b = lowestMissing + 1; + while (b < totalBlocks && !set.Contains(b)) + { + bytereq += 90; + b++; + } + + req.ofs = lowestMissing * 90; + req.count = bytereq; + log.Info("req missing " + req.ofs + " bytes " + req.count + " got " + set.Count + "/" + + totalBlocks); + generatePacket((byte) MAVLINK_MSG_ID.LOG_REQUEST_DATA, req); + } + + // the same total silence budget as the streaming phase - a vehicle + // that stops answering fill-in requests must not hang the download + int dryWindows = 0; + int maxDryWindows = Math.Max(1, LogDataTimeoutMs * 3 / Math.Max(1, LogDataRefetchMs)); + + while ((BaseStream != null && BaseStream.IsOpen) || logreadmode) + { + if (ms.Length >= totallength && lowestMissing >= totalBlocks) + { + giveComport = false; + SendLogRequestEnd(); + return filename; + } + + // park until more data arrives; LogDataRefetchMs of silence means + // the current request ran dry - ask for what is still missing + if (!await queueSignal.WaitAsync(LogDataRefetchMs, cancel).ConfigureAwait(false)) + { + if (++dryWindows > maxDryWindows) + { + giveComport = false; + throw new TimeoutException("Timeout on read - GetLog fill-in"); + } + + RequestFirstMissing(); + continue; + } + + dryWindows = 0; + + if (!queue.TryDequeue(out buffer)) + continue; + + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.LOG_DATA && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + var data = buffer.ToStructure(); + + if (data.id != no) + continue; + + // a corrupt count would overrun the fixed 90-byte payload + if (data.count > data.data.Length) + continue; + + // fill-in data must stay inside the log bounds established by + // the streaming phase - a bogus offset must not extend the file + if ((ulong) data.ofs + data.count > totallength) + continue; + + bps += data.count; + + // record what we have received + var newBlock = set.Add(data.ofs / 90); + while (set.Contains(lowestMissing)) + lowestMissing++; + + ms.Seek((long) data.ofs, SeekOrigin.Begin); + ms.Write(data.data, 0, data.count); + + // update new start point + req.ofs = data.ofs + data.count; + + if (bpstimer.Second != DateTime.Now.Second) + { + if (Progress != null) + { + Progress((int) req.ofs, ""); + } + + //Console.WriteLine("log dl bps: " + bps.ToString()); + bpstimer = DateTime.Now; + bps = 0; + } + + // this fill run has caught up to data we already hold - + // request the next missing run now instead of waiting + // out the 500ms silence. only a newly received block may + // trigger this: stale or duplicated packets must not each + // fire another request on a duplicating link + if (newBlock && set.Contains((data.ofs / 90) + 1)) + RequestFirstMissing(); + } + } + } + + throw new Exception("Failed to get log"); + } + } + catch + { + // tell the vehicle to stop streaming - an abandoned download otherwise + // keeps LOG_DATA flowing and breaks the next log operation + SendLogRequestEnd(); + + // don't leave a partial file behind on timeout/cancel/link loss + try + { + File.Delete(filename); + } + catch + { + } + + throw; + } + finally + { + OnPacketReceived -= handler; + queueSignal.Dispose(); + } + } + + [Obsolete] + public List GetLogList() + { + List ans = new List(); + + return GetLogEntry(0, ushort.MaxValue).AwaitSync().Values + .OrderBy(a => a.id).ToList(); + } + + [Obsolete] + public void GetMountStatus() + { + mavlink_mount_status_t req = new mavlink_mount_status_t(); + req.target_component = MAV.compid; + req.target_system = MAV.sysid; + + generatePacket((byte) MAVLINK_MSG_ID.MOUNT_STATUS, req); + } + + [Obsolete] + public async Task> GetLogEntry(ushort startno = 0, + ushort endno = ushort.MaxValue) + { + giveComport = true; + MAVLinkMessage buffer; + + Dictionary ans = new Dictionary(); + + mavlink_log_request_list_t req = new mavlink_log_request_list_t(); + + req.target_component = MAV.compid; + req.target_system = MAV.sysid; + req.start = startno; + req.end = endno; + + log.Info("GetLogEntry " + startno + "-" + endno); + + // request point + generatePacket((byte) MAVLINK_MSG_ID.LOG_REQUEST_LIST, req); + + DateTime start = DateTime.Now; + int retrys = 4; + + while (true) + { + if (!(start.AddMilliseconds(5000) > DateTime.Now)) + { + if (retrys > 0) + { + req.start = startno; + req.end = endno; + log.Info("GetLogEntry Retry " + retrys + " - giv com " + giveComport); + generatePacket((byte) MAVLINK_MSG_ID.LOG_REQUEST_LIST, req); + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - GetLogEntry"); + } + + buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.LOG_ENTRY && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + var loge = buffer.ToStructure(); + + if (loge.id >= startno && loge.id <= endno) + { + if (loge.num_logs == 0 && loge.last_log_num == 0) + { + giveComport = false; + return ans; + } + + // reset timeout + start = DateTime.Now; + // add the log to our answer + ans[loge.id] = loge; + // set the startno to our new min + startno = (ushort) Math.Min(ans.Keys.Min() + 1, loge.id); + // set the end number to logmax + endno = loge.last_log_num; + if (ans.Count >= loge.num_logs) + { + giveComport = false; + return ans; + } + } + } + + if (buffer.msgid == (byte) MAVLINK_MSG_ID.LOG_DATA && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + giveComport = false; + throw new Exception("Existing log download already in progress."); + } + } + } + } + + [Obsolete] + public void EraseLog() + { + mavlink_log_erase_t req = new mavlink_log_erase_t(); + + req.target_component = MAV.compid; + req.target_system = MAV.sysid; + + // send twice - we have no feedback on this + generatePacket((byte) MAVLINK_MSG_ID.LOG_ERASE, req); + generatePacket((byte) MAVLINK_MSG_ID.LOG_ERASE, req); + } + + [Obsolete] + public async Task> getRallyPoints() + { + List points = new List(); + + if (!MAV.param.ContainsKey("RALLY_TOTAL")) + return points; + + int count = int.Parse(MAV.param["RALLY_TOTAL"].ToString()); + + for (int a = 0; a < (count - 1); a++) + { + try + { + var plla = await getRallyPoint(a).ConfigureAwait(false); + points.Add(plla.plla); + } + catch + { + return points; + } + } + + return points; + } + + [Obsolete] + public async Task<(PointLatLngAlt plla, int total)> getRallyPoint(int no) + { + MAVLinkMessage buffer; + + giveComport = true; + + PointLatLngAlt plla = new PointLatLngAlt(); + mavlink_rally_fetch_point_t req = new mavlink_rally_fetch_point_t(); + + req.idx = (byte) no; + req.target_component = MAV.compid; + req.target_system = MAV.sysid; + + // request point + generatePacket((byte) MAVLINK_MSG_ID.RALLY_FETCH_POINT, req); + + DateTime start = DateTime.Now; + int retrys = 3; + + while (true) + { + if (!(start.AddMilliseconds(700) > DateTime.Now)) + { + if (retrys > 0) + { + log.Info("getRallyPoint Retry " + retrys + " - giv com " + giveComport); + generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req); + start = DateTime.Now; + retrys--; + continue; + } + + giveComport = false; + throw new TimeoutException("Timeout on read - getRallyPoint"); + } + + buffer = await readPacketAsync().ConfigureAwait(false); + if (buffer.Length > 5) + { + if (buffer.msgid == (byte) MAVLINK_MSG_ID.RALLY_POINT && buffer.sysid == req.target_system && + buffer.compid == req.target_component) + { + mavlink_rally_point_t fp = buffer.ToStructure(); + + // check this gcs sent it + if (fp.target_system != gcssysid || + fp.target_component != (byte) MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER) + continue; + + if (req.idx != fp.idx) + { + generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req); + continue; + } + + plla.Lat = fp.lat / t7; + plla.Lng = fp.lng / t7; + plla.Tag = fp.idx.ToString(); + plla.Alt = fp.alt; + + giveComport = false; + + return (plla, fp.count); + } + } + } + } + + [Obsolete] + public bool setFencePoint(byte index, PointLatLngAlt plla, byte fencepointcount) + { + mavlink_fence_point_t fp = new mavlink_fence_point_t(); + + fp.idx = index; + fp.count = fencepointcount; + fp.lat = (float) plla.Lat; + fp.lng = (float) plla.Lng; + fp.target_component = MAV.compid; + fp.target_system = MAV.sysid; + + int retry = 3; + + while (retry > 0) + { + generatePacket((byte) MAVLINK_MSG_ID.FENCE_POINT, fp); + var newfp = getFencePoint(fp.idx).AwaitSync(); + + if (newfp.plla.GetDistance(plla) < 5) + return true; + retry--; + } + + throw new Exception("Could not verify GeoFence Point"); + } + + [Obsolete] + public bool setRallyPoint(byte index, PointLatLngAlt plla, short break_alt, UInt16 land_dir_cd, byte flags, + byte rallypointcount) + { + mavlink_rally_point_t rp = new mavlink_rally_point_t(); + + rp.idx = index; + rp.count = rallypointcount; + rp.lat = (int) (plla.Lat * t7); + rp.lng = (int) (plla.Lng * t7); + rp.alt = (short) plla.Alt; + rp.break_alt = break_alt; + rp.land_dir = land_dir_cd; + rp.flags = (byte) flags; + rp.target_component = MAV.compid; + rp.target_system = MAV.sysid; + + int retry = 3; + + while (retry > 0) + { + generatePacket((byte) MAVLINK_MSG_ID.RALLY_POINT, rp); + + var newfp = getRallyPoint(rp.idx).GetAwaiter().GetResult(); + + if (newfp.plla.Lat == plla.Lat && newfp.plla.Lng == rp.lng) + { + Console.WriteLine("Rally Set"); + return true; + } + + retry--; + } + + return false; + } + + public enum sensoroffsetsenum + { + gyro = 0, + accelerometer = 1, + magnetometer = 2, + barometer = 3, + optical_flow = 4, + second_magnetometer = 5 + } + + public bool SetSensorOffsets(byte sysid, byte compid, sensoroffsetsenum sensor, float x, float y, float z) + { + return doCommand(sysid, compid, MAV_CMD.PREFLIGHT_SET_SENSOR_OFFSETS, (int) sensor, x, y, z, 0, 0, 0); + } + + public void send_text(byte severity, string txt) + { + var temp = Encoding.ASCII.GetBytes(txt); + generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT, + new mavlink_statustext_t() {severity = (byte) severity, text = temp}); + } + + private Dictionary> streamfncache = new Dictionary>(); + private EventHandler _OnPacketReceived; + private EventHandler _OnPacketSent; + private static EventHandler _UpdateADSBPlanePosition; + private static EventHandler<(string id, MAV_COLLISION_THREAT_LEVEL threat_level)> _UpdateADSBCollision; + private EventHandler _ParamListChanged; + private EventHandler _MavChanged; + private EventHandler _CommsClose; + public bool printbps = true; + private bool _openComplete = true; + + private MAVLinkMessage readlogPacketMavlink() + { + byte[] datearray = new byte[8]; + + bool missingtimestamp = false; + + if (logplaybackfile.BaseStream is FileStream) + { + if (!streamfncache.ContainsKey(_logplaybackfile.BaseStream)) + streamfncache[_logplaybackfile.BaseStream] = new Tuple( + ((FileStream) _logplaybackfile.BaseStream).Name.ToLower(), logplaybackfile.BaseStream.Length); + + if (streamfncache[_logplaybackfile.BaseStream].Item1.EndsWith(".rlog")) + missingtimestamp = true; + } + else + { + if (!streamfncache.ContainsKey(_logplaybackfile.BaseStream)) + streamfncache[_logplaybackfile.BaseStream] = + new Tuple("", logplaybackfile.BaseStream.Length); + } + + if (!missingtimestamp) + { + int tem = logplaybackfile.BaseStream.Read(datearray, 0, datearray.Length); + + Array.Reverse(datearray); + + DateTime date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc); + + UInt64 dateint = BitConverter.ToUInt64(datearray, 0); + + try + { + // array is reversed above + if (datearray[7] == 254 || datearray[7] == 253) + { + //rewind 8bytes + logplaybackfile.BaseStream.Seek(-8, SeekOrigin.Current); + } + else + { + if ((dateint / 1000 / 1000 / 60 / 60) < 9999999) + { + date1 = date1.AddMilliseconds(dateint / 1000); + + lastlogread = date1.ToLocalTime(); + lastlogreadwall = DateTime.Now; + } + } + } + catch + { + } + } + + byte[] temp = new byte[0]; + + byte byte0 = 0; + byte byte1 = 0; + byte byte2 = 0; + + var filelength = streamfncache[_logplaybackfile.BaseStream].Item2; + var filepos = logplaybackfile.BaseStream.Position; + + if (filelength == filepos) + return MAVLinkMessage.Invalid; + + int length = 5; + int a = 0; + while (a < length) + { + if (filelength == filepos) + return MAVLinkMessage.Invalid; + + var tempb = (byte) logplaybackfile.ReadByte(); + filepos++; + + switch (a) + { + case 0: + byte0 = tempb; + if (byte0 != MAVLINK_STX_MAVLINK1 && byte0 != MAVLINK_STX) + { + log.DebugFormat("logread - lost sync byte {0} pos {1}", byte0, + logplaybackfile.BaseStream.Position); + // seek to next valid + do + { + byte0 = logplaybackfile.ReadByte(); + } while (byte0 != MAVLINK_STX_MAVLINK1 && byte0 != MAVLINK_STX); + + a = 1; + continue; + } + + break; + + case 1: + byte1 = tempb; + // handle length + { + int headerlength = byte0 == MAVLINK_STX ? 9 : 5; + int headerlengthstx = headerlength + 1; + + length = byte1 + headerlengthstx + 2; // header + 2 checksum + } + break; + + case 2: + byte2 = tempb; + // handle signing and mavlink2 + if (byte0 == MAVLINK_STX) + { + if ((byte2 & MAVLINK_IFLAG_SIGNED) > 0) + length += MAVLINK_SIGNATURE_BLOCK_LEN; + } + + // handle rest + { + temp = new byte[length]; + temp[0] = byte0; + temp[1] = byte1; + temp[2] = byte2; + + var readto = a + 1; + var readlength = length - (a + 1); + logplaybackfile.Read(temp, readto, readlength); + a = length; + } + break; + } + + a++; + } + + MAVLinkMessage tmp = new MAVLinkMessage(temp, lastlogread); + + MAVlist[tmp.sysid, tmp.compid].cs.datetime = lastlogread; + + return tmp; + } + + [Obsolete] + public bool translateMode(string modein, ref mavlink_set_mode_t mode) + { + return translateMode(MAV.sysid, MAV.compid, modein, ref mode); + } + + public bool translateMode(byte sysid, byte compid, string modein, ref mavlink_set_mode_t mode) + { + mode.target_system = sysid; + + if (modein == null || modein == "") + return false; + + try + { + List> modelist = Common.getModesList(MAVlist[sysid, compid].cs.firmware); + + foreach (KeyValuePair pair in modelist) + { + if (pair.Value.ToLower() == modein.ToLower()) + { + mode.base_mode = (byte) MAV_MODE_FLAG.CUSTOM_MODE_ENABLED; + mode.custom_mode = (uint) pair.Key; + } + } + + if (mode.base_mode == 0) + { + log.Error("No Mode Changed " + modein); + return false; + } + } + catch + { + log.Error("Failed to find Mode"); + return false; + } + + return true; + } + + public void setAPType(byte sysid, byte compid) + { + MAVlist[sysid, compid].sysid = sysid; + MAVlist[sysid, compid].compid = compid; + + switch (MAVlist[sysid, compid].apname) + { + case MAV_AUTOPILOT.ARDUPILOTMEGA: + // try and pull vehicle type from version string + if (MAVlist[sysid, compid].VersionString != null && MAVlist[sysid, compid].VersionString.Length > 1) + { + var firmware_lookup = new Dictionary() + { + {"ArduPlane V", Firmwares.ArduPlane}, + {"ArduCopter V", Firmwares.ArduCopter2}, + {"Blimp V", Firmwares.ArduCopter2}, + {"ArduRover V",Firmwares.ArduRover}, + {"ArduSub V",Firmwares.ArduSub}, + {"AntennaTracker V", Firmwares.ArduTracker} + }; + var match = firmware_lookup.Where(d => MAVlist[sysid, compid].VersionString.StartsWith(d.Key)).ToArray(); + if (match.Count() == 1) + { + MAVlist[sysid, compid].cs.firmware = match[0].Value; + break; + } + } + + switch (MAVlist[sysid, compid].aptype) + { + case MAV_TYPE.FIXED_WING: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduPlane; + break; + + case MAV_TYPE n when (n >= MAV_TYPE.VTOL_DUOROTOR && n <= MAV_TYPE.VTOL_RESERVED5): + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduPlane; + break; + + case MAV_TYPE.FLAPPING_WING: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduPlane; + break; + + case MAV_TYPE.QUADROTOR: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; + break; + + case MAV_TYPE.TRICOPTER: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; + break; + + case MAV_TYPE.HEXAROTOR: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; + break; + + case MAV_TYPE.OCTOROTOR: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; + break; + + case MAV_TYPE.HELICOPTER: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; + break; + + case MAV_TYPE.DODECAROTOR: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; + break; + + case MAV_TYPE.GROUND_ROVER: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduRover; + break; + + case MAV_TYPE.SURFACE_BOAT: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduRover; + break; + + case MAV_TYPE.SUBMARINE: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduSub; + break; + + case MAV_TYPE.ANTENNA_TRACKER: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduTracker; + break; + case MAV_TYPE.COAXIAL: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduCopter2; + break; + default: + MAVlist[sysid, compid].cs.firmware = Firmwares.Other; + log.Error(MAVlist[sysid, compid].aptype + " not registered as valid type"); + break; + } + + break; + + case MAV_AUTOPILOT.UDB: + switch (MAVlist[sysid, compid].aptype) + { + case MAV_TYPE.FIXED_WING: + MAVlist[sysid, compid].cs.firmware = Firmwares.ArduPlane; + break; + } + + break; + + case MAV_AUTOPILOT.GENERIC: + switch (MAVlist[sysid, compid].aptype) + { + case MAV_TYPE.FIXED_WING: + MAVlist[sysid, compid].cs.firmware = Firmwares.Ateryx; + break; + } + + break; + + case MAV_AUTOPILOT.PX4: + MAVlist[sysid, compid].cs.firmware = Firmwares.PX4; + break; + + default: + switch (MAVlist[sysid, compid].aptype) + { + case MAV_TYPE.GIMBAL: // storm32 - name 83 + MAVlist[sysid, compid].cs.firmware = Firmwares.Gimbal; + break; + } + + break; + } + } + + public override string ToString() + { + if (BaseStream != null && BaseStream.IsOpen) + return "MAV " + MAV.sysid + " on " + BaseStream.PortName; + + if (logreadmode) + return "MAV " + MAV.sysid + " on LogFile"; + + return "MAV " + MAV.sysid + " on Ice"; + } + + public void Dispose() + { + if (_bytesReceivedSubj != null) + _bytesReceivedSubj.Dispose(); + if (_bytesSentSubj != null) + _bytesSentSubj.Dispose(); + if (MAVlist != null) + MAVlist.Dispose(); + + this.Close(); + + Terrain?.UnSub(); + Terrain = null; + + Mirrors.Clear(); + + logreadmode = false; + logplaybackfile = null; + } + + public void uAvionixADSBControl(int baroAltMSL,ushort squawk,/*UAVIONIX_ADSB_OUT_CONTROL_STATE*/byte state,/*UAVIONIX_ADSB_EMERGENCY_STATUS*/byte emergencyStatus,byte[] flight_id,byte x_bit) + { + mavlink_uavionix_adsb_out_control_t ctrl = new mavlink_uavionix_adsb_out_control_t(); + ctrl.state = state; + ctrl.baroAltMSL = baroAltMSL; + ctrl.squawk = squawk; + ctrl.emergencyStatus = emergencyStatus; + ctrl.flight_id = flight_id; + ctrl.x_bit = x_bit; + generatePacket(MAVLINK_MSG_ID.UAVIONIX_ADSB_OUT_CONTROL, ctrl); + } + + private class NoUIReporter : IProgressReporterDialogue + { + private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + public ProgressWorkerEventArgs doWorkArgs { get; set; } = new ProgressWorkerEventArgs(); + + public event DoWorkEventHandler DoWork; + + public void BeginInvoke(Delegate method) + { + method?.DynamicInvoke(); + } + + public void Dispose() + { + + } + + public void RunBackgroundOperationAsync() + { + Task.Run(() => + { + try + { + log.Info("DoWork"); + if (this.DoWork != null) this.DoWork(this); + log.Info("DoWork Done"); + } + catch (Exception e) + { + } + }).Wait(); + } + + public void UpdateProgressAndStatus(int progress, string status) + { + log.Info(progress + "% " + status); + } + } + } +} diff --git a/ExtLibs/ArduPilot/MissionPlanner.ArduPilot.csproj b/ExtLibs/ArduPilot/MissionPlanner.ArduPilot.csproj index 095ae864f5..52c3fbb296 100644 --- a/ExtLibs/ArduPilot/MissionPlanner.ArduPilot.csproj +++ b/ExtLibs/ArduPilot/MissionPlanner.ArduPilot.csproj @@ -18,6 +18,10 @@ true + + + + all diff --git a/ExtLibs/WinUSBNet/Nefarius.Drivers.WinUSB.csproj b/ExtLibs/WinUSBNet/Nefarius.Drivers.WinUSB.csproj index d03ba0b383..e0e1e2da96 100644 --- a/ExtLibs/WinUSBNet/Nefarius.Drivers.WinUSB.csproj +++ b/ExtLibs/WinUSBNet/Nefarius.Drivers.WinUSB.csproj @@ -26,7 +26,7 @@ - + all diff --git a/Log/LogDownloadMavLink.cs b/Log/LogDownloadMavLink.cs index e4c4021ddc..effd917fc2 100644 --- a/Log/LogDownloadMavLink.cs +++ b/Log/LogDownloadMavLink.cs @@ -7,6 +7,7 @@ using System.IO; using System.Linq; using System.Reflection; +using System.Threading; using System.Threading.Tasks; using System.Windows.Forms; @@ -22,6 +23,7 @@ public partial class LogDownloadMavLink : Form uint tallyBytes; // previous downloaded logs uint totalBytes; // total expected List logEntries; + CancellationTokenSource downloadCts; //List orientation = new List(); @@ -180,11 +182,15 @@ private void BUT_DLall_Click(object sender, EventArgs e) } AppendSerialLog(string.Format(LogStrings.DownloadStarting, Settings.Instance.LogDir)); + // the previous download (if any) has finished - the buttons gate on that + downloadCts?.Dispose(); + downloadCts = new CancellationTokenSource(); + var cancel = downloadCts.Token; System.Threading.Thread t11 = new System.Threading.Thread( delegate () { - DownloadThread(toDownload); + DownloadThread(toDownload, cancel); }) { Name = "Log Download All thread" @@ -193,25 +199,38 @@ private void BUT_DLall_Click(object sender, EventArgs e) } } - async Task GetLog(ushort no, string fileName) + async Task GetLog(MAVLink.mavlink_log_entry_t entry, CancellationToken cancel) { - log.Info("GetLog " + no); + log.Info("GetLog " + entry.id); MainV2.comPort.Progress += ComPort_Progress; + try + { + return await GetLogUnsubscribed(entry, cancel).ConfigureAwait(false); + } + finally + { + // always drop the handler, also when the download throws or is + // canceled - a leaked handler would double-count progress on + // the next download + MainV2.comPort.Progress -= ComPort_Progress; + } + } + async Task GetLogUnsubscribed(MAVLink.mavlink_log_entry_t entry, CancellationToken cancel) + { status = SerialStatus.Reading; // get df log from mav - var fn = await MainV2.comPort.GetLog(MainV2.comPort.MAV.sysid, MainV2.comPort.MAV.compid, no) + var fn = await MainV2.comPort.GetLog(MainV2.comPort.MAV.sysid, MainV2.comPort.MAV.compid, entry.id, cancel) .ConfigureAwait(false); - GC.Collect(); status = SerialStatus.Done; logfile = Settings.Instance.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar - + MainV2.comPort.MAV.sysid + Path.DirectorySeparatorChar + no + " " + - MakeValidFileName(fileName) + ".bin"; + + MainV2.comPort.MAV.sysid + Path.DirectorySeparatorChar + entry.id + " " + + MakeValidFileName(GetItemCaption(entry)) + ".bin"; // make log dir Directory.CreateDirectory(Path.GetDirectoryName(logfile)); @@ -223,17 +242,22 @@ async Task GetLog(ushort no, string fileName) } catch { - CustomMessageBox.Show(Strings.ErrorRenameFile + " " + logfile + "\nto " + logfile, + CustomMessageBox.Show(Strings.ErrorRenameFile + " " + fn + "\nto " + logfile, Strings.ERROR); } // rename file if needed - log.Info("about to GetFirstGpsTime: " + logfile); - // get gps time of assci log - var dflb = new DFLogBuffer(logfile); - DateTime logtime = dflb.dflog.gpsstarttime; - dflb.Clear(); - GC.Collect(); + // LOG_ENTRY already carries the log start time - only fall back to a full scan + // of the fresh download when the vehicle reported no valid time + DateTime logtime = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc).AddSeconds(entry.time_utc).ToLocalTime(); + if (logtime.Year < 1990) + { + log.Info("about to GetFirstGpsTime: " + logfile); + // scan the downloaded log for its first gps time + var dflb = new DFLogBuffer(logfile); + logtime = dflb.dflog.gpsstarttime; + dflb.Clear(); + } // rename log fs we have a valid gps time, logtime is after 1990-01-01, since some GPS does not use Unix epoch for invalid time. if (logtime.Year >= 1990) @@ -256,19 +280,30 @@ async Task GetLog(ushort no, string fileName) } } - MainV2.comPort.Progress -= ComPort_Progress; - return logfile; } protected override void OnClosed(EventArgs e) { this.closed = true; + CancelDownload(); MainV2.comPort.Progress -= ComPort_Progress; base.OnClosed(e); } + void CancelDownload() + { + try + { + downloadCts?.Cancel(); + } + catch (ObjectDisposedException) + { + // the download finished and disposed the source just as we canceled + } + } + protected override void OnClosing(CancelEventArgs e) { if (status == SerialStatus.Reading) @@ -279,6 +314,9 @@ protected override void OnClosing(CancelEventArgs e) e.Cancel = true; return; } + + // actually stop the transfer, not just the form + CancelDownload(); } base.OnClosing(e); @@ -321,7 +359,7 @@ void CreateKML(string logfile) status = SerialStatus.Done; } - private async void DownloadThread(int[] selectedLogs) + private async void DownloadThread(int[] selectedLogs, CancellationToken cancel) { try { @@ -340,11 +378,10 @@ private async void DownloadThread(int[] selectedLogs) foreach (int a in selectedLogs) { var entry = logEntries[a]; // mavlink_log_entry_t - string fileName = GetItemCaption(entry); - AppendSerialLog(string.Format(LogStrings.FetchingLog, fileName)); + AppendSerialLog(string.Format(LogStrings.FetchingLog, GetItemCaption(entry))); - await GetLog(entry.id, fileName).ConfigureAwait(false); + await GetLog(entry, cancel).ConfigureAwait(false); tallyBytes += receivedbytes; receivedbytes = 0; @@ -356,17 +393,27 @@ private async void DownloadThread(int[] selectedLogs) AppendSerialLog("Download complete."); Console.Beep(); } + catch (OperationCanceledException) + { + AppendSerialLog("Download canceled."); + } catch (Exception ex) { AppendSerialLog("Error in log " + ex.Message); } + finally + { + // this download owns the token source - release it before the + // buttons re-arm; Cancel racing this from OnClosing is handled there + Interlocked.Exchange(ref downloadCts, null)?.Dispose(); - RunOnUIThread(() => + RunOnUIThread(() => { BUT_DLall.Enabled = true; BUT_DLthese.Enabled = true; status = SerialStatus.Done; }); + } } IEnumerable GetSelectedLogIndices() @@ -391,9 +438,12 @@ private void UpdateProgress(uint min, uint max, uint current) { RunOnUIThread(() => { - progressBar1.Minimum = (int)min; - progressBar1.Maximum = (int)max; - progressBar1.Value = (int)current; + // scale to 0-1000 so byte counts beyond int.MaxValue don't overflow the + // ProgressBar; clamp because the sender may deliver more bytes than the + // LOG_ENTRY size it reported + progressBar1.Minimum = 0; + progressBar1.Maximum = 1000; + progressBar1.Value = max == 0 ? 0 : (int)Math.Min(1000.0, current * 1000.0 / max); progressBar1.Visible = (current < max); if (current == 0) @@ -412,7 +462,7 @@ private void UpdateProgress(uint min, uint max, uint current) var left = max - current; var eta = DateTime.Now.AddSeconds(left / avgbps); var remaining = new DateTime().AddSeconds(left / avgbps); - labelBytes.Text = MissionPlanner.Controls.ConnectionStats.ToHumanReadableByteCount((int)current) + " " + labelBytes.Text = MissionPlanner.Controls.ConnectionStats.ToHumanReadableByteCount((int)Math.Min(current, int.MaxValue)) + " " + per.ToString("N1") + "% " + MissionPlanner.Controls.ConnectionStats.ToHumanReadableByteCount((int)avgbps) + "/s " + (remaining.Day > 1 || remaining.Hour > 0 ? ((remaining.Day - 1) * 24 + remaining.Hour).ToString() + ":" : "") + remaining.ToString("mm:ss") + " left"; @@ -438,7 +488,11 @@ private void BUT_DLthese_Click(object sender, EventArgs e) { BUT_DLall.Enabled = false; BUT_DLthese.Enabled = false; - System.Threading.Thread t11 = new System.Threading.Thread(delegate () { DownloadThread(toDownload); }) + // the previous download (if any) has finished - the buttons gate on that + downloadCts?.Dispose(); + downloadCts = new CancellationTokenSource(); + var cancel = downloadCts.Token; + System.Threading.Thread t11 = new System.Threading.Thread(delegate () { DownloadThread(toDownload, cancel); }) { Name = "Log download single thread" }; diff --git a/MissionPlanner.csproj b/MissionPlanner.csproj index 3f967e8433..67594fcdff 100644 --- a/MissionPlanner.csproj +++ b/MissionPlanner.csproj @@ -62,6 +62,7 @@ + @@ -73,6 +74,7 @@ + @@ -84,6 +86,7 @@ + diff --git a/tests/MissionPlanner.ArduPilot.SitlTests/MissionPlanner.ArduPilot.SitlTests.csproj b/tests/MissionPlanner.ArduPilot.SitlTests/MissionPlanner.ArduPilot.SitlTests.csproj new file mode 100644 index 0000000000..b3620d3d22 --- /dev/null +++ b/tests/MissionPlanner.ArduPilot.SitlTests/MissionPlanner.ArduPilot.SitlTests.csproj @@ -0,0 +1,16 @@ + + + net472 + Exe + false + latest + + + + + + + + + + diff --git a/tests/MissionPlanner.ArduPilot.SitlTests/Program.cs b/tests/MissionPlanner.ArduPilot.SitlTests/Program.cs new file mode 100644 index 0000000000..048d44a3f0 --- /dev/null +++ b/tests/MissionPlanner.ArduPilot.SitlTests/Program.cs @@ -0,0 +1,181 @@ +using System; +using System.Diagnostics; +using System.IO; +using System.Linq; +using System.Threading; +using System.Threading.Tasks; +using MissionPlanner; +using MissionPlanner.Comms; + +class Program +{ + // args[0]: host, args[1]: port, args[2]: oracle file path (WSL log via \\wsl$), + // args[3]: "cancel" to also run the mid-download cancel check + static async Task Main(string[] args) + { + var host = args.Length > 0 ? args[0] : "127.0.0.1"; + var port = args.Length > 1 ? args[1] : "5760"; + var oracle = args.Length > 2 ? args[2] : null; + var doCancel = args.Contains("cancel"); + + var tcp = new TcpSerial { Host = host, Port = port, autoReconnect = false, retrys = 1 }; + tcp.Open(); + Console.WriteLine($"[harness] connected to {host}:{port}"); + + var mav = new MAVLinkInterface(); + mav.BaseStream = tcp; + + var stop = new CancellationTokenSource(); + var pump = Task.Run(async () => + { + while (!stop.IsCancellationRequested) + { + try + { + // GetLogEntry reads packets itself - stand down like MainV2.SerialReader does + if (mav.giveComport) + { + await Task.Delay(5).ConfigureAwait(false); + continue; + } + + await mav.readPacketAsync().ConfigureAwait(false); + } + catch + { + await Task.Delay(5).ConfigureAwait(false); + } + } + }); + + // wait for a heartbeat so sysid/compid are known + var deadline = DateTime.UtcNow.AddSeconds(20); + while (mav.MAV.sysid == 0 && DateTime.UtcNow < deadline) + await Task.Delay(100); + if (mav.MAV.sysid == 0) + { + Console.WriteLine("[harness] FAIL no heartbeat within 20s"); + return 1; + } + + Console.WriteLine($"[harness] heartbeat from sysid={mav.MAV.sysid} compid={mav.MAV.compid}"); + + long packets = 0; + mav.OnPacketReceived += (s, m) => Interlocked.Increment(ref packets); + + // let SITL finish sensor init before asking for the log list + await Task.Delay(2000); + + // --- list logs --- + System.Collections.Generic.List entries = null; + for (int attempt = 1; attempt <= 3; attempt++) + { + Console.WriteLine($"[harness] log list attempt {attempt}, packets seen so far: {Interlocked.Read(ref packets)}"); + try + { + entries = (await mav.GetLogEntry()).Values.OrderBy(a => a.id).ToList(); + break; + } + catch (TimeoutException) when (attempt < 3) + { + Console.WriteLine("[harness] log list timed out, retrying"); + await Task.Delay(1000); + } + } + Console.WriteLine($"[harness] log list: {entries.Count} entries"); + foreach (var e in entries) + Console.WriteLine($"[harness] id={e.id} size={e.size} utc={e.time_utc}"); + if (entries.Count == 0) + { + Console.WriteLine("[harness] FAIL no logs on vehicle"); + return 1; + } + + var entry = entries.First(); + + // --- download --- + var sw = Stopwatch.StartNew(); + var file = await mav.GetLog(mav.MAV.sysid, mav.MAV.compid, entry.id); + sw.Stop(); + var bytes = File.ReadAllBytes(file); + File.Delete(file); + Console.WriteLine($"[harness] downloaded {bytes.Length} bytes in {sw.Elapsed.TotalSeconds:F2}s " + + $"({bytes.Length / Math.Max(sw.Elapsed.TotalSeconds, 0.001) / 1024:F0} KiB/s)"); + + if (entry.size != 0 && bytes.Length != entry.size) + { + Console.WriteLine($"[harness] FAIL size mismatch: LOG_ENTRY said {entry.size}, got {bytes.Length}"); + return 1; + } + + if (oracle != null) + { + var expected = File.ReadAllBytes(oracle); + if (expected.Length != bytes.Length) + { + Console.WriteLine($"[harness] FAIL oracle length {expected.Length} != downloaded {bytes.Length}"); + return 1; + } + + for (int i = 0; i < expected.Length; i++) + { + if (expected[i] != bytes[i]) + { + Console.WriteLine($"[harness] FAIL first byte difference at offset {i}"); + return 1; + } + } + + Console.WriteLine($"[harness] PASS oracle byte-for-byte match ({expected.Length} bytes)"); + } + + if (doCancel) + { + using (var cts = new CancellationTokenSource(400)) + { + try + { + await mav.GetLog(mav.MAV.sysid, mav.MAV.compid, entry.id, cts.Token); + Console.WriteLine("[harness] FAIL cancel: download completed anyway"); + return 1; + } + catch (OperationCanceledException) + { + Console.WriteLine("[harness] PASS cancel raised OperationCanceledException"); + } + } + + // link must still be usable afterwards: list logs again. + // a couple of LOG_DATA packets may still be in flight right after the + // cancel, so allow a short settle + one retry + await Task.Delay(500); + int again = -1; + for (int attempt = 1; attempt <= 3; attempt++) + { + try + { + again = (await mav.GetLogEntry()).Values.Count; + break; + } + catch (Exception ex) when (attempt < 3) + { + Console.WriteLine($"[harness] post-cancel list attempt {attempt} failed: {ex.Message}"); + await Task.Delay(1000); + } + } + + if (again < 0) + { + Console.WriteLine("[harness] FAIL link unusable after cancel"); + return 1; + } + + Console.WriteLine($"[harness] PASS link usable after cancel (log list: {again} entries)"); + } + + stop.Cancel(); + tcp.Close(); + Console.WriteLine("[harness] DONE"); + return 0; + } +} diff --git a/tests/MissionPlanner.ArduPilot.SitlTests/README.md b/tests/MissionPlanner.ArduPilot.SitlTests/README.md new file mode 100644 index 0000000000..9c2c7cf419 --- /dev/null +++ b/tests/MissionPlanner.ArduPilot.SitlTests/README.md @@ -0,0 +1,95 @@ +# MissionPlanner.ArduPilot.SitlTests + +Manual end-to-end harness for the MAVLink log download protocol +(`MAVLinkInterface.GetLog`) against a real ArduPilot SITL +(Software In The Loop) simulated vehicle. Not part of any automated test +run - it needs SITL running and is driven by hand. + +What it verifies: + +- log listing (`GetLogEntry`) against the vehicle +- full download, timed, with the byte count checked against `LOG_ENTRY.size` +- optional byte-for-byte comparison against an oracle file (the log SITL wrote + to its own disk) +- optional mid-download cancellation, including that the link stays usable + afterwards (`LOG_REQUEST_END` behavior) + +## One-time SITL setup (WSL Ubuntu) + +```bash +mkdir -p ~/sitl && cd ~/sitl +curl -fsSL -o arducopter https://firmware.ardupilot.org/Copter/stable/SITL_x86_64_linux_gnu/arducopter +curl -fsSL -o copter.parm https://raw.githubusercontent.com/ArduPilot/ardupilot/master/Tools/autotest/default_params/copter.parm +chmod +x arducopter +``` + +Generate a static dataflash log (boot once with disarmed logging, then exit so +the file is closed; SITL waits for a TCP client before booting, hence the +python one-liner): + +```bash +cd ~/sitl +printf "LOG_DISARMED 1\n" > extra.parm +(python3 -c "import socket,time; time.sleep(3); s=socket.create_connection(('127.0.0.1',5760)); s.settimeout(35); end=time.time()+30 +while time.time()/sitl/logs/00000001.BIN' /tmp/oracle.bin +``` + +Run the harness (from `bin/Debug/net472` after `dotnet build`): + +``` +SitlLogTest.exe [oraclePath] [cancel] + +MissionPlanner.ArduPilot.SitlTests.exe 127.0.0.1 5760 C:\path\to\oracle.bin cancel +``` + +`cancel` also runs the mid-download cancellation check. + +## Lossy-link run + +`lossy_proxy.py` sits between the harness and SITL and drops every 20th +LOG_DATA frame (~5% loss), forcing the fill-in phase to recover the gaps. +In WSL, with SITL already listening on 5760: + +```bash +python3 lossy_proxy.py # listens on 5770, forwards to 127.0.0.1:5760 +``` + +Then point the harness at port 5770. The download is expected to be slower +(one round-trip per scattered missing block) but still byte-identical to the +oracle. The proxy prints how many frames it saw and dropped on exit. + +## Reference results (2026-08-29, 2.3 MB log, WSL2 loopback) + +- clean link: 2,306,048 bytes in ~0.45 s (~5 MiB/s), byte-identical +- 5% LOG_DATA loss: byte-identical in ~58 s, one streaming pass + (1,348 of 26,971 frames dropped; the extra ~3 s over the earlier ~55 s + reference is one silence window confirming the end of log, whose packet + arrives far past a frontier stalled at the first dropped block) + +The lossy run is the reason this harness exists: a 5% loss stalls the +contiguous frontier near the start of the log, so the genuine end packet +of a large log arrives far past any near-frontier trust window - a case +the unit tests cannot represent because their logs are smaller than the +window itself. A fix that rejected such end packets outright passed the +whole unit suite and never completed here: every recovered gap forced the +vehicle to re-stream the entire log. diff --git a/tests/MissionPlanner.ArduPilot.SitlTests/lossy_proxy.py b/tests/MissionPlanner.ArduPilot.SitlTests/lossy_proxy.py new file mode 100644 index 0000000000..bb86d94cad --- /dev/null +++ b/tests/MissionPlanner.ArduPilot.SitlTests/lossy_proxy.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +"""TCP proxy that forwards MAVLink both ways but drops every Nth LOG_DATA +frame (msgid 120) travelling vehicle -> GCS. Listens on LISTEN_PORT and +connects to the SITL vehicle on VEHICLE_PORT.""" +import socket +import sys +import threading + +LISTEN_PORT = 5770 +VEHICLE_PORT = 5760 +DROP_EVERY = 20 +LOG_DATA = 120 + + +def pump_gcs_to_vehicle(gcs, veh): + try: + while True: + d = gcs.recv(4096) + if not d: + break + veh.sendall(d) + except OSError: + pass + finally: + try: + veh.shutdown(socket.SHUT_WR) + except OSError: + pass + + +def pump_vehicle_to_gcs(veh, gcs): + # bytearray + index cursor: repeated += / slicing on immutable bytes + # would reallocate the whole buffer per frame (O(n^2) on large logs) + buf = bytearray() + seen = 0 + dropped = 0 + try: + while True: + d = veh.recv(4096) + if not d: + break + buf.extend(d) + out = [] + pos = 0 + while pos < len(buf): + b0 = buf[pos] + if b0 == 0xFD: # MAVLink2: 10 byte hdr + payload + 2 crc (+13 sig) + if len(buf) - pos < 10: + break + length = 12 + buf[pos + 1] + (13 if buf[pos + 2] & 0x01 else 0) + if len(buf) - pos < length: + break + msgid = buf[pos + 7] | (buf[pos + 8] << 8) | (buf[pos + 9] << 16) + elif b0 == 0xFE: # MAVLink1: 6 byte hdr + payload + 2 crc + if len(buf) - pos < 6: + break + length = 8 + buf[pos + 1] + if len(buf) - pos < length: + break + msgid = buf[pos + 5] + else: + out.append(buf[pos:pos + 1]) + pos += 1 + continue + + frame = buf[pos:pos + length] + pos += length + if msgid == LOG_DATA: + seen += 1 + if seen % DROP_EVERY == 0: + dropped += 1 + continue + out.append(frame) + del buf[:pos] + if out: + gcs.sendall(b"".join(out)) + except OSError: + pass + finally: + print(f"[proxy] LOG_DATA seen={seen} dropped={dropped}", flush=True) + try: + gcs.shutdown(socket.SHUT_WR) + except OSError: + pass + + +def main(): + srv = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + srv.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + srv.bind(("0.0.0.0", LISTEN_PORT)) + srv.listen(1) + print(f"[proxy] listening on {LISTEN_PORT}, dropping 1/{DROP_EVERY} LOG_DATA", flush=True) + gcs, addr = srv.accept() + print(f"[proxy] client {addr}", flush=True) + veh = socket.create_connection(("127.0.0.1", VEHICLE_PORT)) + gcs.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) + veh.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) + t = threading.Thread(target=pump_gcs_to_vehicle, args=(gcs, veh), daemon=True) + t.start() + pump_vehicle_to_gcs(veh, gcs) + t.join(timeout=2) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/tests/MissionPlanner.ArduPilot.Tests/GetLogTests.cs b/tests/MissionPlanner.ArduPilot.Tests/GetLogTests.cs new file mode 100644 index 0000000000..e266f6228e --- /dev/null +++ b/tests/MissionPlanner.ArduPilot.Tests/GetLogTests.cs @@ -0,0 +1,627 @@ +using System; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Threading; +using System.Threading.Tasks; +using MissionPlanner.Comms; +using Xunit; + +namespace MissionPlanner.ArduPilot.Tests +{ + public class GetLogTests + { + const byte VehicleSysid = 1; + const byte VehicleCompid = 1; + const ushort LogId = 3; + const int BlockSize = 90; + + /// + /// Simulated vehicle end of the MAVLink log download protocol, connected + /// to a MAVLinkInterface through an in-memory CommsInjection link. + /// + sealed class FakeLogVehicle : IDisposable + { + public readonly CommsInjection Link = new CommsInjection(); + public readonly MAVLinkInterface Mav = new MAVLinkInterface(); + public readonly List Requests = new List(); + public Action OnRequest; + int _endRequests; + + public int EndRequests => Volatile.Read(ref _endRequests); + + readonly MAVLink.MavlinkParse _parse = new MAVLink.MavlinkParse(); + readonly byte[] _log; + readonly CancellationTokenSource _stop = new CancellationTokenSource(); + readonly Task _pump; + + public FakeLogVehicle(byte[] log) + { + _log = log; + + Link.WriteCallback += (sender, outbytes) => + { + MAVLink.MAVLinkMessage msg; + try + { + msg = new MAVLink.MAVLinkMessage(outbytes.ToArray()); + } + catch + { + return; + } + + if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.LOG_REQUEST_END) + { + Interlocked.Increment(ref _endRequests); + return; + } + + if (msg.msgid != (uint)MAVLink.MAVLINK_MSG_ID.LOG_REQUEST_DATA) + return; + + var req = msg.ToStructure(); + if (req.id != LogId) + return; + + lock (Requests) + Requests.Add(req); + OnRequest?.Invoke(req); + }; + + Mav.BaseStream = Link; + + // real-time protocol timeouts scaled down so the suite stays fast + Mav.LogDataTimeoutMs = 500; + Mav.LogDataRefetchMs = 100; + + // GetLog consumes packets via OnPacketReceived, which fires from the + // receive loop - pump it the same way MainV2.SerialReader does + _pump = Task.Run(async () => + { + while (!_stop.IsCancellationRequested) + { + try + { + await Mav.readPacketAsync().ConfigureAwait(false); + } + catch + { + try + { + await Task.Delay(1, _stop.Token).ConfigureAwait(false); + } + catch (OperationCanceledException) + { + } + } + } + }); + } + + public void SendBlock(uint ofs, int count) + { + var payload = new byte[BlockSize]; + Array.Copy(_log, ofs, payload, 0, count); + Send(MAVLink.MAVLINK_MSG_ID.LOG_DATA, new MAVLink.mavlink_log_data_t(ofs, LogId, (byte)count, payload)); + } + + public void SendEndMarker(uint ofs) + { + Send(MAVLink.MAVLINK_MSG_ID.LOG_DATA, new MAVLink.mavlink_log_data_t(ofs, LogId, 0, new byte[BlockSize])); + } + + public void Send(MAVLink.MAVLINK_MSG_ID msgid, object indata) + { + Link.AppendBuffer(_parse.GenerateMAVLinkPacket20(msgid, indata, false, VehicleSysid, VehicleCompid)); + } + + /// + /// Serve a LOG_REQUEST_DATA the way ArduPilot does: 90 byte blocks with + /// a short block at end of log, or a zero count marker when the log ends + /// on an exact block boundary. skipBlocks are dropped, but only the + /// first time each is requested. + /// + public void Serve(MAVLink.mavlink_log_request_data_t req, HashSet skipBlocks = null) + { + var end = Math.Min((ulong)req.ofs + req.count, (ulong)_log.Length); + for (var ofs = (ulong)req.ofs; ofs < end; ofs += BlockSize) + { + var block = (uint)(ofs / BlockSize); + if (skipBlocks != null && skipBlocks.Remove(block)) + continue; + SendBlock((uint)ofs, (int)Math.Min(BlockSize, end - ofs)); + } + + if (end == (ulong)_log.Length && (ulong)req.ofs + req.count > end && _log.Length % BlockSize == 0) + SendEndMarker((uint)end); + } + + public void Dispose() + { + _stop.Cancel(); + Link.Close(); + try + { + _pump.Wait(2000); + } + catch + { + } + } + } + + static byte[] MakeLog(int length) + { + var data = new byte[length]; + new Random(42).NextBytes(data); + return data; + } + + /// A private directory owned by a single test, removed on dispose. + sealed class TestDir : IDisposable + { + public string Root { get; } = + Path.Combine(Path.GetTempPath(), "GetLogTests-" + Guid.NewGuid().ToString("N")); + + public TestDir() + { + Directory.CreateDirectory(Root); + } + + public string File(string name) => Path.Combine(Root, name); + + public void Dispose() + { + try + { + Directory.Delete(Root, true); + } + catch + { + } + } + } + + static async Task Download(FakeLogVehicle vehicle, CancellationToken cancel) + { + using (var dir = new TestDir()) + { + var file = await vehicle.Mav.GetLogInternal(VehicleSysid, VehicleCompid, LogId, + dir.File("log.bin"), cancel); + return File.ReadAllBytes(file); + } + } + + [Fact] + public async Task DownloadsCompleteLogInOrder() + { + var log = MakeLog(1234); + using (var vehicle = new FakeLogVehicle(log)) + { + vehicle.OnRequest = req => vehicle.Serve(req); + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.Equal(log, result); + Assert.True(vehicle.EndRequests > 0, + "LOG_REQUEST_END not sent after a completed download"); + } + } + + [Fact] + public async Task StaleDuplicatesDoNotMultiplyFillRequests() + { + var log = MakeLog(1000); + using (var vehicle = new FakeLogVehicle(log)) + { + var drop = new HashSet { 3 }; + var fillRequests = 0; + vehicle.OnRequest = req => + { + if (Interlocked.Increment(ref fillRequests) > 1) + { + // before serving the gap, flood stale duplicates of blocks the + // download already holds - they must not each trigger another + // fill request on a duplicating link + for (var i = 0; i < 30; i++) + { + vehicle.SendBlock(8 * BlockSize, BlockSize); + vehicle.SendBlock(9 * BlockSize, BlockSize); + } + } + + vehicle.Serve(req, drop); + }; + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.Equal(log, result); + lock (vehicle.Requests) + Assert.True(vehicle.Requests.Count <= 8, + $"{vehicle.Requests.Count} requests for one dropped block - stale " + + "duplicates are multiplying fill requests"); + } + } + + [Fact] + public async Task CorruptFarOffsetDoesNotBreakEndDetection() + { + var log = MakeLog(1234); + using (var vehicle = new FakeLogVehicle(log)) + { + var corruptSent = false; + vehicle.OnRequest = req => + { + var end = Math.Min((ulong)req.ofs + req.count, (ulong)log.Length); + for (var ofs = (ulong)req.ofs; ofs < end; ofs += BlockSize) + { + // a corrupt-but-valid-looking packet at a far offset must neither + // poison end-of-log detection nor lengthen the returned file + if (ofs == 5 * BlockSize && !corruptSent) + { + corruptSent = true; + vehicle.Send(MAVLink.MAVLINK_MSG_ID.LOG_DATA, + new MAVLink.mavlink_log_data_t(1_000_000, LogId, BlockSize, + new byte[BlockSize])); + } + + vehicle.SendBlock((uint)ofs, (int)Math.Min(BlockSize, end - ofs)); + } + }; + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.True(result.Length == log.Length, + $"corrupt far-offset packet changed the file length: {result.Length} != {log.Length}"); + Assert.Equal(log, result); + } + } + + [Fact] + public async Task CorruptShortFarPacketDoesNotEndTheDownload() + { + var log = MakeLog(1234); + using (var vehicle = new FakeLogVehicle(log)) + { + var corruptSent = false; + vehicle.OnRequest = req => + { + var end = Math.Min((ulong)req.ofs + req.count, (ulong)log.Length); + for (var ofs = (ulong)req.ofs; ofs < end; ofs += BlockSize) + { + // a packet that is both short and at a far offset clears the + // end-of-log bar trivially - it must not terminate the stream + // at a phantom length + if (ofs == 5 * BlockSize && !corruptSent) + { + corruptSent = true; + vehicle.Send(MAVLink.MAVLINK_MSG_ID.LOG_DATA, + new MAVLink.mavlink_log_data_t(1_000_000, LogId, 40, + new byte[BlockSize])); + } + + vehicle.SendBlock((uint)ofs, (int)Math.Min(BlockSize, end - ofs)); + } + }; + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.True(result.Length == log.Length, + $"corrupt short far packet ended the download at a phantom length: " + + $"{result.Length} != {log.Length}"); + Assert.Equal(log, result); + } + } + + [Fact] + public async Task CorruptFarPacketDoesNotHijackRetryOffset() + { + var log = MakeLog(1234); + using (var vehicle = new FakeLogVehicle(log)) + { + var first = true; + vehicle.OnRequest = req => + { + if (!first) + { + vehicle.Serve(req); + return; + } + + // stream five blocks, inject a corrupt far packet, then go silent - + // the streaming retry must resume from the true stream position, + // not from past the corrupt offset + first = false; + for (var ofs = 0u; ofs < 5 * BlockSize; ofs += BlockSize) + vehicle.SendBlock(ofs, BlockSize); + vehicle.Send(MAVLink.MAVLINK_MSG_ID.LOG_DATA, + new MAVLink.mavlink_log_data_t(1_000_000, LogId, BlockSize, + new byte[BlockSize])); + }; + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.Equal(log, result); + lock (vehicle.Requests) + { + Assert.True(vehicle.Requests.Any(r => r.ofs == 5 * BlockSize), + "retry did not resume from the true stream position"); + Assert.False(vehicle.Requests.Any(r => r.ofs > (uint)log.Length), + "retry requested data past the corrupt offset"); + } + } + } + + [Fact] + public async Task RepeatedStaleDataDoesNotKeepTheStreamAliveForever() + { + var log = MakeLog(1000); + using (var vehicle = new FakeLogVehicle(log)) + using (var dir = new TestDir()) + { + var first = true; + vehicle.OnRequest = req => + { + if (first) + { + // stream only the first three blocks, then answer every retry + // with a stale duplicate - it brings no new data, so it must + // not keep resetting the retry budget forever + first = false; + for (var ofs = 0u; ofs < 3 * BlockSize; ofs += BlockSize) + vehicle.SendBlock(ofs, BlockSize); + return; + } + + vehicle.SendBlock(0, BlockSize); + }; + + using (var guard = new CancellationTokenSource(30000)) + using (var linked = CancellationTokenSource.CreateLinkedTokenSource( + guard.Token, TestContext.Current.CancellationToken)) + { + var path = dir.File("log.bin"); + + await Assert.ThrowsAsync( + () => vehicle.Mav.GetLogInternal(VehicleSysid, VehicleCompid, LogId, + path, linked.Token)); + + Assert.False(guard.IsCancellationRequested, + "download looped instead of timing out"); + } + } + } + + [Fact] + public async Task EndMarkerBeyondStalledFrontierCompletesViaFillin() + { + // an early drop stalls the contiguous frontier while the log is large + // enough that the genuine end packet sits far past its trust window - + // the end must still hand over to the fill-in phase, not force the + // whole stream to be sent again + var log = MakeLog(12000); + using (var vehicle = new FakeLogVehicle(log)) + { + var drop = new HashSet { 3 }; + vehicle.OnRequest = req => vehicle.Serve(req, drop); + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.Equal(log, result); + lock (vehicle.Requests) + Assert.True(vehicle.Requests.Count <= 4, + $"{vehicle.Requests.Count} requests for one dropped block - the " + + "far end marker is forcing full re-streams"); + } + } + + [Fact] + public async Task IgnoresPacketWithOversizedCount() + { + var log = MakeLog(1234); + using (var vehicle = new FakeLogVehicle(log)) + { + vehicle.OnRequest = req => + { + // count larger than the 90-byte payload array must be skipped, + // not abort the download + vehicle.Send(MAVLink.MAVLINK_MSG_ID.LOG_DATA, + new MAVLink.mavlink_log_data_t(0, LogId, 200, new byte[BlockSize])); + vehicle.Serve(req); + }; + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.Equal(log, result); + } + } + + [Fact] + public async Task BogusFillinOffsetDoesNotExtendFile() + { + var log = MakeLog(1000); + using (var vehicle = new FakeLogVehicle(log)) + { + var drop = new HashSet { 3 }; + var bogusSent = false; + vehicle.OnRequest = req => + { + if (req.ofs == 3 * BlockSize && !bogusSent) + { + // a fill-in response beyond the end of the log must not + // grow the file past the length the end marker established + bogusSent = true; + vehicle.Send(MAVLink.MAVLINK_MSG_ID.LOG_DATA, + new MAVLink.mavlink_log_data_t((uint)(log.Length + 10 * BlockSize), + LogId, BlockSize, new byte[BlockSize])); + } + + vehicle.Serve(req, drop); + }; + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.True(result.Length == log.Length, + $"bogus fill-in offset changed the file length: {result.Length} != {log.Length}"); + Assert.Equal(log, result); + } + } + + [Fact] + public async Task FillinTimesOutWhenVehicleGoesSilent() + { + var log = MakeLog(1000); + using (var vehicle = new FakeLogVehicle(log)) + using (var dir = new TestDir()) + { + // serve the streaming phase with a block missing, then never answer + // the fill-in requests - the download must fail, not hang forever + var first = true; + vehicle.OnRequest = req => + { + if (!first) + return; + first = false; + vehicle.Serve(req, new HashSet { 3 }); + }; + + var path = dir.File("log.bin"); + + await Assert.ThrowsAsync( + () => vehicle.Mav.GetLogInternal(VehicleSysid, VehicleCompid, LogId, path, + TestContext.Current.CancellationToken)); + + Assert.False(File.Exists(path), "partial file left behind after fill-in timeout"); + } + } + + [Fact] + public async Task HandlesLogSizeExactMultipleOfBlockSize() + { + var log = MakeLog(900); + using (var vehicle = new FakeLogVehicle(log)) + { + vehicle.OnRequest = req => vehicle.Serve(req); + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.Equal(log, result); + // the fill in phase must not ask for a phantom block past end of log + lock (vehicle.Requests) + Assert.False(vehicle.Requests.Skip(1).Any(r => r.ofs >= 900), + "requested data past the end of the log"); + } + } + + [Fact] + public async Task RefetchesDroppedBlocks() + { + var log = MakeLog(1000); + using (var vehicle = new FakeLogVehicle(log)) + { + var drop = new HashSet { 3, 7 }; + vehicle.OnRequest = req => vehicle.Serve(req, drop); + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.Equal(log, result); + lock (vehicle.Requests) + Assert.True(vehicle.Requests.Any(r => r.ofs == 3 * BlockSize), + "first dropped block was never re-requested"); + } + } + + [Fact] + public async Task IgnoresStrayShortPacketBeforeEndOfLog() + { + var log = MakeLog(1234); + using (var vehicle = new FakeLogVehicle(log)) + { + var first = true; + vehicle.OnRequest = req => + { + if (!first) + { + vehicle.Serve(req); + return; + } + + first = false; + var end = Math.Min((ulong)req.ofs + req.count, (ulong)log.Length); + for (var ofs = (ulong)req.ofs; ofs < end; ofs += BlockSize) + { + // a stale short retransmit mid stream must not truncate the log + if (ofs == 8 * BlockSize) + vehicle.SendBlock(3 * BlockSize, 40); + vehicle.SendBlock((uint)ofs, (int)Math.Min(BlockSize, end - ofs)); + } + }; + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.True(log.Length == result.Length, "stray short packet truncated the download"); + Assert.Equal(log, result); + } + } + + [Fact] + public async Task EmptyLogProducesEmptyFile() + { + using (var vehicle = new FakeLogVehicle(new byte[0])) + { + vehicle.OnRequest = req => vehicle.SendEndMarker(0); + + var result = await Download(vehicle, TestContext.Current.CancellationToken); + + Assert.Empty(result); + } + } + + [Fact] + public async Task CancelStopsDownloadAndDeletesFile() + { + var log = MakeLog(100 * BlockSize); + using (var vehicle = new FakeLogVehicle(log)) + using (var dir = new TestDir()) + { + // stream a few blocks, then go silent so the download hangs mid way + vehicle.OnRequest = req => + { + for (uint block = 0; block < 5; block++) + vehicle.SendBlock(block * BlockSize, BlockSize); + }; + + var path = dir.File("log.bin"); + + using (var cts = CancellationTokenSource.CreateLinkedTokenSource(TestContext.Current.CancellationToken)) + { + cts.CancelAfter(300); + await Assert.ThrowsAnyAsync( + () => vehicle.Mav.GetLogInternal(VehicleSysid, VehicleCompid, LogId, path, cts.Token)); + } + + Assert.False(File.Exists(path), "partial file left behind after cancel"); + Assert.True(vehicle.EndRequests > 0, + "LOG_REQUEST_END not sent - the vehicle would keep streaming LOG_DATA"); + } + } + + [Fact(Timeout = 15000)] + public async Task TimesOutWhenVehicleNeverResponds() + { + using (var vehicle = new FakeLogVehicle(MakeLog(10))) + using (var dir = new TestDir()) + { + var path = dir.File("log.bin"); + + // no OnRequest wired - total silence; LogDataTimeoutMs with 3 retries + await Assert.ThrowsAsync( + () => vehicle.Mav.GetLogInternal(VehicleSysid, VehicleCompid, LogId, path, + TestContext.Current.CancellationToken)); + + Assert.False(File.Exists(path), "partial file left behind after timeout"); + } + } + } +} diff --git a/tests/MissionPlanner.ArduPilot.Tests/MissionPlanner.ArduPilot.Tests.csproj b/tests/MissionPlanner.ArduPilot.Tests/MissionPlanner.ArduPilot.Tests.csproj new file mode 100644 index 0000000000..b797aafaf3 --- /dev/null +++ b/tests/MissionPlanner.ArduPilot.Tests/MissionPlanner.ArduPilot.Tests.csproj @@ -0,0 +1,22 @@ + + + net472 + Exe + false + latest + + direct + + + + + + + + + + + +