Merge aa5952a into 89cbac7

.ycm_extra_conf.py

@@ -83,6 +83,8 @@
 '-I',
 'plugins/mission/include',
 '-I',
+'plugins/mission_raw/include',
+'-I',
 'plugins/offboard/include',
 '-I',
 'plugins/params_raw/include',

integration_tests/CMakeLists.txt

@@ -39,6 +39,7 @@ add_executable(integration_tests_runner
     mission.cpp
     mission_rtl.cpp
     mission_survey.cpp
+    mission_raw_mission_changed.cpp
     offboard_velocity.cpp
     params_raw.cpp
     path_checker.cpp
@@ -66,6 +67,7 @@ target_link_libraries(integration_tests_runner
     dronecode_sdk_telemetry
     dronecode_sdk_action
     dronecode_sdk_mission
+    dronecode_sdk_mission_raw
     dronecode_sdk_offboard
     dronecode_sdk_log_files
     dronecode_sdk_info

integration_tests/mission_raw_mission_changed.cpp

@@ -0,0 +1,108 @@
+#include "integration_test_helper.h"
+#include "dronecode_sdk.h"
+#include "plugins/mission/mission.h"
+#include "plugins/mission_raw/mission_raw.h"
+#include <cmath>
+#include <future>
+
+using namespace dronecode_sdk;
+
+static constexpr double SOME_LATITUDES[] = {47.398170, 47.398175};
+static constexpr double SOME_LONGITUDES[] = {8.545649, 8.545654};
+static constexpr float SOME_ALTITUDES[] = {5.0f, 7.5f};
+static constexpr float SOME_SPEEDS[] = {4.0f, 5.0f};
+static constexpr unsigned NUM_SOME_ITEMS = sizeof(SOME_LATITUDES) / sizeof(SOME_LATITUDES[0]);
+
+static void
+validate_items(const std::vector<std::shared_ptr<MissionRaw::MavlinkMissionItemInt>> &items);
+
+TEST_F(SitlTest, MissionRawMissionChanged)
+{
+    DronecodeSDK dc;
+
+    ConnectionResult ret = dc.add_udp_connection();
+    ASSERT_EQ(ret, ConnectionResult::SUCCESS);
+
+    // Wait for system to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    ASSERT_TRUE(dc.is_connected());
+
+    System &system = dc.system();
+    ASSERT_TRUE(system.has_autopilot());
+
+    auto mission = std::make_shared<Mission>(system);
+    auto mission_raw = std::make_shared<MissionRaw>(system);
+
+    std::promise<void> prom_changed{};
+    std::future<void> fut_changed = prom_changed.get_future();
+
+    LogInfo() << "Subscribe for mission changed notification";
+    mission_raw->subscribe_mission_changed([&prom_changed]() { prom_changed.set_value(); });
+
+    // The mission change callback should not trigger yet.
+    EXPECT_EQ(fut_changed.wait_for(std::chrono::milliseconds(500)), std::future_status::timeout);
+
+    std::vector<std::shared_ptr<MissionItem>> mission_items;
+
+    for (unsigned i = 0; i < NUM_SOME_ITEMS; ++i) {
+        auto new_item = std::make_shared<MissionItem>();
+        new_item->set_position(SOME_LATITUDES[i], SOME_LONGITUDES[i]);
+        new_item->set_relative_altitude(SOME_ALTITUDES[i]);
+        new_item->set_speed(SOME_SPEEDS[i]);
+        mission_items.push_back(new_item);
+    }
+
+    {
+        LogInfo() << "Uploading mission...";
+        // We only have the upload_mission function asynchronous for now, so we wrap it using
+        // std::future.
+        std::promise<void> prom{};
+        std::future<void> fut = prom.get_future();
+        mission->upload_mission_async(mission_items, [&prom](Mission::Result result) {
+            ASSERT_EQ(result, Mission::Result::SUCCESS);
+            prom.set_value();
+            LogInfo() << "Mission uploaded.";
+        });
+
+        auto status = fut.wait_for(std::chrono::seconds(2));
+        ASSERT_EQ(status, std::future_status::ready);
+        fut.get();
+    }
+
+    // The mission change callback should have triggered now because we have uploaded a mission.
+    EXPECT_EQ(fut_changed.wait_for(std::chrono::milliseconds(500)), std::future_status::ready);
+
+    {
+        std::promise<void> prom{};
+        std::future<void> fut = prom.get_future();
+        LogInfo() << "Download raw mission items.";
+        mission_raw->download_mission_async(
+            [&prom](MissionRaw::Result result,
+                    std::vector<std::shared_ptr<MissionRaw::MavlinkMissionItemInt>> items) {
+                EXPECT_EQ(result, MissionRaw::Result::SUCCESS);
+                // TODO: validate items
+                validate_items(items);
+                prom.set_value();
+            });
+        auto status = fut.wait_for(std::chrono::seconds(2));
+        ASSERT_EQ(status, std::future_status::ready);
+    }
+}
+
+void validate_items(const std::vector<std::shared_ptr<MissionRaw::MavlinkMissionItemInt>> &items)
+{
+    for (unsigned i = 0; i < items.size(); ++i) {
+        // Even items are waypoints, odd ones are the speed commands.
+        if (i % 2 == 0) {
+            EXPECT_EQ(items[i]->command, 16); // MAV_CMD_NAV_WAYPOINT
+            EXPECT_EQ(items[i]->x, std::round(SOME_LATITUDES[i / 2] * 1e7));
+            EXPECT_EQ(items[i]->y, std::round(SOME_LONGITUDES[i / 2] * 1e7));
+            EXPECT_EQ(items[i]->z, SOME_ALTITUDES[i / 2]);
+            LogWarn() << "i/2: " << i / 2;
+        } else {
+            EXPECT_EQ(items[i]->command, 178); // MAV_CMD_DO_CHANGE_SPEED
+            EXPECT_EQ(items[i]->param2, SOME_SPEEDS[i / 2]);
+            LogWarn() << "i/2: " << i / 2;
+        }
+    }
+}

plugins/CMakeLists.txt

@@ -18,6 +18,7 @@ add_subdirectory(info)
 add_subdirectory(log_files)
 #add_subdirectory(logging) # Not implemented completely
 add_subdirectory(mission)
+add_subdirectory(mission_raw)
 add_subdirectory(offboard)
 add_subdirectory(params_raw)
 add_subdirectory(telemetry)

plugins/action/action_impl.cpp

@@ -2,6 +2,7 @@
 #include "dronecode_sdk_impl.h"
 #include "global_include.h"
 #include "px4_custom_mode.h"
+#include <cmath>
 
 namespace dronecode_sdk {
 
@@ -158,8 +159,8 @@ Action::Result ActionImpl::goto_location(double latitude_deg,
     command.command = MAV_CMD_DO_REPOSITION;
     command.target_component_id = _parent->get_autopilot_id();
     command.params.param4 = to_rad_from_deg(yaw_deg);
-    command.params.x = (int32_t)(latitude_deg * 1E7);
-    command.params.y = (int32_t)(longitude_deg * 1E7);
+    command.params.x = int32_t(std::round(latitude_deg * 1e7));
+    command.params.y = int32_t(std::round(longitude_deg * 1e7));
     command.params.z = altitude_amsl_m;
 
     return action_result_from_command_result(_parent->send_command(command));

plugins/follow_me/follow_me_impl.cpp

@@ -2,6 +2,7 @@
 #include "system.h"
 #include "global_include.h"
 #include "px4_custom_mode.h"
+#include <cmath>
 
 namespace dronecode_sdk {
 
@@ -253,8 +254,8 @@ void FollowMeImpl::send_target_location()
     //   LogDebug() << debug_str <<  "Lat: " << _target_location.latitude_deg << " Lon: " <<
     //   _target_location.longitude_deg <<
     //	" Alt: " << _target_location.absolute_altitude_m;
-    const int32_t lat_int = static_cast<int32_t>(_target_location.latitude_deg * 1e7);
-    const int32_t lon_int = static_cast<int32_t>(_target_location.longitude_deg * 1e7);
+    const int32_t lat_int = int32_t(std::round(_target_location.latitude_deg * 1e7));
+    const int32_t lon_int = int32_t(std::round(_target_location.longitude_deg * 1e7));
     const float alt = static_cast<float>(_target_location.absolute_altitude_m);
     _mutex.unlock();
 

plugins/gimbal/gimbal_impl.cpp

@@ -1,6 +1,7 @@
 #include "gimbal_impl.h"
 #include "global_include.h"
 #include <functional>
+#include <cmath>
 
 namespace dronecode_sdk {
 
@@ -61,8 +62,8 @@ GimbalImpl::set_roi_location(double latitude_deg, double longitude_deg, float al
     MAVLinkCommands::CommandInt command{};
 
     command.command = MAV_CMD_DO_SET_ROI_LOCATION;
-    command.params.x = int32_t(latitude_deg * 1e7);
-    command.params.y = int32_t(longitude_deg * 1e7);
+    command.params.x = int32_t(std::round(latitude_deg * 1e7));
+    command.params.y = int32_t(std::round(longitude_deg * 1e7));
     command.params.z = altitude_m;
     command.target_component_id = _parent->get_autopilot_id();
 
@@ -77,8 +78,8 @@ void GimbalImpl::set_roi_location_async(double latitude_deg,
     MAVLinkCommands::CommandInt command{};
 
     command.command = MAV_CMD_DO_SET_ROI_LOCATION;
-    command.params.x = int32_t(latitude_deg * 1e7);
-    command.params.y = int32_t(longitude_deg * 1e7);
+    command.params.x = int32_t(std::round(latitude_deg * 1e7));
+    command.params.y = int32_t(std::round(longitude_deg * 1e7));
     command.params.z = altitude_m;
     command.target_component_id = _parent->get_autopilot_id();
 

plugins/mission/mission.cpp

@@ -65,18 +65,20 @@ const char *Mission::result_str(Result result)
     switch (result) {
         case Result::SUCCESS:
             return "Success";
-        case Result::BUSY:
-            return "Busy";
         case Result::ERROR:
             return "Error";
         case Result::TOO_MANY_MISSION_ITEMS:
             return "Too many mission items";
-        case Result::INVALID_ARGUMENT:
-            return "Invalid argument";
+        case Result::BUSY:
+            return "Busy";
         case Result::TIMEOUT:
             return "Timeout";
+        case Result::INVALID_ARGUMENT:
+            return "Invalid argument";
         case Result::UNSUPPORTED:
             return "Mission downloaded from system is unsupported";
+        case Result::NO_MISSION_AVAILABLE:
+            return "No mission available";
         case Result::FAILED_TO_OPEN_QGC_PLAN:
             return "Failed to open QGC plan";
         case Result::FAILED_TO_PARSE_QGC_PLAN:

plugins/mission/mission_item_impl.cpp

@@ -108,12 +108,12 @@ float MissionItemImpl::get_mavlink_param4() const
 
 int32_t MissionItemImpl::get_mavlink_x() const
 {
-    return int32_t(_latitude_deg * 1e7);
+    return int32_t(std::round(_latitude_deg * 1e7));
 }
 
 int32_t MissionItemImpl::get_mavlink_y() const
 {
-    return int32_t(_longitude_deg * 1e7);
+    return int32_t(std::round(_longitude_deg * 1e7));
 }
 
 float MissionItemImpl::get_mavlink_z() const

plugins/mission_raw/CMakeLists.txt

@@ -0,0 +1,33 @@
+add_library(dronecode_sdk_mission_raw ${PLUGIN_LIBRARY_TYPE}
+    mission_raw.cpp
+    mission_raw_impl.cpp
+)
+
+include_directories(
+    ${PROJECT_SOURCE_DIR}/core
+)
+
+set_target_properties(dronecode_sdk_mission_raw
+    PROPERTIES COMPILE_FLAGS ${warnings}
+)
+
+target_link_libraries(dronecode_sdk_mission_raw
+    dronecode_sdk
+)
+
+install(FILES
+    include/plugins/mission_raw/mission_raw.h
+    DESTINATION ${dronecode_sdk_install_include_dir}
+)
+
+install(TARGETS dronecode_sdk_mission_raw
+    #EXPORT dronecode_sdk-targets
+    DESTINATION ${dronecode_sdk_install_lib_dir}
+)
+
+target_include_directories(dronecode_sdk_mission_raw
+PUBLIC
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)