Ejemplo n.º 1
0
        // __ MavLink events __________________________________________________


        private void HandlePacketReceived(object sender, MavlinkPacket e)
        {
            if (OnPacketReceived != null)
            {
                OnPacketReceived(sender, e);
            }
        }
Ejemplo n.º 2
0
 static void NetworkLayerPacketReceived(object sender, MavlinkPacket e)
 {
     if (e.Message is MAVLink_vfr_hud_message)
         ShowMessage((MAVLink_vfr_hud_message)e.Message);
     else
         Console.WriteLine(DefaultLineFormat, e.SystemId, e.ComponentId, e.Message.GetType().Name);
 }
Ejemplo n.º 3
0
        private void SendMavlinkMessage(IPEndPoint ep, MavlinkMessage msg)
        {
            MavlinkPacket p = GetPacketFromMsg(msg);

            byte[] buffer = mMavLink.Send(p);

            mUdpClient.Send(buffer, buffer.Length, ep);
        }
Ejemplo n.º 4
0
        public override void SendMessage(MavlinkMessage msg)
        {
            MavlinkPacket packet = CreatePacket(msg);

            byte[] bytes = Send(packet);
            try { serial.Write(bytes, 0, bytes.Length); }
            catch { }
        }
Ejemplo n.º 5
0
        void Mv_PacketReceived(object sender, MavLink.MavlinkPacket e)
        {
            uint           x = Mv.PacketsReceived;
            MavlinkMessage m = e.Message;

            if (m.GetType() == Hb.GetType())
            {
                Hb = (Msg_heartbeat)e.Message;
            }
            if (m.GetType() == Ss.GetType())
            {
                Ss = (Msg_sys_status)e.Message;
            }
            if (m.GetType() == Ps.GetType())
            {
                Ps = (Msg_power_status)e.Message;
            }
            if (m.GetType() == At.GetType())
            {
                At = (Msg_attitude)e.Message;
            }
            if (m.GetType() == Gps.GetType())
            {
                Gps = (Msg_gps_raw_int)e.Message;
            }
            if (m.GetType() == Vfr.GetType())
            {
                Vfr = (Msg_vfr_hud)e.Message;
            }
            if (m.GetType() == Rp.GetType())
            {
                Rp = (Msg_raw_pressure)e.Message;
            }
            if (m.GetType() == Sp.GetType())
            {
                Sp  = (Msg_scaled_pressure)e.Message;
                Prs = (int)(Sp.press_abs * 1000f);
                Tep = Sp.temperature;
                Dif = (int)(Sp.press_diff * 1000f);

                /*
                 * if (Sw == null)
                 * {
                 * Sw = new StreamWriter("c:\\Baro.csv");
                 * Sw.Write("\"Item\", \"Pressure\", \"Temp\", \"Difference\"\r\n");
                 * }
                 * String S = String.Format("{0}, {1}, {2}, {3}\r\n", Total++, Prs, Tep, Dif);
                 * Sw.Write(S);*/
            }
            if (x > 0)
            {
                if (MavLinkReader.ActiveForm != null)
                {
                    MavLinkReader.ActiveForm.Invalidate();
                }
            }
        }
Ejemplo n.º 6
0
        private MavlinkPacket CreatePacket(MavlinkMessage msg)
        {
            MavlinkPacket packet = new MavlinkPacket();

            packet.ComponentId = (int)MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
            packet.SystemId    = 0xCE;
            packet.Message     = msg;

            return(packet);
        }
Ejemplo n.º 7
0
        private MavlinkPacket GetPacketFromMsg(MavlinkMessage msg)
        {
            MavlinkPacket p = new MavlinkPacket();

            p.Message        = msg;
            p.TimeStamp      = DateTime.Now;
            p.SequenceNumber = 1;
            p.SystemId       = MavlinkSystemId;
            p.ComponentId    = MavlinkComponentId;

            return(p);
        }
Ejemplo n.º 8
0
        // add timestamp to message ( called directly after receive)
        protected override void postMessage(MavlinkPacket msg)
        {
            base.postMessage(msg);

            if (_noTimeStamps)
                msg.TimeStamp = DateTime.Now;
            else
                msg.TimeStamp = _timestamp;
            if (_planner_format)
                _stream.ReadByte(); // trailing newline

            _timestamp = msg.TimeStamp;
        }
Ejemplo n.º 9
0
        private static void SendParamListRequest(MavlinkPacket mavlinkPacket)
        {
            if (_mavStream.CanWrite)
            {
                Console.WriteLine("Sending param list request: " + mavlinkPacket.SystemId);

                var req = new MAVLink_param_request_list_message()
                {
                    target_system = (byte) mavlinkPacket.SystemId,
                    target_component = (byte) mavlinkPacket.ComponentId,
                };

                _net.Send(new MavlinkPacket { ComponentId = 1, SystemId = 255, Message = req });
            }
            else
            {
                Console.WriteLine("Cannot send paramlist request. Scanning for params... ");
            }
        }
