Merge pull request #2253 from mavlink/pr-winch-workaround

Winch example and workaround

examples/CMakeLists.txt

@@ -36,3 +36,4 @@ add_subdirectory(terminate)
 add_subdirectory(transponder)
 add_subdirectory(tune)
 add_subdirectory(vtol_transition)
+add_subdirectory(winch)

examples/winch/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+project(winch)
+
+add_executable(winch
+    winch.cpp
+)
+
+find_package(MAVSDK REQUIRED)
+
+target_link_libraries(winch
+    MAVSDK::mavsdk
+)
+
+if(NOT MSVC)
+    add_compile_options(winch PRIVATE -Wall -Wextra)
+else()
+    add_compile_options(winch PRIVATE -WX -W2)
+endif()

examples/winch/winch.cpp

@@ -0,0 +1,59 @@
+//
+// Example how to winch MAVSDK.
+//
+
+#include <cstdint>
+#include <future>
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/winch/winch.h>
+#include <iostream>
+#include <thread>
+
+using namespace mavsdk;
+using namespace std::this_thread;
+using namespace std::chrono;
+
+void usage(const std::string& bin_name)
+{
+    std::cerr << "Usage : " << bin_name << " <connection_url>\n"
+              << "Connection URL format should be :\n"
+              << " For TCP : tcp://[server_host][:server_port]\n"
+              << " For UDP : udp://[bind_host][:bind_port]\n"
+              << " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
+              << "For example, to connect to the simulator use URL: udp://:14540\n";
+}
+
+int main(int argc, char** argv)
+{
+    if (argc != 2) {
+        usage(argv[0]);
+        return 1;
+    }
+
+    Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}};
+    ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);
+
+    if (connection_result != ConnectionResult::Success) {
+        std::cerr << "Connection failed: " << connection_result << '\n';
+        return 1;
+    }
+
+    auto system = mavsdk.first_autopilot(3.0);
+    if (!system) {
+        std::cerr << "Timed out waiting for system\n";
+        return 1;
+    }
+
+    // Instantiate plugin.
+    Winch winch(system.value());
+
+    // This seems to fail against normal PX4 SITL with payload_deliverer started.
+    // Presumably that's because commands are sent to component 169 and not 1.
+    const auto result = winch.lock(1);
+    if (result != Winch::Result::Success) {
+        std::cerr << "Winch result: " << result << '\n';
+        return 1;
+    }
+
+    return 0;
+}

src/mavsdk/core/mavlink_command_sender.cpp

@@ -155,11 +155,16 @@ void MavlinkCommandSender::receive_command_ack(mavlink_message_t message)
             return;
         }
 
+        // Currently, the gripper/winch sends the ack with source sysid/compid 1/1 instead of 1/169.
+        // Until that's fixed, we ignore the component ID for any commands going to the winch.
+        const bool compid_exception =
+            (work->identification.target_component_id == MAV_COMP_ID_WINCH);
+
         if (work->identification.command != command_ack.command ||
             (work->identification.target_system_id != 0 &&
              work->identification.target_system_id != message.sysid) ||
             (work->identification.target_component_id != 0 &&
-             work->identification.target_component_id != message.compid)) {
+             work->identification.target_component_id != message.compid && !compid_exception)) {
             if (_command_debugging) {
                 LogDebug() << "Command ack for " << command_ack.command
                            << " (from: " << std::to_string(message.sysid) << "/"