QGroundControl doesn't receive my custom messages

#1

Hi,

I’m trying to extend QGC with some custom messages from a JMAVSim simulator.

I build the simulator adding my “TEST” message in the custom.xml:

<message id="9001" name="TEST">
      <description>New Test Message</description>
      <field type="uint32_t" name="value1">Random number</field>
      <field type="uint32_t" name="value2">Random number</field>
      <field type="uint8_t" name="value3">Random number</field>
</message>

and copying the already existing code for the HEARTBEAT, to send my message in MAVLinkSystem.java:

MAVLinkMessage msg_test = new MAVLinkMessage(schema, "TEST", sysId, componentId, protocolVersion);
msg_test.set("value1", 1);
msg_test.set("value2", 3);
msg_test.set("value3", 5);
sendMessage(msg_test); 

At this point I added the same message definition to ground control. For doing this I edited the ardupilotmega.xml with the same message definition, i generated the libs with the pymavgen script end I copied the result in the lib directory of qgc project. Then I build the qgc to test the communication.

RESULT:

From JMAVSim the message seems to be sent correctly (I can get it with wireshark):

1118 0.272100 127.0.0.1 127.0.0.1 TCP 77 4561 → 56597 [PSH, ACK] Seq=22 Ack=1 Win=6378 Len=21 TSval=258240130 TSecr=258240130

Data (21Bytes):
fd 09 00 00 f1 00 33 29 23 00 01 00 00 00 03 00 00 00 05 32 d0

But in QGC I can’t even see the byte stream. I went in:

void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)

that from the doc, seems to be the right function. But if I modify it, adding this piece of code, nothing happens when in debug:

if(b.size() == 21){ //heartbeat or test messages

        if(b[18].operator==(0x05) && b[14].operator==(0x03) && b[10].operator==(0x01)){
            qDebug() << "***** Found msg: TEST(1,3,5) *****"; **//breakpoint here**
        }

        if(b[18].operator==(0x06) && b[14].operator==(0x04) && b[10].operator==(0x02)){
            qDebug() << "***** Found msg: TEST(2,4,6) *****"; **//breakpoint here**
        }

        if(b[18].operator==(0x03)){
            //qDebug() << "***** Found msg: HEARTBEAT(mavlink_v = 3) *****";
        }
}

The breakpoint are never triggered even if I can see the messages flying through wireshark.
Can anyone help me to understand what am I doing wrong?

Thank you!