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);