Set up Mavlink connection and serialize messages

Hi, I am trying to set up a Mavlink connection with my drone via Matlab. I have the following Hardware.

Flight Controller Matek H743 Slim ---> Holybro Telemetry Radio

connecting wirelessly by MAVLink2 protocol with

Holybro Telemetry Radio ---> connected via USB to my PC.

I have a following code to open communication in matlab:

port = "COM33"; % COM Port where USB is connected
baudRate = 57600; 

% Tworzenie obiektu portu szeregowego
serialObj = serialport(port, baudRate);

dialect = mavlinkdialect("common.xml",2); %MAVLink2 protocol
mavlink = mavlinkio(dialect);

buffer = uint8([]); %Read buffer

while true
    if serialObj.NumBytesAvailable > 0 %Check if Message available
        newData = uint8(read(serialObj, serialObj.NumBytesAvailable, "uint8")); %Read new message
        buffer = [buffer newData]; % add message to buffer

        Messages = deserializemsg(dialect, buffer) %deserialize Messsages stack
    end

    pause(0.1); % mała przerwa dla stabilności
end

With this code I am able to receive the messages constantly and see them in the stack. Currently this are only HEARTBEAT (MsgID:0) and TIMESYNC (Msg ID:111).

So technically it works, but maybe I have missed some part and it can be optimised? I would appreciate any help with that.

-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

And now, second part which concerns me more. I would like to command my Drone to start sending other messages with a given frequency. For the purposes of this example let's say it to be ATTITUDE (MsgID:30)

If I understood correctly, I have to send a message of appropriate type to the drone, which is COMMAND_LONG (MsgID:76).

And then fill it with appropriate parameters, which would be

(according to this https://ardupilot.org/dev/docs/mavlink-requesting-data.html -> Using SET_MESSAGE_INTERVAL):

• param1: 30 %This is MsgID I want to be sent
• param2: 100000 %This is time interval
• param3: 0 %Params 3-7 does not matter
• param4: 0
• param5: 0
• param6: 0
• param7: 0
• command: 511 %Type of Command SET_MESSAGE_INTERVAL
• target_system: 1 %default
• target_component: 0 %default
• confirmation: 0 %default

So to prepare this message I use the following code:

msg = createmsg(dialect,76);
msg.Payload.target_system=1;
msg.Payload.command=511;
msg.Payload.param1=30;
msg.Payload.param2=100000;

And when I check msg.Payload i can see that it is Correct:

disp(msg.Payload)
              param1: 30
              param2: 100000
              param3: 0
              param4: 0
              param5: 0
              param6: 0
              param7: 0
             command: 511
       target_system: 1
    target_component: 0
        confirmation: 0

and later I serialize it and write it to the serial port for it to be sent to the drone:

MsgToSend=serializemsg(mavlink,msg);
write(serialObj, DoWyslania,"uint8");

I can see in the message log, that some message was received, as there is "COMMAND_ACK" (MsgID:77) message. However in the message log there are no "ATTITUDE" (MsgID:30) messages, but there appear "STATUSTEXT" (MsgID:253) messages.

I wanted to check if the message I prepared to be sent was serialized correctly I did Deserialize.

Deserialized=deserializemsg(dialect,MsgToSend)

and the result is as follows:

MsgID: 76
SystemID: 255
ComponentID: 1
Payload: [1×1 struct]
Seq: 0

PAYLOAD:
param1: 0
param2: 2.9688
param3: 0
param4: 7.7629
param5: 0
param6: 0
param7: 0
command: 0
target_system: 0
target_component: 0
confirmation: 0

so the MsgID is correct, but Payload is complitely different. Which means that process of Serialize->Deserialize is not reverteing correctly.

Which leads me think that I did something wrong with serialization. However if I repeat the process again with this message (serialize -> deserialize) the result is now the same as it was befor serialization. So this looks weird to me.

I would like to ask for help, as I cannot find what do i do wrong. I would appreciate all answers.

[Walter Roberson]

        buffer = [buffer newData]; % add message to buffer
        Messages = deserializemsg(dialect, buffer) %deserialize Messsages stack

You are growing the buffer each round, and deserializing the grown buffer each time. That is either going to fail (if it is not expecting multiple messages) or else it is going to be redundant and slow, continually going back to the beginning and deserializing everything that has happened so far.

[Michal]

Hi, thank you for your answer. So in this case it is better to desrialize a single message and then add it to stack.

Do you have any thoughts about other parts of the question?

[Walter Roberson]

Sorry, I do not have any ideas about the serialize / deserialize issue.

[Walter Roberson]

I worry about the possibility of a partial message or more than a full message being in the serial port buffer at the time you do the read(serialObj, serialObj.NumBytesAvailable, "uint8"). Are messages a fixed length so you could break up the buffer yourself?

[Michal]

@Walter Roberson thanks again for your answer.

I was able to find out the solution for serialization and it works.

A now, As you predicted, I have landed on another problem. I am able to read continuously a given amount of Messages and then buffer overflows and application is teminated.

So I think I should clear the buffer after each messeage read? The messages are different length depending on what type of message it is.

Answer 1

[Michal]

I was able to find the solution. It is necessary to set a correct type of each variable that we want to serialize.

In this case, the following change was enough:

msg = createmsg(dialect,76);
msg.Payload.target_system=uint8(1);
msg.Payload.command=uint16(511);
msg.Payload.param1=single(30);
msg.Payload.param2=single(1000000);

This allowed me to serialize ↔ deserialize message without losing infromations.

I was able to find it because Simulink Serialize block requires correct data types. So to check them for different messages you can use this method.