Ejemplo n.º 10
0
        // Process a raw packet in it's entirety in the given byte array
        // if deserialization is successful, then the packetdecoded event will be raised
        private void ProcessPacketBytes(byte[] packetBytes, byte rxPacketSequence)
        {
            //	 System ID	 1 - 255
            //	 Component ID	 0 - 255
            //	 Message ID	 0 - 255
            //   6 to (n+6)	 Data	 (0 - 255) bytes
            var packet = new MavlinkPacket
            {
                SystemId       = packetBytes[0],
                ComponentId    = packetBytes[1],
                SequenceNumber = rxPacketSequence,
                Message        = this.Deserialize(packetBytes, 2)
            };

            if (PacketReceived != null)
            {
                PacketReceived(this, packet);
            }

            // else do what?
        }
Ejemplo n.º 11
0
        /// <summary>
        /// 发送数据包,使用此方法时,请先对事件SetMessage赋值
        /// 如果消息没有被处理赋值,则会返回null
        /// </summary>
        /// <param name="obj">message的<see cref="Type"></param>
        /// <returns></returns>
        public byte[] Send(Type type)
        {
            var           message = Activator.CreateInstance(type) as MavlinkMessage;
            MavlinkPacket packet  = new MavlinkPacket(message);

            packet.SystemId       = 1;
            packet.ComponentId    = 1;
            packet.SequenceNumber = (byte)((SequenceNumber >> 24) & 0xFF);
            if (SequenceNumber == 255)
            {
                SequenceNumber = 0;
            }
            else
            {
                SequenceNumber++;
            }
            type.GetField("time_usec").SetValue(message, GetTimeStamp());
            if (SetMessage == null || !SetMessage.Invoke(message, type))
            {
                return(null);
            }
            return(Send(packet));
        }
Ejemplo n.º 12
0
        void Mv_PacketReceived(object sender, MavLink.MavlinkPacket e)
        {
            uint x = Mv.PacketsReceived;

            Systemid    = e.SystemId;
            Componentid = e.ComponentId;
            Sequence    = e.SequenceNumber;
            MavlinkMessage m = e.Message;

            if (m.GetType() == Hb.GetType())
            {
                Hb = (Msg_heartbeat)e.Message;
            }
            if (m.GetType() == Ss.GetType())
            {
                Ss = (Msg_sys_status)e.Message;
            }
            if (m.GetType() == Ps.GetType())
            {
                Ps = (Msg_power_status)e.Message;
            }
            if (m.GetType() == At.GetType())
            {
                At = (Msg_attitude)e.Message;
            }
            if (m.GetType() == Gps.GetType())
            {
                Gps = (Msg_gps_raw_int)e.Message;
            }
            if (m.GetType() == Vfr.GetType())
            {
                Vfr = (Msg_vfr_hud)e.Message;
            }
            if (m.GetType() == Rp.GetType())
            {
                Rp = (Msg_raw_pressure)e.Message;
            }
            if (m.GetType() == Sp.GetType())
            {
                Sp  = (Msg_scaled_pressure)e.Message;
                Prs = (int)(Sp.press_abs * 1000f);
                Tep = Sp.temperature;
                Dif = (int)(Sp.press_diff * 1000f);

                /*
                 * Special log of Barometric data to the D drive
                 * Can be disable if not needed
                 */
                ut.LogPressure(Sp);
            }
            if (m.GetType() == Ca.GetType())
            {
                Ca = (Msg_command_ack)e.Message;
            }
            if (m.GetType() == St.GetType())
            {
                St = (Msg_statustext)e.Message;
            }
            if (m.GetType() == Mc.GetType())
            {
                Mc = (Msg_mission_count)e.Message;
                MI = 0;
                GetMissionValues.RunWorkerAsync();
            }
            if (m.GetType() == Mi[0].GetType())
            {
                Mi[MI++] = (Msg_mission_item)e.Message;
                if (MI >= Mc.count)
                {
                    MI = -1;
                }
            }
            if (x > 0)
            {
                if (MavLinkReader.ActiveForm != null)
                {
                    MavLinkReader.ActiveForm.Invalidate();
                }
            }
        }
Ejemplo n.º 13
0
 public void Send(MavlinkPacket mavlinkPacket)
 {
     var bytes = _encoder.Serialize(mavlinkPacket.Message, mavlinkPacket.SystemId, mavlinkPacket.ComponentId);
     SendPacketLinkLayer(bytes);
 }
Ejemplo n.º 14
0
        private void mavlinkNetwork_PacketReceived(object sender, MavlinkPacket e)
        {
            if (e.Message is Msg_heartbeat)
            {
                if (RemoteSystemDetected != null)
                    RemoteSystemDetected(this, (Msg_heartbeat)e.Message);
            }

            //if (e.SystemId==_srcSystemId && e.ComponentId==_srcComponentId && PacketReceived!=null)
            if (PacketReceived!=null)
            {
                PacketReceived(this,e.Message);
            }
        }
