From c5c98e35d49771b4587a76c68c73293ca4d7515d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 8 Jun 2018 14:15:53 +0200 Subject: [PATCH 1/3] mission: import null as NaN When QGC sets a param as `null` it means unchanged which should translate to NaN for mavlink mission items. --- plugins/mission/mission_impl.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/plugins/mission/mission_impl.cpp b/plugins/mission/mission_impl.cpp index 78e1e4828a..8c531cca0f 100644 --- a/plugins/mission/mission_impl.cpp +++ b/plugins/mission/mission_impl.cpp @@ -1203,7 +1203,12 @@ Mission::Result MissionImpl::import_mission_items(Mission::mission_items_t &all_ // Extract parameters of each mission item std::vector params; for (auto &p : json_mission_item["params"].array_items()) { - params.push_back(p.number_value()); + if (p.is_null()) { + // QGC sets params as `null` if they should be unchanged. + params.push_back(NAN); + } else { + params.push_back(p.number_value()); + } } result = build_mission_items(command, params, new_mission_item, all_mission_items); From 4a8f74862f25c51214d78bc276e84d0cd90c2e49 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 8 Jun 2018 14:25:41 +0200 Subject: [PATCH 2/3] mission: update unit test that null is NaN We should add the functionality to support the yaw setting and then test this here as well. --- plugins/mission/mission_import_qgc_test.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/plugins/mission/mission_import_qgc_test.cpp b/plugins/mission/mission_import_qgc_test.cpp index 2cde2b1e62..5e505a9cc7 100644 --- a/plugins/mission/mission_import_qgc_test.cpp +++ b/plugins/mission/mission_import_qgc_test.cpp @@ -38,8 +38,8 @@ TEST(QGCMissionImport, ValidateQGCMissonItems) // These mission items are meant to match those in // file:://plugins/mission/qgroundcontrol_sample.plan QGCMissionItem items_test[] = { - {MAV_CMD_NAV_TAKEOFF, {0., 0., 0., 0., 47.39781011, 8.54553801, 15.}}, - {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., 0., 47.39779921, 8.54546693, 15.}}, + {MAV_CMD_NAV_TAKEOFF, {0., 0., 0., NAN, 47.39781011, 8.54553801, 15.}}, + {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., NAN, 47.39779921, 8.54546693, 15.}}, {MAV_CMD_DO_MOUNT_CONTROL, {25.0, 0., 50.0}}, // Gimbal pitch & yaw in deg {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., 0., 47.39773658, 8.54543743, 15.}}, {MAV_CMD_IMAGE_START_CAPTURE, @@ -55,13 +55,13 @@ TEST(QGCMissionImport, ValidateQGCMissonItems) 1., 0., }}, // Start image capture - {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., 0., 47.39768029, 8.54561177, 15.}}, + {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., NAN, 47.39768029, 8.54561177, 15.}}, {MAV_CMD_DO_CHANGE_SPEED, {1., 100., -1., 0}}, - {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., 0., 47.39779649, 8.54566005, 15.}}, + {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., NAN, 47.39779649, 8.54566005, 15.}}, {MAV_CMD_NAV_LOITER_TIME, {30.}}, // Loiter for 30 seconds - {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., 0., 47.39779468, 8.54561445, 15.}}, + {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., NAN, 47.39779468, 8.54561445, 15.}}, {MAV_CMD_VIDEO_START_CAPTURE, {}}, // Start video capture - {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., 0., 47.39784279, 8.54553533, 15.}}, + {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., NAN, 47.39784279, 8.54553533, 15.}}, {MAV_CMD_IMAGE_STOP_CAPTURE, {}}, // Stop image capture {MAV_CMD_VIDEO_STOP_CAPTURE, {}}, // Stop video capture }; From 20676e4fda30311806f39be20859ae0b3c4762d0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 8 Jun 2018 16:46:29 +0200 Subject: [PATCH 3/3] mission: NAN needs to be double here --- plugins/mission/mission_import_qgc_test.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/plugins/mission/mission_import_qgc_test.cpp b/plugins/mission/mission_import_qgc_test.cpp index 5e505a9cc7..fe3f0d5711 100644 --- a/plugins/mission/mission_import_qgc_test.cpp +++ b/plugins/mission/mission_import_qgc_test.cpp @@ -38,8 +38,8 @@ TEST(QGCMissionImport, ValidateQGCMissonItems) // These mission items are meant to match those in // file:://plugins/mission/qgroundcontrol_sample.plan QGCMissionItem items_test[] = { - {MAV_CMD_NAV_TAKEOFF, {0., 0., 0., NAN, 47.39781011, 8.54553801, 15.}}, - {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., NAN, 47.39779921, 8.54546693, 15.}}, + {MAV_CMD_NAV_TAKEOFF, {0., 0., 0., double(NAN), 47.39781011, 8.54553801, 15.}}, + {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., double(NAN), 47.39779921, 8.54546693, 15.}}, {MAV_CMD_DO_MOUNT_CONTROL, {25.0, 0., 50.0}}, // Gimbal pitch & yaw in deg {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., 0., 47.39773658, 8.54543743, 15.}}, {MAV_CMD_IMAGE_START_CAPTURE, @@ -55,13 +55,13 @@ TEST(QGCMissionImport, ValidateQGCMissonItems) 1., 0., }}, // Start image capture - {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., NAN, 47.39768029, 8.54561177, 15.}}, + {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., double(NAN), 47.39768029, 8.54561177, 15.}}, {MAV_CMD_DO_CHANGE_SPEED, {1., 100., -1., 0}}, - {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., NAN, 47.39779649, 8.54566005, 15.}}, + {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., double(NAN), 47.39779649, 8.54566005, 15.}}, {MAV_CMD_NAV_LOITER_TIME, {30.}}, // Loiter for 30 seconds - {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., NAN, 47.39779468, 8.54561445, 15.}}, + {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., double(NAN), 47.39779468, 8.54561445, 15.}}, {MAV_CMD_VIDEO_START_CAPTURE, {}}, // Start video capture - {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., NAN, 47.39784279, 8.54553533, 15.}}, + {MAV_CMD_NAV_WAYPOINT, {0., 0., 0., double(NAN), 47.39784279, 8.54553533, 15.}}, {MAV_CMD_IMAGE_STOP_CAPTURE, {}}, // Stop image capture {MAV_CMD_VIDEO_STOP_CAPTURE, {}}, // Stop video capture };