From f361749d703673d5305175e995bb0c06fabf8bf9 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 17 Sep 2018 19:24:20 +0200 Subject: [PATCH] mavlink temporary workarounds for dronekit: added parameters to disable (#10422) hash check and heartbeat forwarding - hash check disabling: for systems where a companion link forwards messages to QGC (e.g. via LTE) parameter streaming will be stopped as soon as QGC sends the request to stop the stream. If the companion side for some reason still requires the stream to be active we need to disable QGC from stopping the stream. Normally dronekit should be responsible for filtering out the request from QGC but apparently this is not possible. - disable heartbeat forwarding: dronekit does seem to get confused if heartbeats from another system other than the autopilot get forwarded to it. Example: Sending messages from QGC via the autopilot to dronekit running on companion computer. Signed-off-by: Roman --- src/modules/mavlink/mavlink_main.cpp | 3 ++- src/modules/mavlink/mavlink_main.h | 8 +++++++- src/modules/mavlink/mavlink_parameters.cpp | 2 +- src/modules/mavlink/mavlink_params.c | 22 ++++++++++++++++++++++ 4 files changed, 32 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 519dd777e4..1bdd517d5f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -546,7 +546,8 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) // Broadcast or addressing this system and not trying to talk // to the autopilot component -> pass on to other components if ((target_system_id == 0 || target_system_id == self->get_system_id()) - && (target_component_id == 0 || target_component_id != self->get_component_id())) { + && (target_component_id == 0 || target_component_id != self->get_component_id()) + && !(!self->forward_heartbeats_enabled() && msg->msgid == MAVLINK_MSG_ID_HEARTBEAT)) { inst->pass_message(msg); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 4a4e1e0535..62501a943b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -487,6 +487,10 @@ public: bool ftp_enabled() const { return _ftp_on; } + bool hash_check_enabled() { return _param_hash_check_enabled.get(); } + + bool forward_heartbeats_enabled() { return _param_heartbeat_forwarding_enabled.get(); } + struct ping_statistics_s { uint64_t last_ping_time; uint32_t last_ping_seq; @@ -629,7 +633,9 @@ private: (ParamInt) _param_system_type, (ParamBool) _param_use_hil_gps, (ParamBool) _param_forward_externalsp, - (ParamInt) _param_broadcast_mode + (ParamInt) _param_broadcast_mode, + (ParamBool) _param_hash_check_enabled, + (ParamBool) _param_heartbeat_forwarding_enabled ) perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 5a329a762c..08d0e2e901 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -134,7 +134,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* Whatever the value is, we're being told to stop sending */ - if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) { + if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0 && _mavlink->hash_check_enabled()) { _send_all_index = -1; /* No other action taken, return */ return; diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index abaae951e3..ac85625e5a 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -141,3 +141,25 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); * @group MAVLink */ PARAM_DEFINE_INT32(MAV_BROADCAST, 0); + +/** + * Parameter hash check. + * + * Disabling the parameter hash check functionality will make the mavlink instance + * stream parameters continuously. + * + * @boolean + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_HASH_CHK_EN, 1); + +/** + * Hearbeat message forwarding. + * + * The mavlink hearbeat message will not be forwarded if this parameter is set to 'disabled'. + * The main reason for disabling heartbeats to be forwarded is because they confuse dronekit. + * + * @boolean + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);