fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 15:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink : implement full ping protocol and link latency reporting

Browse Source
This commit is contained in:
Mohammed Kabir
2018-04-07 00:54:05 -04:00
committed by Lorenz Meier
parent 8d6e209888
commit 92457bcafd
5 changed files with 171 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+13
View File
@@ -18,6 +18,7 @@
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
@@ -2027,6 +2028,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("VFR_HUD", 4.0f);
configure_stream("WIND_COV", 1.0f);
configure_stream("CAMERA_IMAGE_CAPTURED");
configure_stream("PING", 0.1f);
break;
case MAVLINK_MODE_ONBOARD:
@@ -2063,6 +2065,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("CAMERA_TRIGGER");
configure_stream("CAMERA_IMAGE_CAPTURED");
configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
configure_stream("PING", 1.0f);
break;
case MAVLINK_MODE_OSD:
@@ -2121,6 +2124,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("CAMERA_IMAGE_CAPTURED");
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
configure_stream("MANUAL_CONTROL", 5.0f);
configure_stream("PING", 1.0f);
break;
case MAVLINK_MODE_IRIDIUM:
@@ -2658,6 +2662,15 @@ Mavlink::display_status()
printf("serial (%s @%i)\n", _device_name, _baudrate);
break;
}
if (_ping_stats.last_ping_time > 0) {
printf("\tping statistics:\n");
printf("\t last: %0.2f ms\n", (double)_ping_stats.last_rtt);
printf("\t mean: %0.2f ms\n", (double)_ping_stats.mean_rtt);
printf("\t max: %0.2f ms\n", (double)_ping_stats.max_rtt);
printf("\t min: %0.2f ms\n", (double)_ping_stats.min_rtt);
printf("\t dropped packets: %u\n", _ping_stats.dropped_packets);
}
}
int
src/modules/mavlink/mavlink_main.h
+17
View File
@@ -480,6 +480,21 @@ public:
bool ftp_enabled() const { return _ftp_on; }
struct ping_statistics_s {
uint64_t last_ping_time;
uint32_t last_ping_seq;
uint32_t dropped_packets;
float last_rtt;
float mean_rtt;
float max_rtt;
float min_rtt;
};
/**
* Get the ping statistics of this MAVLink link
*/
struct ping_statistics_s &get_ping_statistics() { return _ping_stats; }
protected:
Mavlink *next;
@@ -582,6 +597,8 @@ private:
struct telemetry_status_s _rstatus; ///< receive status
struct ping_statistics_s _ping_stats; ///< ping statistics
struct mavlink_message_buffer {
int write_ptr;
int read_ptr;
src/modules/mavlink/mavlink_messages.cpp
+67 -1
View File
@@ -4063,6 +4063,71 @@ protected:
}
};
class MavlinkStreamPing : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamPing::get_name_static();
}
static const char *get_name_static()
{
return "PING";
}
static uint16_t get_id_static()
{
return MAVLINK_MSG_ID_PING;
}
uint16_t get_id()
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamPing(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_PING_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
bool const_rate()
{
return true;
}
private:
uint32_t _sequence;
/* do not allow top copying this class */
MavlinkStreamPing(MavlinkStreamPing &);
MavlinkStreamPing &operator = (const MavlinkStreamPing &);
protected:
explicit MavlinkStreamPing(Mavlink *mavlink) : MavlinkStream(mavlink),
_sequence(0)
{}
bool send(const hrt_abstime t)
{
mavlink_ping_t msg = {};
msg.time_usec = hrt_absolute_time();
msg.seq = _sequence++;
msg.target_system = 0; // All systems
msg.target_component = 0; // All components
mavlink_msg_ping_send_struct(_mavlink->get_channel(), &msg);
return true;
}
};
static const StreamListItem streams_list[] = {
StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static),
StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static),
@@ -4113,7 +4178,8 @@ static const StreamListItem streams_list[] = {
StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static),
StreamListItem(&MavlinkStreamMountOrientation::new_instance, &MavlinkStreamMountOrientation::get_name_static, &MavlinkStreamMountOrientation::get_id_static),
StreamListItem(&MavlinkStreamHighLatency2::new_instance, &MavlinkStreamHighLatency2::get_name_static, &MavlinkStreamHighLatency2::get_id_static),
StreamListItem(&MavlinkStreamGroundTruth::new_instance, &MavlinkStreamGroundTruth::get_name_static, &MavlinkStreamGroundTruth::get_id_static)
StreamListItem(&MavlinkStreamGroundTruth::new_instance, &MavlinkStreamGroundTruth::get_name_static, &MavlinkStreamGroundTruth::get_id_static),
StreamListItem(&MavlinkStreamPing::new_instance, &MavlinkStreamPing::get_name_static, &MavlinkStreamPing::get_id_static)
};
const char *get_stream_name(const uint16_t msg_id)
src/modules/mavlink/mavlink_receiver.cpp
+72 -12
View File
@@ -125,6 +125,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_vision_position_pub(nullptr),
_vision_attitude_pub(nullptr),
_telemetry_status_pub(nullptr),
_ping_pub(nullptr),
_rc_pub(nullptr),
_manual_pub(nullptr),
_obstacle_distance_pub(nullptr),
@@ -1367,6 +1368,77 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
{
mavlink_ping_t ping;
mavlink_msg_ping_decode(msg, &ping);
if ((ping.target_system == 0) &&
(ping.target_component == 0)) { // This is a ping request. Return it to the system which requested the ping.
ping.target_system = msg->sysid;
ping.target_component = msg->compid;
mavlink_msg_ping_send_struct(_mavlink->get_channel(), &ping);
} else if ((ping.target_system == mavlink_system.sysid) &&
(ping.target_component ==
mavlink_system.compid)) { // This is a returned ping message from this system. Calculate latency from it.
const hrt_abstime now = hrt_absolute_time();
// Calculate round trip time
float rtt_ms = (now - ping.time_usec) / 1000.0f;
// Update ping statistics
struct Mavlink::ping_statistics_s &pstats = _mavlink->get_ping_statistics();
pstats.last_ping_time = now;
if (pstats.last_ping_seq == 0 && ping.seq > 0) {
// This is the first reply we are receiving from an offboard system.
// We may have been broadcasting pings for some time before it came online,
// and these do not count as dropped packets.
// Reset last_ping_seq counter for correct packet drop detection
pstats.last_ping_seq = ping.seq - 1;
}
// We can only count dropped packets after the first message
if (ping.seq > pstats.last_ping_seq) {
pstats.dropped_packets += ping.seq - pstats.last_ping_seq - 1;
}
pstats.last_ping_seq = ping.seq;
pstats.last_rtt = rtt_ms;
pstats.mean_rtt = (rtt_ms + pstats.mean_rtt) / 2.0f;
pstats.max_rtt = fmaxf(rtt_ms, pstats.max_rtt);
pstats.min_rtt = pstats.min_rtt > 0.0f ? fminf(rtt_ms, pstats.min_rtt) : rtt_ms;
/* 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 uorb_ping_msg = {};
uorb_ping_msg.timestamp = now;
uorb_ping_msg.ping_time = ping.time_usec;
uorb_ping_msg.ping_sequence = ping.seq;
uorb_ping_msg.dropped_packets = pstats.dropped_packets;
uorb_ping_msg.rtt_ms = rtt_ms;
uorb_ping_msg.system_id = msg->sysid;
uorb_ping_msg.component_id = msg->compid;
if (_ping_pub == nullptr) {
int multi_instance;
_ping_pub = orb_advertise_multi(ORB_ID(ping), &uorb_ping_msg, &multi_instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(ping), _ping_pub, &uorb_ping_msg);
}
}
}
}
void
MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
{
@@ -1711,18 +1783,6 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
{
mavlink_ping_t ping;
mavlink_msg_ping_decode(msg, &ping);
if ((mavlink_system.sysid == ping.target_system) &&
(mavlink_system.compid == ping.target_component)) {
mavlink_msg_ping_send_struct(_mavlink->get_channel(), &ping);
}
}
int
MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate)
{
src/modules/mavlink/mavlink_receiver.h
+2
View File
@@ -66,6 +66,7 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/ping.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/debug_vect.h>
@@ -224,6 +225,7 @@ private:
orb_advert_t _vision_position_pub;
orb_advert_t _vision_attitude_pub;
orb_advert_t _telemetry_status_pub;
orb_advert_t _ping_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
orb_advert_t _obstacle_distance_pub;