Mavsdk: Routing custom dialect with MavlinkPassthrough

[depauwjimmy]

Hello,

It's the first time woking with MAVLink in general and i am not sure what is the cause of my problem.
Basically i have a companion computer and a ground station that need to talk using custom messages.
I managed to get them to talk to each other but only using common messages.
My companion computer gets system id 1 and component 240 while the GS gets 190/191 and they get both discovered.
From the GS i receive heartbeat and timesync message.
On a side note, about timesync i always receive [12:02:24|Warn ] RTT too high for timesync: 1.57917e+12 ms. (timesync.cpp:87) on both side.

So i tried sending a simple STATUSTEXT message from companion computer and i receive it just fine.
If i try with my own dialect, message is accepted but i can't see it arriving anywhere.
I generated headers for my dialect and add them to my projects (not in mavsdk)
Since my messages are only meant for the GS, i added target_system and target_component fields.

Documentation about creating your own dialect is sparse so i have no idea how can i debug this.
Thanks for your help.

[JonasVautherin]

Did you subscribe_message_async to your custom message?

    /**
     * @brief Subscribe to messages using message ID.
     *
     * This means that all future messages being received will trigger the
     * callback to be called. To stop the subscription, call this method with
     * `nullptr` as the argument.
     *
     * @param message_id The MAVLink message ID.
     * @param callback Callback to be called for message subscription.
     */
    void subscribe_message_async(
        uint16_t message_id, std::function<void(const mavlink_message_t&)> callback);

[depauwjimmy]

Yes i did. Since it was never being called i used intercept_incoming_messages_async to print everything.
I receive all sorts of message from the autopilot alongside HEARTBEAT and TIMESYNC message from my companion computer.
I also sent a STATUSTEXT message from the companion computer and i also got it.

Since i could see messages coming from the companion computer on TELEM2 on my little test program connected on TELEM1 i figure it's properly configured on the autopilot and routing is working.

I also added my own dialect to QGroundControl and rebuild it. In mavlink inspector again i don't see any custom message.
Is there anything to do on the vehicle firmware so custom message a routed? I am using 1.11.0Beta because 1.10 would not boot at all.
Is there a list of things i can verify to make sure it is supposed to work?
I am reading documentation for the past week now and cannot figure it out.

[julianoes]

@depauwjimmy all MAVLink nodes talking or routing the custom messages need to be aware of the custom messages by using the custom headers. If not, the messages won't be parsed.

For MAVSDK, this means that you need to swap out the c_library_v2 submodule with your custom messages.

Does that make sense?

[depauwjimmy]

So it means i have to generate C headers for my dialect and put them in

• MAVSDK
  In the folder MAVSDK/src/third_party/mavlink/include/mavlink/v2.0 then compile and install

• Firmware
  In the folder Firmware/mavlink/include/mavlink/v2.0 then compile for my board and flash

I just done it but it makes no difference so i am probably missing something.
How can i be certain my dialect is included?

[julianoes]

And you need to make sure the dialect is included:
https://github.com/mavlink/MAVSDK/blob/601dd3e3bce36a1515b26e6579beb3eaa4e29550/src/core/mavlink_include.h#L7

https://github.com/PX4/Firmware/blob/1cbb3ebd4f5b2118cfc851ddbf1230be1b26d9d9/src/modules/mavlink/mavlink_bridge_header.h#L89

[depauwjimmy]

So i changed the include in those 2 files, flashed/re-installed and compiled everything back.
Still the same behavior, no changes.
I really don't know what to do.

Update : It works but only if i put 0/0 for target_system and target_component.
If i use 190/191 it does not work.

Since my dialect is only between the GS and companion computer i would like to avoid a total broadcast.

[JonasVautherin]

How is your network setup? I mean, does the companion go through PX4 to send messages to the GCS? I don't know exactly how it works, but the fact that a broadcast works and not a directed message sounds like there is forwarding somewhere :thinking:. Could it be?

[depauwjimmy]

Companion computer is on TELEM2 (MAV_1_FORWARD to 1, MAV_1_MODE Onboard, MAV_1_RATE 0) GCS is on TELEM1 and is currently a test program running on my computer using an FTDI cable (MAV_0_FORWARD to 1, MAV_0_MODE Normal, MAV_0_RATE 1200B/s)

Could it be because both TELEM1 and TELEM2 have forward enabled?
I am not sure to understand what this parameter actually does.

Another thing is i don't seem to have lost message from CC to GCS but the other way around it's very common and only receive 1 out of 3 custom message.

PS: Should i care about the spamming of this warning?

RTT too high for timesync: 1.57952e+12 ms

[irsdkv]

RTT too high for timesync: 1.57952e+12 ms

Hm. I have no idea why RTT can be so high.
But I think that we can disable syncing for default. Drone still will can initiate time syncing.
@JonasVautherin @julianoes should I disable time syncing on MAVSDK side by default?

[julianoes]

To me it seems like the rates might be too high that you are sending.

You can debug these rates in the nsh console (e.g. mavlink_shell or a console) by doing:

mavlink status

Best would be if you can paste the output here.

I'm not too sure what messages the autpilot forwards, I suggest you check the code there to see what it forwards:
https://github.com/PX4/Firmware/blob/44cac04abe70c928135822ae6e55af7d45e5b644/src/modules/mavlink/mavlink_main.cpp#L468-L485

[JonasVautherin]

Hm. I have no idea why RTT can be so high.

@irsdkv: I'm not sure what the timesync does, nor what it uses, but 1.57952e+12 seems like the time since 1970: https://currentmillis.com :sweat_smile:. Does that mean that autopilot time started at 0? Is it expected behavior, or does that show a bug in the way MAVSDK deals with the timesync?

[irsdkv]

I was checked timesynk.cpp and all seems ok.
Also I checked RTT for default PX4 Firmware and develop MAVSDK branch.
It is also ok:

[11:31:20|Info ] System discovered [UUID: 3546673858082977847] (connection_initiator.h:61)
[11:31:25|Warn ] RTT ok for timesync: 3.096 ms. (timesync.cpp:78)
[11:31:30|Warn ] RTT ok for timesync: 5.47 ms. (timesync.cpp:78)
[11:31:35|Warn ] RTT ok for timesync: 5.978 ms. (timesync.cpp:78)
[11:31:40|Warn ] RTT ok for timesync: 5.707 ms. (timesync.cpp:78)
[11:31:45|Warn ] RTT ok for timesync: 5.724 ms. (timesync.cpp:78)

I suppose that problem can be in the modified (if it modified) MavLink libraries or in the one of components.
Because of it looks like ts1 in TIMESYNC message filled by zero (thank you @JonasVautherin for pointing to 1970 :) ) instead of copy of already received ts1:

rsync.tc1 = now * 1000ULL;
rsync.ts1 = tsync.ts1;
mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync);

https://github.com/PX4/Firmware/blob/07d656e971a72d1202651dfd3b4642736fb078d7/src/modules/mavlink/mavlink_timesync.cpp#L67

@depauwjimmy if you can provide your modifications I will try to find problem, at least related to timesync

[depauwjimmy]

@julianoes here's the result when ma companion computers is sending some data.

instance #0:
    mavlink chan: #0
    no radio status.
    flow control: OFF
    rates:
      tx: 0.000 kB/s
      txerr: 0.000 kB/s
      tx rate mult: 1.000
      tx rate max: 800000 B/s
      rx: 0.000 kB/s
    FTP enabled: YES, TX enabled: YES
    mode: Config
    MAVLink version: 1
    transport protocol: serial (/dev/ttyACM0 @57600)

instance #1:
    GCS heartbeat:  613766 us ago
    mavlink chan: #1
    type:       GENERIC LINK OR RADIO
    flow control: OFF
    rates:
      tx: 1.452 kB/s
      txerr: 0.000 kB/s
      tx rate mult: 0.712
      tx rate max: 1200 B/s
      rx: 0.016 kB/s
    FTP enabled: YES, TX enabled: YES
    mode: Normal
    MAVLink version: 1
    transport protocol: serial (/dev/ttyS1 @57600)

instance #2:
    GCS heartbeat:  756832 us ago
    mavlink chan: #2
    type:       GENERIC LINK OR RADIO
    flow control: OFF
    rates:
      tx: 24.226 kB/s
      txerr: 0.000 kB/s
      tx rate mult: 1.000
      tx rate max: 46080 B/s
      rx: 0.395 kB/s
    FTP enabled: YES, TX enabled: YES
    mode: Onboard
    MAVLink version: 2
    transport protocol: serial (/dev/ttyS2 @921600)

I guess 1200 B/s is too low but i just left the default value.

Also i checked the forward code. To me it seems that message for different system id are not forwarded which make my behavior normal.
My system id for AP and CC is 1 but my test code (configured as GroundStation) is 190.
Therefore i can send whatever i want, the firmware will discard that message.
Can i set my own sys/comp id in mavlinkpassthrough or do i need to edit the source code?

@irsdkv I had those messages in the beginning without any custom changes. Even now my changes are only adding my dialect. It imports common and add (for now 8 messages, 1 enum)
I took common.xml from MAVSDK repo.
The big thing is i am using a Firmware 1.11Beta from master (commit url https://github.com/PX4/Firmware/commit/3387c9599cc09c1171a41617d1d30c4c7b01fd22)
The last commit does not compile so can't test it. I am forced to use a beta Firmware because the last stable does not boot at all.
The timesync problem may originate from this.

Thanks you all for the support, really appreciate it.

[JonasVautherin]

Where does an autopilot get the time from? GPS? What if there is no GPS?

[depauwjimmy]

I could never receive message from GS to CC even with targets to 0 now even though i could get some at random before.
CC to GS always worked with targets to 0.

So i changed the MAVSDK codes to have my GS on system 1 as well ... and it works a lot better.
Direct targeted messages now works both ways.
I think it would be great to allow set of systemid and componentid manually to avoid such problems.

I don't know what would be the recommended rate, i have set both TELEM ports to 0.
Also managed to flash the last beta firmware but still have those timesync warnings.

[JonasVautherin]

Also managed to flash the last beta firmware but still have those timesync warnings.

What's the time on your autopilot? Where does it come from? Do you have a GPS?

[depauwjimmy]

@JonasVautherin I have a Here2 GPS connected on GPS1. For how to setup or check the AP time, i have no idea. Is it possible using GroundControl?

[JonasVautherin]

I don't know how the timesync works, maybe @irsdkv has a comment on that :confused:.

I think it would be great to allow set of systemid and componentid manually to avoid such problems.

I agree that it would be great to allow setting the ids, but I believe that the GCS should not have the same sys_id as the drone. In your case I believe you have an issue with the forwarding :sweat_smile:

[irsdkv]

Hm. I have an assumption.

1. Companion computer sends TIMESYNC request to Autopilot. Companion have a zero-time (not a current world-time) and ts1 is about zero.
2. Autopilot then send TIMESYNC response to MAVSDK which receive it and handle it as if request has been sent from itself (see this).

It can explain why ts1 so low. I can be wrong with some details such as directions of requests, but in common that can explain this behavior.

I'm a little lost the thread of conversation, but I think that some problems may be with MAVLINK streams on Autopilot.

[julianoes]

@depauwjimmy please open a new thread for the timesync issue.

@depauwjimmy if you can explain to me exactly where the target id problem is in mavsdk, I'm happy to fix this but I don't entirely understand it.

[depauwjimmy]

Ok i'll open a new thread for the timesync.

For the forwarding, as you pointed out in the firmware code
https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_main.cpp#L468-L485

Message from different system id will simply not be forwarded at all.
This means that a message sent from GS (sys_id 190) will never reach my companion computer (sys_id 1)
Right now i simply cannot choose the sys_id myself and i am forced to use 0 for both my target_system and target_component value.
I don't know why this is a choice made from the firmware perspective.

I had some weird problem with message from the CC to my GS test program but since it worked a few time it's another issue entirely. My guess would be the beta firmware version i was using at the time.

In short :

• Targeted message between CC and GS => never works
• Message to all systems => works
• Modified MAVSDK to set GS as system_id 1, targeted messages => works

[julianoes]

@depauwjimmy thanks for explaining that. It's now clear to me. And I'm thinking of raising it for PX4 or creating a pull request to change it. I Don't understand why it is that way.

[julianoes]

@depauwjimmy could you test if it works with https://github.com/PX4/Firmware/pull/14016?

[depauwjimmy]

@julianoes This is so weird.
So with your change targeted message from CC to GS work fine.
But targeted message from GS to CC are lost.

Without changing anything but the system_id of my GS to 1, all is working fine.
Seems to me there is another part of the code that influence message forwarding.

[julianoes]

Interesting, would be great if you could debug where in PX4 it gets dropped.

[depauwjimmy]

Would love to but i am clueless on how i could debug the firmware while it is running on my cube.
Can i have breakpoints on some sort to observe execution of firmware code?

[julianoes]

I would build the firmware myself and add printfs where it is interesting, or where I suspect something.
You can also do breakpoints with gdb, even using VSCode if you debug it using SITL:
https://dev.px4.io/master/en/setup/vscode.html#debugging_sitl

[depauwjimmy]

The simple things would be to add logging information but i really can't figure out where there is log at all.
I can see logging in the current code like PX4_DEBUG() but where does it write it? I have no idea and could not find that information.
Nothing of interest appears on the SDCARD.
For VSCode i am not sure but can i debug a firmware running on my hardware from it?

[julianoes]

PX4_DEBUG() but where does it write it

PX4_DEBUG are only included in the debug build. I can see how that is a bit confusing.

[TechNaturalist]

@depauwjimmy
I am so sorry to be that guy, I was wondering how was it you were able to change the target system to broadcast. I am also new to MavSDK and I have yet to figure out how to do that.

I am attempting to do something similar to what you are implementing and I seem to be stuck there. I have scoured the forums, the git, and wiki and I have nothing to show for it.

Truly, truly sorry for not being able to figure it out myself.

[depauwjimmy]

@TechNaturalist
I was never able to really debug the firmware to find why forwarding now works one way but not the other way.
I am currently trying to do a SITL using AirSim but no luck so far. AirSim documentation for PX4 is outdated and SITL documentation is sparse.

Anyway i can't fly anyway since i don't have a stable firmware yet (thus the simulator)

But to make progress in my onboard software you can go to SDK code in src/core/mavsdk_impl.cpp
In MavsdkImpl::get_own_system_id() just change

case Mavsdk::Configuration::GroundStation:
    return MAV_COMP_ID_MISSIONPLANNER;

to

case Mavsdk::Configuration::GroundStation:
    return 1;

It's dirty but at least doing this makes targeted messages from my custom dialect work both ways.

[TechNaturalist]

@depauwjimmy

Thank you so much! I will give it a try. I'm sorry to hear about your lack of working firmware. I will see if I come across anything new from this.

I appreciate your timely response immensely!

Best of luck to you!

[julianoes]

SITL using AirSim

This should actually work if you use latest AirSim (compiled, not the pre-built release).

[julianoes]

I'm going to close this thread because it has been silent for a while. Feel free to re-open and comment for more updates.

I would also pick https://github.com/mavlink-router/mavlink-router here as this might be an alternative for anyone trying to forward messages.