From 2727ecafad5e9ff9343592510a2842c11e508ad0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jul 2018 11:00:30 +0200 Subject: [PATCH 01/10] plugins: enable warnings By oversight we did not have the special warnings enabled for the plugins. --- plugins/action/CMakeLists.txt | 4 ++++ plugins/calibration/CMakeLists.txt | 4 ++++ plugins/camera/CMakeLists.txt | 4 ++++ plugins/follow_me/CMakeLists.txt | 4 ++++ plugins/gimbal/CMakeLists.txt | 4 ++++ plugins/info/CMakeLists.txt | 4 ++++ plugins/logging/CMakeLists.txt | 4 ++++ plugins/mission/CMakeLists.txt | 4 ++++ plugins/offboard/CMakeLists.txt | 4 ++++ plugins/telemetry/CMakeLists.txt | 4 ++++ 10 files changed, 40 insertions(+) diff --git a/plugins/action/CMakeLists.txt b/plugins/action/CMakeLists.txt index 9d0ab827f1..cc175d2ca0 100644 --- a/plugins/action/CMakeLists.txt +++ b/plugins/action/CMakeLists.txt @@ -7,6 +7,10 @@ target_link_libraries(dronecode_sdk_action dronecode_sdk ) +set_target_properties(dronecode_sdk_action + PROPERTIES COMPILE_FLAGS ${warnings} +) + install(FILES action.h action_result.h diff --git a/plugins/calibration/CMakeLists.txt b/plugins/calibration/CMakeLists.txt index b2b0d4534d..1431e39c1d 100644 --- a/plugins/calibration/CMakeLists.txt +++ b/plugins/calibration/CMakeLists.txt @@ -8,6 +8,10 @@ target_link_libraries(dronecode_sdk_calibration dronecode_sdk ) +set_target_properties(dronecode_sdk_calibration + PROPERTIES COMPILE_FLAGS ${warnings} +) + install(FILES calibration.h DESTINATION ${dronecode_sdk_install_include_dir} diff --git a/plugins/camera/CMakeLists.txt b/plugins/camera/CMakeLists.txt index 171cff7310..141ac7c09f 100644 --- a/plugins/camera/CMakeLists.txt +++ b/plugins/camera/CMakeLists.txt @@ -10,6 +10,10 @@ target_link_libraries(dronecode_sdk_camera ${CURL_LIBRARY} ) +set_target_properties(dronecode_sdk_camera + PROPERTIES COMPILE_FLAGS ${warnings} +) + include_directories( ${CURL_INCLUDE_DIRS} ) diff --git a/plugins/follow_me/CMakeLists.txt b/plugins/follow_me/CMakeLists.txt index 51e6c9d201..a90ed11024 100644 --- a/plugins/follow_me/CMakeLists.txt +++ b/plugins/follow_me/CMakeLists.txt @@ -7,6 +7,10 @@ target_link_libraries(dronecode_sdk_follow_me dronecode_sdk ) +set_target_properties(dronecode_sdk_follow_me + PROPERTIES COMPILE_FLAGS ${warnings} +) + install(FILES follow_me.h DESTINATION ${dronecode_sdk_install_include_dir} diff --git a/plugins/gimbal/CMakeLists.txt b/plugins/gimbal/CMakeLists.txt index 1d7d29002f..9ade113f43 100644 --- a/plugins/gimbal/CMakeLists.txt +++ b/plugins/gimbal/CMakeLists.txt @@ -7,6 +7,10 @@ target_link_libraries(dronecode_sdk_gimbal dronecode_sdk ) +set_target_properties(dronecode_sdk_gimbal + PROPERTIES COMPILE_FLAGS ${warnings} +) + install(FILES gimbal.h DESTINATION ${dronecode_sdk_install_include_dir} diff --git a/plugins/info/CMakeLists.txt b/plugins/info/CMakeLists.txt index b3e30842a1..c16b342274 100644 --- a/plugins/info/CMakeLists.txt +++ b/plugins/info/CMakeLists.txt @@ -7,6 +7,10 @@ target_link_libraries(dronecode_sdk_info dronecode_sdk ) +set_target_properties(dronecode_sdk_info + PROPERTIES COMPILE_FLAGS ${warnings} +) + install(FILES info.h DESTINATION ${dronecode_sdk_install_include_dir} diff --git a/plugins/logging/CMakeLists.txt b/plugins/logging/CMakeLists.txt index ee77db117f..f0a258af66 100644 --- a/plugins/logging/CMakeLists.txt +++ b/plugins/logging/CMakeLists.txt @@ -7,6 +7,10 @@ target_link_libraries(dronecode_sdk_logging dronecode_sdk ) +set_target_properties(dronecode_sdk_logging + PROPERTIES COMPILE_FLAGS ${warnings} +) + install(FILES logging.h DESTINATION ${dronecode_sdk_install_include_dir} diff --git a/plugins/mission/CMakeLists.txt b/plugins/mission/CMakeLists.txt index 947762a55c..f415022d77 100644 --- a/plugins/mission/CMakeLists.txt +++ b/plugins/mission/CMakeLists.txt @@ -10,6 +10,10 @@ include_directories( SYSTEM third_party/json11 ) +set_target_properties(dronecode_sdk_mission + PROPERTIES COMPILE_FLAGS ${warnings} +) + # JSON parser library for parsing QGC plan for Mission add_subdirectory(third_party/json11) diff --git a/plugins/offboard/CMakeLists.txt b/plugins/offboard/CMakeLists.txt index 93a7c2b8aa..f4d2c45d9c 100644 --- a/plugins/offboard/CMakeLists.txt +++ b/plugins/offboard/CMakeLists.txt @@ -7,6 +7,10 @@ target_link_libraries(dronecode_sdk_offboard dronecode_sdk ) +set_target_properties(dronecode_sdk_offboard + PROPERTIES COMPILE_FLAGS ${warnings} +) + install(FILES offboard.h DESTINATION ${dronecode_sdk_install_include_dir} diff --git a/plugins/telemetry/CMakeLists.txt b/plugins/telemetry/CMakeLists.txt index 041202a0b2..18bca3d4fb 100644 --- a/plugins/telemetry/CMakeLists.txt +++ b/plugins/telemetry/CMakeLists.txt @@ -8,6 +8,10 @@ target_link_libraries(dronecode_sdk_telemetry dronecode_sdk ) +set_target_properties(dronecode_sdk_telemetry + PROPERTIES COMPILE_FLAGS ${warnings} +) + install(FILES telemetry.h DESTINATION ${dronecode_sdk_install_include_dir} From f8db29c125dbc6b73cddd8e4d4b4b70b00bb62df Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jul 2018 11:06:53 +0200 Subject: [PATCH 02/10] calibration: fix warning statement had no effect --- plugins/calibration/calibration_impl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/calibration/calibration_impl.cpp b/plugins/calibration/calibration_impl.cpp index 4b60dd87f6..c39f3bb211 100644 --- a/plugins/calibration/calibration_impl.cpp +++ b/plugins/calibration/calibration_impl.cpp @@ -330,7 +330,7 @@ void CalibrationImpl::process_statustext(const mavlink_message_t &message) // FALLTHROUGH case CalibrationStatustextParser::Status::CANCELLED: _calibration_callback = nullptr; - _state == State::NONE; + _state = State::NONE; break; default: break; From b58f65720352f8fbf1ebffc31153891ab2d68602 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jul 2018 11:08:19 +0200 Subject: [PATCH 03/10] camera: fixed several warnings --- plugins/camera/camera_impl.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index 35e8dcd4d6..0417989983 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -396,8 +396,8 @@ Camera::Result CameraImpl::get_video_stream_info(Camera::VideoStreamInfo &info) auto prom = std::make_shared>(); auto ret = prom->get_future(); - get_video_stream_info_async([prom](Camera::Result result, Camera::VideoStreamInfo info) { - UNUSED(info); + get_video_stream_info_async([prom](Camera::Result result, Camera::VideoStreamInfo info_gotten) { + UNUSED(info_gotten); prom->set_value(result); }); @@ -426,6 +426,7 @@ void CameraImpl::get_video_stream_info_async( auto command = make_command_request_video_stream_info(); _parent->send_command_async(command, [this](MAVLinkCommands::Result result, float progress) { + UNUSED(progress); Camera::Result camera_result = camera_result_from_command_result(result); if (camera_result != Camera::Result::SUCCESS) { if (_video_stream_info.callback) { @@ -1102,7 +1103,7 @@ void CameraImpl::get_option_async(const std::string &setting_id, LogWarn() << "The param was probably outdated, trying to fetch it"; _parent->get_param_async( setting_id, - [setting_id, this](bool success, MAVLinkParameters::ParamValue value) { + [setting_id, this](bool success, MAVLinkParameters::ParamValue value_gotten) { if (!success) { LogWarn() << "Fetching the param failed"; return; @@ -1111,7 +1112,7 @@ void CameraImpl::get_option_async(const std::string &setting_id, if (!this->_camera_definition) { return; } - this->_camera_definition->set_setting(setting_id, value); + this->_camera_definition->set_setting(setting_id, value_gotten); }, true); @@ -1204,7 +1205,7 @@ void CameraImpl::refresh_params() return; } - int count = 0; + unsigned count = 0; for (const auto ¶m : params) { std::string param_name = param; const bool is_last = (count + 1 == params.size()); From f3c70bbadb0fe9f3e980ec29d04ceff850628049 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jul 2018 11:08:42 +0200 Subject: [PATCH 04/10] telemetry: fixed warnings --- plugins/telemetry/telemetry.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index 24bd85e28d..b98fb23330 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -319,17 +319,17 @@ const char *Telemetry::result_str(Result result) bool operator==(const Telemetry::PositionVelocityNED &lhs, const Telemetry::PositionVelocityNED &rhs) { - return abs(lhs.position.north_m - rhs.position.north_m) <= - std::numeric_limits::epsilon() && - abs(lhs.position.east_m - rhs.position.east_m) <= - std::numeric_limits::epsilon() && - abs(lhs.position.down_m - rhs.position.down_m) <= + return fabs(lhs.position.north_m - rhs.position.north_m) <= std::numeric_limits::epsilon() && - abs(lhs.velocity.north_m_s - rhs.velocity.north_m_s) <= + fabs(lhs.position.east_m - rhs.position.east_m) <= std::numeric_limits::epsilon() && - abs(lhs.velocity.east_m_s - rhs.velocity.east_m_s) <= + fabs(lhs.position.down_m - rhs.position.down_m) <= std::numeric_limits::epsilon() && - abs(lhs.velocity.down_m_s - rhs.velocity.down_m_s) <= + fabs(lhs.velocity.north_m_s - rhs.velocity.north_m_s) <= + std::numeric_limits::epsilon() && + fabs(lhs.velocity.east_m_s - rhs.velocity.east_m_s) <= + std::numeric_limits::epsilon() && + fabs(lhs.velocity.down_m_s - rhs.velocity.down_m_s) <= std::numeric_limits::epsilon(); } From 10825cc6d51107570d20a8ac1d5fe1f3cfd774a6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jul 2018 11:20:48 +0200 Subject: [PATCH 05/10] info: fix warning We need to reserve one char for null termination. --- plugins/info/info_impl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/info/info_impl.cpp b/plugins/info/info_impl.cpp index f851bb851c..6f14e37716 100644 --- a/plugins/info/info_impl.cpp +++ b/plugins/info/info_impl.cpp @@ -100,11 +100,11 @@ void InfoImpl::process_autopilot_version(const mavlink_message_t &message) product.vendor_id = autopilot_version.vendor_id; const char *vendor_name = vendor_id_str(autopilot_version.vendor_id); - STRNCPY(product.vendor_name, vendor_name, sizeof(product.vendor_name)); + STRNCPY(product.vendor_name, vendor_name, sizeof(product.vendor_name - 1)); product.product_id = autopilot_version.product_id; const char *product_name = product_id_str(autopilot_version.product_id); - STRNCPY(product.product_name, product_name, sizeof(product.product_name)); + STRNCPY(product.product_name, product_name, sizeof(product.product_name - 1)); set_product(product); } From 56d0f81d6db1eb1b7520dbb85edded8d96a6069d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jul 2018 12:52:39 +0200 Subject: [PATCH 06/10] core: get rid of global math.h/cmath include We want to try to avoid to use abs/fabs and instead use std::abs/std::fabs. --- core/global_include.cpp | 5 +++-- core/global_include.h | 19 ++++++++----------- integration_tests/mission.cpp | 1 + integration_tests/simple_hover.cpp | 1 + plugins/camera/camera_impl.cpp | 1 + plugins/telemetry/telemetry.cpp | 23 +++++++++++++---------- 6 files changed, 27 insertions(+), 23 deletions(-) diff --git a/core/global_include.cpp b/core/global_include.cpp index 62c3f95d14..102c828ac6 100644 --- a/core/global_include.cpp +++ b/core/global_include.cpp @@ -3,6 +3,7 @@ #include #include #include +#include namespace dronecode_sdk { @@ -149,12 +150,12 @@ float to_deg_from_rad(float rad) bool are_equal(float one, float two) { - return (fabsf(one - two) < std::numeric_limits::epsilon()); + return (std::fabs(one - two) < std::numeric_limits::epsilon()); } bool are_equal(double one, double two) { - return (fabs(one - two) < std::numeric_limits::epsilon()); + return (std::fabs(one - two) < std::numeric_limits::epsilon()); } } // namespace dronecode_sdk diff --git a/core/global_include.h b/core/global_include.h index ccd4f63191..366b6f4a80 100644 --- a/core/global_include.h +++ b/core/global_include.h @@ -5,14 +5,15 @@ #include #include -#ifdef WINDOWS -#ifndef _USE_MATH_DEFINES -#define _USE_MATH_DEFINES +// Instead of using the constant from math.h or cmath we define it ourselves. This way +// we don't import all the other C math functions and make sure to use the C++ functions +// from the standard library (e.g. std::abs() instead of abs()). +#ifndef M_PI +constexpr double M_PI = 3.14159265358979323846; #endif -// cmath doesn't contain M_PI -#include -#else -#include + +#ifndef M_PI_F +constexpr float M_PI_F = float(M_PI); #endif #define MIN(x_, y_) ((x_) > (y_)) ? (y_) : (x_) @@ -24,10 +25,6 @@ #define STRNCPY strncpy #endif -#ifndef M_PI_F -#define M_PI_F float(M_PI) -#endif - namespace dronecode_sdk { typedef std::chrono::time_point dl_time_t; diff --git a/integration_tests/mission.cpp b/integration_tests/mission.cpp index 8fd6e74bc4..cb4d5f431d 100644 --- a/integration_tests/mission.cpp +++ b/integration_tests/mission.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include "integration_test_helper.h" #include "dronecode_sdk.h" #include "plugins/telemetry/telemetry.h" diff --git a/integration_tests/simple_hover.cpp b/integration_tests/simple_hover.cpp index 88a8c8138d..1ff5ec29f9 100644 --- a/integration_tests/simple_hover.cpp +++ b/integration_tests/simple_hover.cpp @@ -1,4 +1,5 @@ #include +#include #include "integration_test_helper.h" #include "dronecode_sdk.h" #include "plugins/action/action.h" diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index 0417989983..3aeec63cc0 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -5,6 +5,7 @@ #include "mavlink_include.h" #include "http_loader.h" #include +#include namespace dronecode_sdk { diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index b98fb23330..98cd0c6c72 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -1,6 +1,7 @@ #include "telemetry.h" #include +#include #include "telemetry_impl.h" @@ -319,27 +320,29 @@ const char *Telemetry::result_str(Result result) bool operator==(const Telemetry::PositionVelocityNED &lhs, const Telemetry::PositionVelocityNED &rhs) { - return fabs(lhs.position.north_m - rhs.position.north_m) <= + return std::fabs(lhs.position.north_m - rhs.position.north_m) <= std::numeric_limits::epsilon() && - fabs(lhs.position.east_m - rhs.position.east_m) <= + std::fabs(lhs.position.east_m - rhs.position.east_m) <= std::numeric_limits::epsilon() && - fabs(lhs.position.down_m - rhs.position.down_m) <= + std::fabs(lhs.position.down_m - rhs.position.down_m) <= std::numeric_limits::epsilon() && - fabs(lhs.velocity.north_m_s - rhs.velocity.north_m_s) <= + std::fabs(lhs.velocity.north_m_s - rhs.velocity.north_m_s) <= std::numeric_limits::epsilon() && - fabs(lhs.velocity.east_m_s - rhs.velocity.east_m_s) <= + std::fabs(lhs.velocity.east_m_s - rhs.velocity.east_m_s) <= std::numeric_limits::epsilon() && - fabs(lhs.velocity.down_m_s - rhs.velocity.down_m_s) <= + std::fabs(lhs.velocity.down_m_s - rhs.velocity.down_m_s) <= std::numeric_limits::epsilon(); } bool operator==(const Telemetry::Position &lhs, const Telemetry::Position &rhs) { - return abs(lhs.latitude_deg - rhs.latitude_deg) <= std::numeric_limits::epsilon() && - abs(lhs.longitude_deg - rhs.longitude_deg) <= std::numeric_limits::epsilon() && - abs(lhs.absolute_altitude_m - rhs.absolute_altitude_m) <= + return std::abs(lhs.latitude_deg - rhs.latitude_deg) <= + std::numeric_limits::epsilon() && + std::abs(lhs.longitude_deg - rhs.longitude_deg) <= + std::numeric_limits::epsilon() && + std::abs(lhs.absolute_altitude_m - rhs.absolute_altitude_m) <= std::numeric_limits::epsilon() && - abs(lhs.relative_altitude_m - rhs.relative_altitude_m) <= + std::abs(lhs.relative_altitude_m - rhs.relative_altitude_m) <= std::numeric_limits::epsilon(); } From efd26abe5e8b3a008c4065b69945fd6d01091b3a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jul 2018 12:56:30 +0200 Subject: [PATCH 07/10] camera: fix missing cast --- plugins/camera/camera_impl.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index 3aeec63cc0..6d83e1ffc3 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -510,8 +510,13 @@ Camera::Result CameraImpl::set_mode(const Camera::Mode mode) void CameraImpl::save_camera_mode(const float mavlink_camera_mode) { + if (!std::isfinite(mavlink_camera_mode)) { + LogWarn() << "Can't save NAN as camera mode"; + return; + } + MAVLinkParameters::ParamValue value; - value.set_uint32(mavlink_camera_mode); + value.set_uint32(uint32_t(mavlink_camera_mode)); _camera_definition->set_setting("CAM_MODE", value); refresh_params(); } From 33365ebb91e10219df4f6172bbe5fa53ed498cb3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jul 2018 13:18:00 +0200 Subject: [PATCH 08/10] mission: add missing include --- plugins/mission/mission_import_qgc_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/mission/mission_import_qgc_test.cpp b/plugins/mission/mission_import_qgc_test.cpp index b9f7a21632..3120cce6ed 100644 --- a/plugins/mission/mission_import_qgc_test.cpp +++ b/plugins/mission/mission_import_qgc_test.cpp @@ -5,7 +5,7 @@ #include #include -#include +#include #include "mission.h" #include "mavlink_include.h" #include "global_include.h" From c8bfbfdab290e47cf8a8df986c344983288eb2b1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jul 2018 13:19:36 +0200 Subject: [PATCH 09/10] info: -1 inside a sizeof doesn't make sense --- plugins/info/info_impl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/info/info_impl.cpp b/plugins/info/info_impl.cpp index 6f14e37716..597e311927 100644 --- a/plugins/info/info_impl.cpp +++ b/plugins/info/info_impl.cpp @@ -100,11 +100,11 @@ void InfoImpl::process_autopilot_version(const mavlink_message_t &message) product.vendor_id = autopilot_version.vendor_id; const char *vendor_name = vendor_id_str(autopilot_version.vendor_id); - STRNCPY(product.vendor_name, vendor_name, sizeof(product.vendor_name - 1)); + STRNCPY(product.vendor_name, vendor_name, sizeof(product.vendor_name) - 1); product.product_id = autopilot_version.product_id; const char *product_name = product_id_str(autopilot_version.product_id); - STRNCPY(product.product_name, product_name, sizeof(product.product_name - 1)); + STRNCPY(product.product_name, product_name, sizeof(product.product_name) - 1); set_product(product); } From 34d18d9e78d94480c01bd592252c5fe18a498921 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jul 2018 14:09:50 +0200 Subject: [PATCH 10/10] mission: fix NAN casting to double --- plugins/mission/mission_impl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/mission/mission_impl.cpp b/plugins/mission/mission_impl.cpp index 2237619331..70dc1a5991 100644 --- a/plugins/mission/mission_impl.cpp +++ b/plugins/mission/mission_impl.cpp @@ -1210,7 +1210,7 @@ Mission::Result MissionImpl::import_mission_items(Mission::mission_items_t &all_ for (auto &p : json_mission_item["params"].array_items()) { if (p.is_null()) { // QGC sets params as `null` if they should be unchanged. - params.push_back(NAN); + params.push_back(double(NAN)); } else { params.push_back(p.number_value()); }