Mavlink communications

Hi,

So I am trying to retrieve sensor information from my ArduCopter to my c# application and as such need to use Mavlink.

From the Mission Planner source I have found functions to create the packets with check-sums and all. Once connected I can see my ArduCopter streaming heart beat messages. As I am a newbie to Mavlink, how do I get it to stream sensor info or how can I poll the ArduCopter? Send messages with no payload?

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –

Replies

  • Developer

    This is a very oldd post try here http://discuss.ardupilot.org/c/ground-control-software/mavlink and ask the question

    and this post http://discuss.ardupilot.org/t/mavlink-step-by-step/9629

    Hint: pocketed is actually packet sequence so declaring it as a global byte starting at zero and make the line  Packet[2] = packetid++; means the sequence goes up each packet


    endrie said:

    Hi Jan, could you show me how you declare variable "packetid" ???

    is that integer or byte or what ??

    thanks

    Jan Swanepoel said:

    Hi Ryan,

    Here is what I have done and the functions that handle the sending of messages.

           public void requestDatastream(byte id, byte rate)
            {
                mavlink_request_data_stream_t rqstDataStream = new mavlink_request_data_stream_t
                {
                    req_message_rate = rate,
                    req_stream_id = id,
                    start_stop = 1,
                    target_system = 1,
                    target_component = 0,
                };

                byte[] packet = GeneratePacket(rqstDataStream, MAVLINK_MSG_ID_REQUEST_DATA_STREAM);

                SendPacket(packet);
            }

           public byte[] GeneratePacket(byte[] packetData, byte msgID)
            {
                byte[] Packet = new byte[packetData.Length + 6 + 2];

                Packet[0] = (byte)254;
                Packet[1] = (byte)packetData.Length;
                Packet[2] = (byte)packetid;
                Packet[3] = (byte)255;
                Packet[4] = (byte)1;
                Packet[5] = msgID;

                for (int n = 0; n < packetData.Length; n++)
                    Packet[6 + n] = packetData[n];

                ushort checksum = MavlinkCRC.crc_calculate(Packet, Packet[1] + 6);

                checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[msgID], checksum);
                
                byte ck_a = (byte)(checksum & 0xFF); ///< High byte
                byte ck_b = (byte)(checksum >> 8); ///< Low byte

                Packet[Packet.Length - 2] = ck_a;
                Packet[Packet.Length - 1] = ck_b;

                return Packet;
            }

    //Overloaded function
            public byte[] GeneratePacket(object packetData, byte msgID)
            {
                int len = System.Runtime.InteropServices.Marshal.SizeOf(packetData);
                byte[] arr = new byte[len];
                IntPtr ptr = System.Runtime.InteropServices.Marshal.AllocHGlobal(len);
                System.Runtime.InteropServices.Marshal.StructureToPtr(packetData, ptr, true);
                System.Runtime.InteropServices.Marshal.Copy(ptr, arr, 0, len);
                System.Runtime.InteropServices.Marshal.FreeHGlobal(ptr);

                return GeneratePacket(arr, msgID);

            }


            public bool SendPacket(byte[] packetData)
            {
                try
                {
                    if (MAVlinkPort.IsOpen)
                    {
                        MAVlinkPort.Write(packetData, 0, packetData.Length);
                        return true;
                    }
                }
                catch
                {
                    return false;
                }
                return false;
            }

    The serial port is created as:

            public bool Connect(string COMPort, int BaudRate)
            {
                try
                {
                    MAVlinkPort = new System.IO.Ports.SerialPort(COMPort, BaudRate);
                    MAVlinkPort.DataReceived += new System.IO.Ports.SerialDataReceivedEventHandler(MAVlinkPort_DataReceived);
                    MAVlinkPort.DtrEnable = true;   //Required to communicate with ArduPilot
                    MAVlinkPort.RtsEnable = true;
                    MAVlinkPort.Open();
                    MAVlinkPort.DiscardInBuffer();
                    IsConnected = true;
                    return true;
                }
                catch
                {
                    return false;
                }
            }

    I am also very new to Mavlink but this has worked for me. I usually also wait a few seconds after the autopilot has booted before sending my first messages.

    This is how the packet for requesting to stream all data looks:

    254 6 0 255 1 66 20 0 1 0 0 1 183 129

    Hope this can help.

  • Hi Jan, could you show me how you declare variable "packetid" ???

    is that integer or byte or what ??

    thanks

    Jan Swanepoel said:

    Hi Ryan,

    Here is what I have done and the functions that handle the sending of messages.

           public void requestDatastream(byte id, byte rate)
            {
                mavlink_request_data_stream_t rqstDataStream = new mavlink_request_data_stream_t
                {
                    req_message_rate = rate,
                    req_stream_id = id,
                    start_stop = 1,
                    target_system = 1,
                    target_component = 0,
                };

                byte[] packet = GeneratePacket(rqstDataStream, MAVLINK_MSG_ID_REQUEST_DATA_STREAM);

                SendPacket(packet);
            }

           public byte[] GeneratePacket(byte[] packetData, byte msgID)
            {
                byte[] Packet = new byte[packetData.Length + 6 + 2];

                Packet[0] = (byte)254;
                Packet[1] = (byte)packetData.Length;
                Packet[2] = (byte)packetid;
                Packet[3] = (byte)255;
                Packet[4] = (byte)1;
                Packet[5] = msgID;

                for (int n = 0; n < packetData.Length; n++)
                    Packet[6 + n] = packetData[n];

                ushort checksum = MavlinkCRC.crc_calculate(Packet, Packet[1] + 6);

                checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[msgID], checksum);
                
                byte ck_a = (byte)(checksum & 0xFF); ///< High byte
                byte ck_b = (byte)(checksum >> 8); ///< Low byte

                Packet[Packet.Length - 2] = ck_a;
                Packet[Packet.Length - 1] = ck_b;

                return Packet;
            }

    //Overloaded function
            public byte[] GeneratePacket(object packetData, byte msgID)
            {
                int len = System.Runtime.InteropServices.Marshal.SizeOf(packetData);
                byte[] arr = new byte[len];
                IntPtr ptr = System.Runtime.InteropServices.Marshal.AllocHGlobal(len);
                System.Runtime.InteropServices.Marshal.StructureToPtr(packetData, ptr, true);
                System.Runtime.InteropServices.Marshal.Copy(ptr, arr, 0, len);
                System.Runtime.InteropServices.Marshal.FreeHGlobal(ptr);

                return GeneratePacket(arr, msgID);

            }


            public bool SendPacket(byte[] packetData)
            {
                try
                {
                    if (MAVlinkPort.IsOpen)
                    {
                        MAVlinkPort.Write(packetData, 0, packetData.Length);
                        return true;
                    }
                }
                catch
                {
                    return false;
                }
                return false;
            }

    The serial port is created as:

            public bool Connect(string COMPort, int BaudRate)
            {
                try
                {
                    MAVlinkPort = new System.IO.Ports.SerialPort(COMPort, BaudRate);
                    MAVlinkPort.DataReceived += new System.IO.Ports.SerialDataReceivedEventHandler(MAVlinkPort_DataReceived);
                    MAVlinkPort.DtrEnable = true;   //Required to communicate with ArduPilot
                    MAVlinkPort.RtsEnable = true;
                    MAVlinkPort.Open();
                    MAVlinkPort.DiscardInBuffer();
                    IsConnected = true;
                    return true;
                }
                catch
                {
                    return false;
                }
            }

    I am also very new to Mavlink but this has worked for me. I usually also wait a few seconds after the autopilot has booted before sending my first messages.

    This is how the packet for requesting to stream all data looks:

    254 6 0 255 1 66 20 0 1 0 0 1 183 129

    Hope this can help.

  • Hello folks,

     

    you can find the last version of my MAVLinkJava project here :

    https://code.google.com/p/mavlinkjava/

     

    Best

     

    Guillaume

    • Hello, 

      Since Im doing something really similar Im gonna post my problem here:

      I wanna to retrieve altitude of ardupilot. When I connect to APM, I can see in telemetry the altitude and if I move ardupilot in my hands, altitude is changing. So my goal is to print all that. I found out that the float altitude is in mavlink_msg_vfr_hud.h. So far its only printing: "ALTITUDE: 0.0"

      Here is what I have so far:

      #include "/Users/Gabriele/Documents/Arduino/libraries/ArdunioMAVLink/libraries/mavlink/include/mavlink.h" 

      #include <mavlink_msg_vfr_hud.h>

      int system_type = MAV_QUADROTOR;

      int autopilot_type = MAV_AUTOPILOT_GENERIC;

      uint16_t len;

      mavlink_message_t msg; 

      uint8_t buf[MAVLINK_MAX_PACKET_LEN];

      mavlink_status_t status;

      float mavlink_msg_vfr_hud_get_alt;

      char buffer[32];

      void readSerialMavLink();

      void setup() { 

        Serial.begin(115200);

        Serial.println("Booting...");

      }

      void loop() {

          readSerialMavlink();

          delay(1000); 

      }

      void readSerialMavlink() {

        while(Serial.available() > 0) {

         Serial.println("Serial available"); 

                Serial.print("ALTITUTDE: ");

                Serial.println((dtostrf(mavlink_msg_vfr_hud_get_alt,10,2,buffer))); 

                }        

                   }


  • Hi

    This is cool! But does anyone have a simple vb.net example of send and recieve like this ?

    Tommy

    VB.NET Shop
  • From gcs-copter Java:

    - from APM heartbeat get info:

    MAVLink.CURRENT_SYSID = m.sysID;
    MAVLink.ARDUCOPTER_COMPONENT_ID = m.componentID;

    - send a heartbeat presenting yourself:

    msg_heartbeat msg = new msg_heartbeat();
    msg.type = MAVLink.MAV_TYPE.MAV_TYPE_GCS;
    msg.autopilot = MAVLink.MAV_AUTOPILOT.MAV_AUTOPILOT_GENERIC;
    sendBytesToComm(MAVLink.createMessage(msg));

    - and request all packets or only the ones you are interested in:

                    msg_request_data_stream req = new msg_request_data_stream();
                    req.req_message_rate = 20;
                    req.req_stream_id = MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_ALL;
                    req.start_stop = 1;
                    req.target_system = MAVLink.CURRENT_SYSID;
                    req.target_component = 0;
                    sendBytesToComm( MAVLink.createMessage(req));

This reply was deleted.

Activity