Ejemplo n.º 15
0
        /// <summary>
        /// Send a mavlink message through this connection
        /// </summary>
        /// <param name="message">the message to send</param>
        public void Send(object message)
        {
            var mvp = new MavlinkPacket
                          {
                              ComponentId = _srcComponentId,
                              SystemId = _srcSystemId,
                              Message = message
                          };

            _mavlink.Send(mvp);
        }
Ejemplo n.º 16
0
        /// <summary>
        /// 发送数据包
        /// </summary>
        /// <param name="mavlinkPacket"></param>
        /// <returns></returns>
        public byte[] Send(MavlinkPacket mavlinkPacket)
        {
            var bytes = this.Serialize(mavlinkPacket.Message, mavlinkPacket.SystemId, mavlinkPacket.ComponentId);

            return(SendPacketLinkLayer(bytes));
        }
Ejemplo n.º 17
0
 public byte[] Send(MavlinkPacket mavlinkPacket)
 {
     var bytes = this.Serialize(mavlinkPacket.Message, mavlinkPacket.SystemId, mavlinkPacket.ComponentId);
     return SendPacketLinkLayer(bytes);
 }
Ejemplo n.º 18
0
        // Process a raw packet in it's entirety in the given byte array
        // if deserialization is successful, then the packetdecoded event will be raised
        private MavlinkPacket ProcessPacketBytes(byte[] packetBytes, byte rxPacketSequence)
        {
            //	 System ID	 1 - 255
            //	 Component ID	 0 - 255
            //	 Message ID	 0 - 255
            //   6 to (n+6)	 Data	 (0 - 255) bytes
            var packet = new MavlinkPacket
            {
                SystemId = packetBytes[0],
                ComponentId = packetBytes[1],
                SequenceNumber = rxPacketSequence,
                Message = _encoder.Deserialize(packetBytes, 2)
            };

            if (PacketReceived != null)
            {
                PacketReceived(this, packet);
            }

            return packet;
        }
Ejemplo n.º 19
0
        private void SendPacket(object msg)
        {
            var packet = new MavlinkPacket
                            {
                                ComponentId = _sourceSystemId,
                                SystemId = _sourceComponentId,
                                Message = msg,
                            };

            _mavlink.Send(packet);
        }
Ejemplo n.º 20
0
 protected void SetStream(Stream stream)
 {
     _stream = stream;
     _mavlink = new Mavlink(stream);
     _mavlink.PacketReceived += (s, e) => {  _latestPacket = e; };
 }
Ejemplo n.º 21
0
        // Called directly after a receive but before sending packet to client
        protected virtual void postMessage(MavlinkPacket packet)
        {
            //msg._timestamp = time.time()
            var type = packet.Message.GetType();
            _messages[type] = packet.Message;

            // TODO: timestamp somehow
            //_timestamp = msg._timestamp;

            // Do I want to do this? i.e always aquire target id?
            //if type == 'HEARTBEAT':
            //    self.target_system = msg.get_srcSystem()
            //    self.target_component = msg.get_srcComponent()
            //    if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
            //        self.flightmode = mode_string_v10(msg)

            if (type == typeof(Msg_param_value))
            {
                var m = (Msg_param_value)packet.Message;
                _parameters[ByteArrayUtil.ToString(m.param_id)] = m.param_value;
                if (m.param_index + 1 == m.param_count)
                {
                    _paramFetchInProgress = false;
                    _paramFetchComplete = true;
                }
            }
            // what is this all about then?
            //           else if (type == typeof(Msg_sys_status) && WIRE_PROTOCOL_VERSION == '0.9' )
            //           {
            //               _flightmode = mode_string_v09(msg);
            //           }

            // Todo: what's all this about then?
            //             elif type == 'GPS_RAW':
            //            if self.messages['HOME'].fix_type < 2:
            //                self.messages['HOME'] = msg

            // Todo: implement message receive 'hook' event
            //for hook in self.message_hooks:
            // hook(self, msg)
        }
Ejemplo n.º 22
0
        // message receive routine
        public MavlinkPacket ReceiveMessage()
        {
            preMessage();

            while (true)
            {
                // Todo: how many bytes to read?
                //var n = _mav.bytes_needed();
                var n = 1;

                var s = new byte[n];
                var bytesRead = _stream.Read(s, 0, n);

                // todo: get this woriking
                //if (bytesRead == 0 && _mav.buf.Length == 0) // Whaaaa?)
                //    return null;

                if (_logfile_raw != null)
                    _logfile_raw.Write(s, 0, s.Length);

                //var packet = _mavnet.ProcessPacketBytes(s, bytesRead);
                _latestPacket = null;
                _mavlink.ParseBytes(s);

                if (_latestPacket != null)
                {
                    postMessage(_latestPacket);
                    return _latestPacket;
                }
            }
        }