Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Mavlink Ping Protocol Implementation #9399

Merged
merged 3 commits into from
May 15, 2018
Merged

Mavlink Ping Protocol Implementation #9399

merged 3 commits into from
May 15, 2018

Conversation

mhkabir
Copy link
Member

@mhkabir mhkabir commented May 3, 2018

This implements the full ping protocol from MAVLink so that we can accurately measure link quality. Status is published to the ping uORB topic and logged, and is also available from the system console using mavlink status :

Example :

pxh> mavlink status

instance #0:
GCS heartbeat: 720751 us ago
mavlink chan: #0
type: GENERIC LINK OR RADIO
flow control: OFF
rates:
tx: 31.229 kB/s
txerr: 0.000 kB/s
rx: 0.047 kB/s
rate mult: 1.000
accepting commands: YES, FTP enabled: YES
transmitting enabled: YES
mode: Normal
MAVLink version: 2
transport protocol: UDP (14556)
ping statistics:
last: 152.93 ms
mean: 138.95 ms
max: 184.96 ms
min: 2.78 ms
dropped packets: 9

@mhkabir
Copy link
Member Author

mhkabir commented May 3, 2018

@dagar @bkueng What do you think about rolling telemetry_status and ping into one single link_health topic, with the radio-specific fields only being filled if the link is wireless.

We might also want to enable a 1Hz ping on the low-bandwidth telemetry links (e.g SiK radios) for latency measurement and packet drop statistics.

@mhkabir mhkabir force-pushed the pr-ping-protocol branch 2 times, most recently from ddd9b91 to 210317c Compare May 3, 2018 03:59
@dagar
Copy link
Member

dagar commented May 3, 2018

Yes I think that makes sense. It's come up before in the context of iridium status. The current telemetry_status has quite a few fields from RADIO_STATUS that don't apply generally.

  • rename telemetry_status to mavlink_status (or similar) stripped down to common fields.
  • create a new radio_status (and iridium_status, etc) to use when appropriate

@dagar
Copy link
Member

dagar commented May 3, 2018

Streaming ping at 10 Hz? Shouldn't it only be responding to pings?

@hamishwillee
Copy link
Contributor

hamishwillee commented May 3, 2018

@mhkabir Once this is accepted, can we please have a summary of the Ping protocol as a sub page of https://mavlink.io/en/protocol/overview.html ?

(assuming that makes sense)

@mhkabir
Copy link
Member Author

mhkabir commented May 3, 2018

@dagar It is sending pings requests at 10Hz on onboard links. A companion system will bounce these back and we can calculate latency from that.

@hamishwillee Will do!

Copy link
Member

@bkueng bkueng left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm also wondering whether we really need 10Hz?

What do you think about rolling telemetry_status and ping into one single link_health topic, with the radio-specific fields only being filled if the link is wireless.

Makes sense.

@@ -480,6 +480,21 @@ class Mavlink

bool ftp_enabled() const { return _ftp_on; }

struct ping_statistics {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I suggest to rename to something like ping_statistics_t for clarity

/* Ping status is supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */
if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {

struct ping_s png = {};
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

uorb_ping_message and mavlink_ping_message instead of png?

(ping.target_component ==
mavlink_system.compid)) { // This is a returned ping message from this system. Calculate latency from it.

double rtt_ms = (hrt_absolute_time() - ping.time_usec) / 1000.0;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you directly use float here?

// Update ping statistics
struct Mavlink::ping_statistics &pstats = _mavlink->get_ping_statistics();

pstats.last_ping_time = hrt_absolute_time();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you calculate the time only once in this code block?
const hrt_abstime now = hrt_absolute_time();

pstats.last_ping_seq = ping.seq - 1;
}

pstats.dropped_packets += (ping.seq - pstats.last_ping_seq - 1);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

unnecessary brackets

@dagar dagar removed this from the Release v1.8.0 milestone May 4, 2018
@mhkabir
Copy link
Member Author

mhkabir commented May 12, 2018

@bkueng All addressed. I also reduced rate of ping on onboard/USB links to 2Hz. Please review and merge. :)

@LorenzMeier
Copy link
Member

Is there any reason it needs to be sent regularly? On a radio link, this already consumes some bandwidth. 0.1 Hz would be more than sufficient for normal instrumentation.

@mhkabir
Copy link
Member Author

mhkabir commented May 12, 2018

The reason why I implemented the ping in the first place was to debug transient spikes in the link quality on a companion link, which would usually last under 1s. I can reduce it to 0.1Hz for radio links if desired.

@LorenzMeier
Copy link
Member

That makes sense - let's use different rates for the different configurations. We have the distinction already, so let's leverage it.

@mhkabir
Copy link
Member Author

mhkabir commented May 14, 2018

Updated PR with reduced rates. Please review @LorenzMeier @bkueng.

@mhkabir
Copy link
Member Author

mhkabir commented May 14, 2018

It seems like the SiK radios use the "normal" mode. This degree of distinction seems insufficient to me. Even SITL uses the "normal" mode, and reducing the rate to 0.1Hz in SITL is unnecessary. Can we have a separate mode for the SiK radios if we don't have one already? @dagar Thoughts?

@LorenzMeier
Copy link
Member

It's not because SITL needs to run in the same mode as the radio (otherwise its not anymore simulating the system).

@mhkabir
Copy link
Member Author

mhkabir commented May 15, 2018

Okay, in that case, this is ready to merge.

@LorenzMeier LorenzMeier merged commit bbcf5b1 into master May 15, 2018
@LorenzMeier LorenzMeier deleted the pr-ping-protocol branch May 15, 2018 16:57
@hamishwillee
Copy link
Contributor

Cool. @mhkabir don't forget mavlink/mavlink-devguide#52 :-)

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants