From 72dbe7f188603bbd49b01eeed7b39ef2484d1bca Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 26 Jul 2021 20:47:18 +0200 Subject: [PATCH 01/13] core: comment out some noisy printfs --- src/core/mavlink_commands.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/core/mavlink_commands.cpp b/src/core/mavlink_commands.cpp index 81428a9084..6b70706f53 100644 --- a/src/core/mavlink_commands.cpp +++ b/src/core/mavlink_commands.cpp @@ -307,7 +307,7 @@ void MavlinkCommandSender::do_work() return; } - LogDebug() << "sending it the first time (" << work->mavlink_command << ")"; + // LogDebug() << "sending it the first time (" << work->mavlink_command << ")"; work->time_started = _parent.get_time().steady_time(); { @@ -318,9 +318,10 @@ void MavlinkCommandSender::do_work() // The command is inserted in the list only if another command with the same id does // not exist. If such an element exists, we refuse to send the new command. if (!was_inserted) { - LogWarn() - << "A command cannot be sent if another command with the same command_id is still processing! Ignoring command " - << static_cast(work->mavlink_command); + // LogWarn() + // << "A command cannot be sent if another command with the same command_id is still + // processing! Ignoring command " + // << static_cast(work->mavlink_command); auto temp_callback = work->callback; auto temp_result = std::make_pair(Result::CommandDenied, NAN); From 6250e459dd1cb5176ed7e989d9360bb74eb71570 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 27 Jul 2021 18:12:53 +0200 Subject: [PATCH 02/13] camera: reduce message requests Let's reduce the load on the autopilot a bit by reducing the amount of requests for commands that we do. --- src/plugins/camera/camera_impl.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/plugins/camera/camera_impl.cpp b/src/plugins/camera/camera_impl.cpp index 8cf2827fc9..e670fe0c97 100644 --- a/src/plugins/camera/camera_impl.cpp +++ b/src/plugins/camera/camera_impl.cpp @@ -499,9 +499,13 @@ void CameraImpl::subscribe_information(const Camera::InformationCallback& callba } if (callback) { - _parent->add_call_every([this]() { request_status(); }, 1.0, &_status.call_every_cookie); + if (_status.call_every_cookie == nullptr) { + _parent->add_call_every( + [this]() { request_status(); }, 5.0, &_status.call_every_cookie); + } } else { _parent->remove_call_every(_status.call_every_cookie); + _status.call_every_cookie = nullptr; } } @@ -702,7 +706,7 @@ void CameraImpl::subscribe_mode(const Camera::ModeCallback callback) if (callback) { _parent->add_call_every( - [this]() { request_camera_settings(); }, 1.0, &_mode.call_every_cookie); + [this]() { request_camera_settings(); }, 5.0, &_mode.call_every_cookie); } else { _parent->remove_call_every(_mode.call_every_cookie); } @@ -732,9 +736,13 @@ void CameraImpl::subscribe_status(const Camera::StatusCallback callback) _status.subscription_callback = callback; if (callback) { - _parent->add_call_every([this]() { request_status(); }, 1.0, &_status.call_every_cookie); + if (_status.call_every_cookie == nullptr) { + _parent->add_call_every( + [this]() { request_status(); }, 5.0, &_status.call_every_cookie); + } } else { _parent->remove_call_every(_status.call_every_cookie); + _status.call_every_cookie = nullptr; } } From a57de86b5680a6364261425efb71bf5dbc0ea03b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Jul 2021 10:31:38 +0200 Subject: [PATCH 03/13] core: add debug printfs to mission upload --- src/core/mavlink_mission_transfer.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/core/mavlink_mission_transfer.cpp b/src/core/mavlink_mission_transfer.cpp index 3c063eccfd..240abef1b8 100644 --- a/src/core/mavlink_mission_transfer.cpp +++ b/src/core/mavlink_mission_transfer.cpp @@ -251,6 +251,9 @@ void MAVLinkMissionTransfer::UploadWorkItem::send_count() return; } + // LogDebug() << "Sending send_count, count: " << _items.size() << ", retries: " << + // _retries_done; + ++_retries_done; } @@ -329,6 +332,9 @@ void MAVLinkMissionTransfer::UploadWorkItem::process_mission_request_int( _step = Step::SendItems; + // LogDebug() << "Process mission_request_int, seq: " << request_int.seq + // << ", next expected sequence: " << _next_sequence; + if (_next_sequence < request_int.seq) { // We should not go back to a previous one. // TODO: figure out if we should error here. @@ -338,6 +344,7 @@ void MAVLinkMissionTransfer::UploadWorkItem::process_mission_request_int( } else if (_next_sequence > request_int.seq) { // We have already sent that one before. if (_retries_done >= retries) { + LogWarn() << "mission_request_int: retries exceeded"; _timeout_handler.remove(_cookie); callback_and_reset(Result::Timeout); return; @@ -382,6 +389,9 @@ void MAVLinkMissionTransfer::UploadWorkItem::send_mission_item() _items[_next_sequence].z, _type); + // LogDebug() << "Sending mission_item_int seq: " << _next_sequence + // << ", retry: " << _retries_done; + ++_next_sequence; if (!_sender.send_message(message)) { @@ -400,6 +410,8 @@ void MAVLinkMissionTransfer::UploadWorkItem::process_mission_ack(const mavlink_m mavlink_mission_ack_t mission_ack; mavlink_msg_mission_ack_decode(&message, &mission_ack); + // LogDebug() << "Received mission_ack type: " << static_cast(mission_ack.type); + _timeout_handler.remove(_cookie); switch (mission_ack.type) { @@ -454,7 +466,10 @@ void MAVLinkMissionTransfer::UploadWorkItem::process_timeout() { std::lock_guard lock(_mutex); + // LogDebug() << "Timeout triggered, retries: " << _retries_done; + if (_retries_done >= retries) { + LogWarn() << "timeout: retries exceeded"; callback_and_reset(Result::Timeout); return; } @@ -466,6 +481,7 @@ void MAVLinkMissionTransfer::UploadWorkItem::process_timeout() break; case Step::SendItems: + LogWarn() << "send_items: timeout"; callback_and_reset(Result::Timeout); break; } From 2ff536f9b461a7fd213d7ecc3459acb241726c63 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Jul 2021 10:31:58 +0200 Subject: [PATCH 04/13] core: ignore nullptr cookies --- src/core/timeout_handler.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/core/timeout_handler.cpp b/src/core/timeout_handler.cpp index 4e1c821657..a75f384f8e 100644 --- a/src/core/timeout_handler.cpp +++ b/src/core/timeout_handler.cpp @@ -27,6 +27,10 @@ void TimeoutHandler::add(std::function callback, double duration_s, void void TimeoutHandler::refresh(const void* cookie) { + if (cookie == nullptr) { + return; + } + std::lock_guard lock(_timeouts_mutex); auto it = _timeouts.find(const_cast(cookie)); @@ -38,6 +42,10 @@ void TimeoutHandler::refresh(const void* cookie) void TimeoutHandler::remove(const void* cookie) { + if (cookie == nullptr) { + return; + } + std::lock_guard lock(_timeouts_mutex); auto it = _timeouts.find(const_cast(cookie)); From 42968908bc73d31b387ecde2f49036f024fc5615 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Jul 2021 10:32:53 +0200 Subject: [PATCH 05/13] gimbal: only set protocol once This should avoid resetting the protocol each time we get a gimbal_manager_information message which happens for some systems. --- src/plugins/gimbal/gimbal_impl.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/plugins/gimbal/gimbal_impl.cpp b/src/plugins/gimbal/gimbal_impl.cpp index 9c9266183c..c32bc6dd11 100644 --- a/src/plugins/gimbal/gimbal_impl.cpp +++ b/src/plugins/gimbal/gimbal_impl.cpp @@ -57,6 +57,7 @@ void GimbalImpl::receive_protocol_timeout() // assume Version2 is not available. LogDebug() << "Falling back to Gimbal Version 1"; _gimbal_protocol.reset(new GimbalProtocolV1(*_parent)); + _protocol_cookie = nullptr; } void GimbalImpl::process_gimbal_manager_information(const mavlink_message_t& message) @@ -64,13 +65,16 @@ void GimbalImpl::process_gimbal_manager_information(const mavlink_message_t& mes mavlink_gimbal_manager_information_t gimbal_manager_information; mavlink_msg_gimbal_manager_information_decode(&message, &gimbal_manager_information); - LogDebug() << "Using Gimbal Version 2 as gimbal manager information for gimbal device " - << static_cast(gimbal_manager_information.gimbal_device_id) - << " was discovered"; + if (_protocol_cookie != nullptr) { + LogDebug() << "Using Gimbal Version 2 as gimbal manager information for gimbal device " + << static_cast(gimbal_manager_information.gimbal_device_id) + << " was discovered"; - _parent->unregister_timeout_handler(_protocol_cookie); - _gimbal_protocol.reset( - new GimbalProtocolV2(*_parent, gimbal_manager_information, message.sysid, message.compid)); + _parent->unregister_timeout_handler(_protocol_cookie); + _protocol_cookie = nullptr; + _gimbal_protocol.reset(new GimbalProtocolV2( + *_parent, gimbal_manager_information, message.sysid, message.compid)); + } } Gimbal::Result GimbalImpl::set_pitch_and_yaw(float pitch_deg, float yaw_deg) From bd20dad25e3182a12336196d377b3118d208dcf7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Jul 2021 10:33:31 +0200 Subject: [PATCH 06/13] mission: only set gimbal protocol once Otherwise we keep setting this if the gimbal_manager_information message is sent again. --- src/plugins/mission/mission_impl.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/plugins/mission/mission_impl.cpp b/src/plugins/mission/mission_impl.cpp index 2c05ce2241..74e4ec0903 100644 --- a/src/plugins/mission/mission_impl.cpp +++ b/src/plugins/mission/mission_impl.cpp @@ -106,9 +106,11 @@ void MissionImpl::process_mission_item_reached(const mavlink_message_t& message) void MissionImpl::process_gimbal_manager_information(const mavlink_message_t& message) { UNUSED(message); - LogDebug() << "Using gimbal protocol v2"; - _gimbal_protocol = GimbalProtocol::V2; - _parent->unregister_timeout_handler(_gimbal_protocol_cookie); + if (_gimbal_protocol_cookie != nullptr) { + LogDebug() << "Using gimbal protocol v2"; + _gimbal_protocol = GimbalProtocol::V2; + _parent->unregister_timeout_handler(_gimbal_protocol_cookie); + } } void MissionImpl::wait_for_protocol() @@ -128,6 +130,7 @@ void MissionImpl::receive_protocol_timeout() { LogDebug() << "Falling back to gimbal protocol v1"; _gimbal_protocol = GimbalProtocol::V1; + _gimbal_protocol_cookie = nullptr; } Mission::Result MissionImpl::upload_mission(const Mission::MissionPlan& mission_plan) From 68a296dc46c15b0d5cee3f54a080aa17346d1b24 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 30 Jul 2021 11:30:50 +0200 Subject: [PATCH 07/13] core: potential fix for mission transfer timeout This could potentially fix an issue where we saw timeouts when uploading mission items. Basically, the change increases our timeout waiting for the autopilot to request the next mission item. So if the autopilot takes a bit longer, we should not give up too early. The timeout is retries*timeout instead of the timeout only. --- src/core/mavlink_mission_transfer.cpp | 6 ++++-- src/core/mavlink_mission_transfer_test.cpp | 11 ++++++++++- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/core/mavlink_mission_transfer.cpp b/src/core/mavlink_mission_transfer.cpp index 240abef1b8..08be85b635 100644 --- a/src/core/mavlink_mission_transfer.cpp +++ b/src/core/mavlink_mission_transfer.cpp @@ -481,8 +481,10 @@ void MAVLinkMissionTransfer::UploadWorkItem::process_timeout() break; case Step::SendItems: - LogWarn() << "send_items: timeout"; - callback_and_reset(Result::Timeout); + // When waiting for items requested we should wait longer than + // just our timeout, otherwise we give up too quickly. + ++_retries_done; + _timeout_handler.add([this]() { process_timeout(); }, _timeout_s, &_cookie); break; } } diff --git a/src/core/mavlink_mission_transfer_test.cpp b/src/core/mavlink_mission_transfer_test.cpp index 4eb8b20b21..3ae56b60e5 100644 --- a/src/core/mavlink_mission_transfer_test.cpp +++ b/src/core/mavlink_mission_transfer_test.cpp @@ -718,9 +718,18 @@ TEST_F(MAVLinkMissionTransferTest, UploadMissionTimeoutAfterSendMissionItem) message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); - time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); + // Make sure single timeout does not trigger it yet. + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1000. + 250))); timeout_handler.run_once(); + EXPECT_EQ(fut.wait_for(std::chrono::milliseconds(50)), std::future_status::timeout); + + // But multiple do. + for (unsigned i = 0; i < (MAVLinkMissionTransfer::retries - 1); ++i) { + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1000.))); + timeout_handler.run_once(); + } + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); // Ignore later (wrong) ack. From 9260f25ddf1c209dd9e761dfb4a431b87ac86857 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 30 Jul 2021 11:46:56 +0200 Subject: [PATCH 08/13] telemetry: keep trying to get calibration params Rather than giving up right at the beginning, we now keep trying every 5 seconds. --- src/plugins/telemetry/telemetry_impl.cpp | 95 ++++++++++++++---------- src/plugins/telemetry/telemetry_impl.h | 4 + 2 files changed, 60 insertions(+), 39 deletions(-) diff --git a/src/plugins/telemetry/telemetry_impl.cpp b/src/plugins/telemetry/telemetry_impl.cpp index 527d9be1e5..02e1a8c6ac 100644 --- a/src/plugins/telemetry/telemetry_impl.cpp +++ b/src/plugins/telemetry/telemetry_impl.cpp @@ -145,6 +145,7 @@ void TelemetryImpl::init() void TelemetryImpl::deinit() { + _parent->remove_call_every(_calibration_cookie); _parent->unregister_statustext_handler(this); _parent->unregister_timeout_handler(_gps_raw_timeout_cookie); _parent->unregister_timeout_handler(_unix_epoch_timeout_cookie); @@ -165,45 +166,7 @@ void TelemetryImpl::enable() // FIXME: The calibration check should eventually be better than this. // For now, we just do the same as QGC does. - if (_parent->autopilot() == SystemImpl::Autopilot::Px4) { - if (_parent->has_autopilot()) { - _parent->get_param_int_async( - std::string("CAL_GYRO0_ID"), - std::bind( - &TelemetryImpl::receive_param_cal_gyro, - this, - std::placeholders::_1, - std::placeholders::_2), - this); - - _parent->get_param_int_async( - std::string("CAL_ACC0_ID"), - std::bind( - &TelemetryImpl::receive_param_cal_accel, - this, - std::placeholders::_1, - std::placeholders::_2), - this); - - _parent->get_param_int_async( - std::string("CAL_MAG0_ID"), - std::bind( - &TelemetryImpl::receive_param_cal_mag, - this, - std::placeholders::_1, - std::placeholders::_2), - this); - - _parent->get_param_int_async( - std::string("SYS_HITL"), - std::bind( - &TelemetryImpl::receive_param_hitl, - this, - std::placeholders::_1, - std::placeholders::_2), - this); - } - } + _parent->add_call_every([this]() { check_calibration(); }, 5.0, &_calibration_cookie); } void TelemetryImpl::disable() {} @@ -2149,6 +2112,60 @@ std::pair TelemetryImpl::get_gps_ return fut.get(); } +void TelemetryImpl::check_calibration() +{ + { + std::lock_guard lock(_health_mutex); + if (_health.is_gyrometer_calibration_ok && _health.is_accelerometer_calibration_ok && + _health.is_magnetometer_calibration_ok) { + _parent->remove_call_every(_calibration_cookie); + return; + } + } + + if (_parent->autopilot() != SystemImpl::Autopilot::ArduPilot) { + if (_parent->has_autopilot()) { + _parent->get_param_int_async( + std::string("CAL_GYRO0_ID"), + std::bind( + &TelemetryImpl::receive_param_cal_gyro, + this, + std::placeholders::_1, + std::placeholders::_2), + this); + + _parent->get_param_int_async( + std::string("CAL_ACC0_ID"), + std::bind( + &TelemetryImpl::receive_param_cal_accel, + this, + std::placeholders::_1, + std::placeholders::_2), + this); + + _parent->get_param_int_async( + std::string("CAL_MAG0_ID"), + std::bind( + &TelemetryImpl::receive_param_cal_mag, + this, + std::placeholders::_1, + std::placeholders::_2), + this); + + _parent->get_param_int_async( + std::string("SYS_HITL"), + std::bind( + &TelemetryImpl::receive_param_hitl, + this, + std::placeholders::_1, + std::placeholders::_2), + this); + } + } else { + _parent->remove_call_every(_calibration_cookie); + } +} + void TelemetryImpl::process_parameter_update(const std::string& name) { if (name.compare("CAL_GYRO0_ID") == 0) { diff --git a/src/plugins/telemetry/telemetry_impl.h b/src/plugins/telemetry/telemetry_impl.h index 8291bcefc9..2aa9aaacd6 100644 --- a/src/plugins/telemetry/telemetry_impl.h +++ b/src/plugins/telemetry/telemetry_impl.h @@ -214,6 +214,8 @@ class TelemetryImpl : public PluginImplBase { void receive_statustext(const MavlinkStatustextHandler::Statustext&); + void check_calibration(); + static Telemetry::Result telemetry_result_from_command_result(MavlinkCommandSender::Result command_result); @@ -355,5 +357,7 @@ class TelemetryImpl : public PluginImplBase { // Battery info can be extracted from SYS_STATUS or from BATTERY_STATUS. // If no BATTERY_STATUS messages are received, use info from SYS_STATUS. bool _has_bat_status{false}; + + void* _calibration_cookie{nullptr}; }; } // namespace mavsdk From 6b19d72bcbc67bd7db20f78095bca3e0ff29a7a8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Aug 2021 23:07:06 +0200 Subject: [PATCH 09/13] telemetry: limit param retrying Instead of re-trying to get the params until they signal that it's calibrated, only do it until we have received them once. --- src/plugins/telemetry/telemetry_impl.cpp | 13 +++++++++++-- src/plugins/telemetry/telemetry_impl.h | 5 +++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/plugins/telemetry/telemetry_impl.cpp b/src/plugins/telemetry/telemetry_impl.cpp index 02e1a8c6ac..42b594886e 100644 --- a/src/plugins/telemetry/telemetry_impl.cpp +++ b/src/plugins/telemetry/telemetry_impl.cpp @@ -151,6 +151,10 @@ void TelemetryImpl::deinit() _parent->unregister_timeout_handler(_unix_epoch_timeout_cookie); _parent->unregister_param_changed_handler(this); _parent->unregister_all_mavlink_message_handlers(this); + + _has_received_gyro_calibration = false; + _has_received_accel_calibration = false; + _has_received_mag_calibration = false; } void TelemetryImpl::enable() @@ -1396,6 +1400,7 @@ void TelemetryImpl::receive_param_cal_gyro(MAVLinkParameters::Result result, int bool ok = (value != 0); set_health_gyrometer_calibration(ok); + _has_received_gyro_calibration = true; } void TelemetryImpl::receive_param_cal_accel(MAVLinkParameters::Result result, int value) @@ -1407,6 +1412,7 @@ void TelemetryImpl::receive_param_cal_accel(MAVLinkParameters::Result result, in bool ok = (value != 0); set_health_accelerometer_calibration(ok); + _has_received_accel_calibration = true; } void TelemetryImpl::receive_param_cal_mag(MAVLinkParameters::Result result, int value) @@ -1418,6 +1424,7 @@ void TelemetryImpl::receive_param_cal_mag(MAVLinkParameters::Result result, int bool ok = (value != 0); set_health_magnetometer_calibration(ok); + _has_received_mag_calibration = true; } void TelemetryImpl::receive_param_hitl(MAVLinkParameters::Result result, int value) @@ -1435,6 +1442,7 @@ void TelemetryImpl::receive_param_hitl(MAVLinkParameters::Result result, int val set_health_gyrometer_calibration(true); set_health_magnetometer_calibration(true); } + _has_received_hitl_param = true; } void TelemetryImpl::receive_gps_raw_timeout() @@ -2116,8 +2124,9 @@ void TelemetryImpl::check_calibration() { { std::lock_guard lock(_health_mutex); - if (_health.is_gyrometer_calibration_ok && _health.is_accelerometer_calibration_ok && - _health.is_magnetometer_calibration_ok) { + if ((_has_received_gyro_calibration && _has_received_accel_calibration && + _has_received_mag_calibration) || + _has_received_hitl_param) { _parent->remove_call_every(_calibration_cookie); return; } diff --git a/src/plugins/telemetry/telemetry_impl.h b/src/plugins/telemetry/telemetry_impl.h index 2aa9aaacd6..1f15be125c 100644 --- a/src/plugins/telemetry/telemetry_impl.h +++ b/src/plugins/telemetry/telemetry_impl.h @@ -359,5 +359,10 @@ class TelemetryImpl : public PluginImplBase { bool _has_bat_status{false}; void* _calibration_cookie{nullptr}; + + bool _has_received_hitl_param{false}; + bool _has_received_gyro_calibration{false}; + bool _has_received_accel_calibration{false}; + bool _has_received_mag_calibration{false}; }; } // namespace mavsdk From 7d21446eb26519171af2e34d30258716eb88274e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Aug 2021 08:39:45 +0200 Subject: [PATCH 10/13] core: use global mavlink_status_t This means that the sequence number of outgoing mavlink messages is no longer different between messages originating from different plugins in separated libraries but synced across. However, this will still be wrong if more than one instance of Mavsdk is instantiated. --- src/core/mavlink_include.h | 6 ++++++ src/core/mavsdk_impl.cpp | 8 ++++++++ 2 files changed, 14 insertions(+) diff --git a/src/core/mavlink_include.h b/src/core/mavlink_include.h index 93fed8029d..96f8e4fc51 100644 --- a/src/core/mavlink_include.h +++ b/src/core/mavlink_include.h @@ -4,4 +4,10 @@ #pragma GCC system_header #endif +#define MAVLINK_GET_CHANNEL_STATUS + +#include "mavlink/v2.0/mavlink_types.h" + +extern mavlink_status_t* mavlink_get_channel_status(uint8_t chan); + #include "mavlink/v2.0/common/mavlink.h" diff --git a/src/core/mavsdk_impl.cpp b/src/core/mavsdk_impl.cpp index 905996cf8a..bb7d474b2d 100644 --- a/src/core/mavsdk_impl.cpp +++ b/src/core/mavsdk_impl.cpp @@ -14,6 +14,14 @@ #include "cli_arg.h" #include "version.h" +static mavlink_status_t status_; + +mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ + UNUSED(chan); + return &status_; +} + namespace mavsdk { MavsdkImpl::MavsdkImpl() : timeout_handler(_time), call_every_handler(_time) From 414c758554fa21b65e686e981003d803f702bad1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Aug 2021 10:51:01 +0200 Subject: [PATCH 11/13] mavlink_commands: don't pack message too early By packing the commands too early and sending them later, or even re-sending them, we mess up the sequence number in the MAVLink header. Therefore, we need to keep the CommandInt and CommandLong structs around until actually sending the message. --- src/core/mavlink_commands.cpp | 96 ++++++++++++++++++++--------------- src/core/mavlink_commands.h | 13 +++-- 2 files changed, 64 insertions(+), 45 deletions(-) diff --git a/src/core/mavlink_commands.cpp b/src/core/mavlink_commands.cpp index 6b70706f53..063cfd249a 100644 --- a/src/core/mavlink_commands.cpp +++ b/src/core/mavlink_commands.cpp @@ -85,28 +85,11 @@ void MavlinkCommandSender::queue_command_async( // LogDebug() << "Command " << (int)(command.command) << " to send to " // << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id); - auto new_work = std::make_shared(_parent.timeout_s()); - - mavlink_msg_command_int_pack( - _parent.get_own_system_id(), - _parent.get_own_component_id(), - &new_work->mavlink_message, - command.target_system_id, - command.target_component_id, - command.frame, - command.command, - command.current, - command.autocontinue, - command.params.param1, - command.params.param2, - command.params.param3, - command.params.param4, - command.params.x, - command.params.y, - command.params.z); - - new_work->callback = callback; + auto new_work = std::make_shared(); + new_work->timeout_s = _parent.timeout_s(); + new_work->command = command; new_work->mavlink_command = command.command; + new_work->callback = callback; _work_queue.push_back(new_work); } @@ -116,25 +99,11 @@ void MavlinkCommandSender::queue_command_async( // LogDebug() << "Command " << (int)(command.command) << " to send to " // << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id); - auto new_work = std::make_shared(_parent.timeout_s()); - mavlink_msg_command_long_pack( - _parent.get_own_system_id(), - _parent.get_own_component_id(), - &new_work->mavlink_message, - command.target_system_id, - command.target_component_id, - command.command, - command.confirmation, - command.params.param1, - command.params.param2, - command.params.param3, - command.params.param4, - command.params.param5, - command.params.param6, - command.params.param7); - - new_work->callback = callback; + auto new_work = std::make_shared(); + new_work->timeout_s = _parent.timeout_s(); + new_work->command = command; new_work->mavlink_command = command.command; + new_work->callback = callback; new_work->time_started = _parent.get_time().steady_time(); _work_queue.push_back(new_work); } @@ -264,7 +233,8 @@ void MavlinkCommandSender::receive_timeout(const uint16_t command) << " s, retries to do: " << work->retries_to_do << " (" << work->mavlink_command << ")."; - if (!_parent.send_message(work->mavlink_message)) { + mavlink_message_t message = create_mavlink_message(work->command); + if (!_parent.send_message(message)) { LogErr() << "connection send error in retransmit (" << work->mavlink_command << ")."; temp_callback = work->callback; @@ -332,7 +302,8 @@ void MavlinkCommandSender::do_work() return; } - if (!_parent.send_message(work->mavlink_message)) { + mavlink_message_t message = create_mavlink_message(work->command); + if (!_parent.send_message(message)) { LogErr() << "connection send error (" << work->mavlink_command << ")"; auto temp_callback = work->callback; auto temp_result = std::make_pair(Result::ConnectionError, NAN); @@ -368,6 +339,49 @@ void MavlinkCommandSender::call_callback( _parent.call_user_callback([callback, result, progress]() { callback(result, progress); }); } +mavlink_message_t MavlinkCommandSender::create_mavlink_message(const Command& command) +{ + mavlink_message_t message; + + if (auto command_int = std::get_if(&command)) { + mavlink_msg_command_int_pack( + _parent.get_own_system_id(), + _parent.get_own_component_id(), + &message, + command_int->target_system_id, + command_int->target_component_id, + command_int->frame, + command_int->command, + command_int->current, + command_int->autocontinue, + command_int->params.param1, + command_int->params.param2, + command_int->params.param3, + command_int->params.param4, + command_int->params.x, + command_int->params.y, + command_int->params.z); + + } else if (auto command_long = std::get_if(&command)) { + mavlink_msg_command_long_pack( + _parent.get_own_system_id(), + _parent.get_own_component_id(), + &message, + command_long->target_system_id, + command_long->target_component_id, + command_long->command, + command_long->confirmation, + command_long->params.param1, + command_long->params.param2, + command_long->params.param3, + command_long->params.param4, + command_long->params.param5, + command_long->params.param6, + command_long->params.param7); + } + return message; +} + MavlinkCommandReceiver::MavlinkCommandReceiver(SystemImpl& system_impl) : _parent(system_impl) { _parent.register_mavlink_message_handler( diff --git a/src/core/mavlink_commands.h b/src/core/mavlink_commands.h index 5cdf629b99..7aa024c82f 100644 --- a/src/core/mavlink_commands.h +++ b/src/core/mavlink_commands.h @@ -9,6 +9,7 @@ #include #include #include +#include namespace mavsdk { @@ -112,17 +113,19 @@ class MavlinkCommandSender { const MavlinkCommandSender& operator=(const MavlinkCommandSender&) = delete; private: + // The std::monostate is required to work around the fact that + // the default ctor of CommandLong and CommandInt is ill-defined. + using Command = std::variant; + struct Work { int retries_to_do{3}; double timeout_s{0.5}; - uint16_t mavlink_command{0}; bool already_sent{false}; - mavlink_message_t mavlink_message{}; + Command command; + uint16_t mavlink_command{}; CommandResultCallback callback{}; dl_time_t time_started{}; void* timeout_cookie = nullptr; - - explicit Work(double new_timeout_s) : timeout_s(new_timeout_s) {} }; void receive_command_ack(mavlink_message_t message); @@ -130,6 +133,8 @@ class MavlinkCommandSender { void call_callback(const CommandResultCallback& callback, Result result, float progress); + mavlink_message_t create_mavlink_message(const Command& command); + SystemImpl& _parent; LockedQueue _work_queue{}; std::unordered_map> _sent_commands{}; From 0186dae42da896b401ba11eb6b8bcb67721d478f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Aug 2021 10:51:59 +0200 Subject: [PATCH 12/13] core: add note about wrong sequence number If we pack a message and don't actually send it, we increase the sequence number. This will mess with message delivery stats. --- src/core/mavlink_mission_transfer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/core/mavlink_mission_transfer.cpp b/src/core/mavlink_mission_transfer.cpp index 08be85b635..da64240b90 100644 --- a/src/core/mavlink_mission_transfer.cpp +++ b/src/core/mavlink_mission_transfer.cpp @@ -286,6 +286,7 @@ void MAVLinkMissionTransfer::UploadWorkItem::process_mission_request( mavlink_mission_request_t request; mavlink_msg_mission_request_decode(&request_message, &request); + // FIXME: this will mess with the sequence number. mavlink_message_t request_int_message; mavlink_msg_mission_request_int_pack( request_message.sysid, From 4e70b28fda4a3d8174f7456e2397433ae1d98daa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Aug 2021 14:18:14 +0200 Subject: [PATCH 13/13] core: bump mavlink mission retries --- src/core/mavlink_mission_transfer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/mavlink_mission_transfer.h b/src/core/mavlink_mission_transfer.h index a30944620a..e16c8ac012 100644 --- a/src/core/mavlink_mission_transfer.h +++ b/src/core/mavlink_mission_transfer.h @@ -302,7 +302,7 @@ class MAVLinkMissionTransfer { unsigned _retries_done{0}; }; - static constexpr unsigned retries = 4; + static constexpr unsigned retries = 5; using TimeoutSCallback = std::function;