diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5498dea466..26e354f145 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -427,6 +427,8 @@ public: bool accepting_commands() { return true; /* non-trivial side effects ((!_config_link_on) || (_mode == MAVLINK_MODE_CONFIG));*/ } + bool verbose() { return _verbose; } + int get_data_rate() { return _datarate; } void set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 33f0a8c06c..dbc9f5eb1e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -458,11 +458,17 @@ protected: /* only send commands for other systems/components, don't forward broadcast commands */ if ((cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) && (cmd.target_component != MAV_COMP_ID_ALL)) { - PX4_INFO("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + + if (_mavlink->verbose()) { + PX4_INFO("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + } + MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel()); } else { - PX4_INFO("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + if (_mavlink->verbose()) { + PX4_INFO("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + } } } diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 72a02c55af..9c18d4a742 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -87,7 +87,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _mission_result_sub(-1), _offboard_mission_pub(nullptr), _slow_rate_limiter(100 * 1000), // Rate limit sending of the current WP sequence to 10 Hz - _verbose(false), + _verbose(mavlink->verbose()), _mavlink(mavlink) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); @@ -385,10 +385,14 @@ MavlinkMissionManager::send(const hrt_abstime now) && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { if (_transfer_seq == 0) { /* try to send items count again after timeout */ + if (_verbose) { warnx("WPM: send count timeout"); } + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count); } else { /* try to send item again after timeout */ + if (_verbose) { warnx("WPM: item re-send timeout"); } + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1); }