From e09ac5a45247073c45c52ba568a6751ffba55fd7 Mon Sep 17 00:00:00 2001 From: Shakthi Prashanth M Date: Fri, 6 Apr 2018 18:54:02 +0530 Subject: [PATCH] Add "reserved_val" as argument to `set_as_reserved()` Most of the "Reserved" values in MAVLink spec are NAN. However, in some cases "Reserved" value could be "0". This utility method supplied can be used to set any value to mark as "Reserved". For.eg: For commands `MAV_CMD_LOGGING_START`, `MAV_CMD_LOGGING_STOP` etc, "Reserved" value is 0. --- core/mavlink_commands.h | 34 ++++++++++++++++++-------------- plugins/logging/logging_impl.cpp | 8 ++++---- 2 files changed, 23 insertions(+), 19 deletions(-) diff --git a/core/mavlink_commands.h b/core/mavlink_commands.h index a274eb77b5..80595a02bf 100644 --- a/core/mavlink_commands.h +++ b/core/mavlink_commands.h @@ -6,7 +6,6 @@ #include #include #include -#include namespace dronecore { @@ -38,6 +37,7 @@ class MAVLinkCommands uint16_t command; bool current = 0; bool autocontinue = false; + // Most of the "Reserved" values in MAVLink spec are NAN. struct Params { float param1 = NAN; float param2 = NAN; @@ -48,15 +48,18 @@ class MAVLinkCommands float z = NAN; } params; - static void set_as_reserved(Params ¶ms) + // In some cases "Reserved" value could be "0". + // This utility method can be used in such case. + static + void set_as_reserved(Params ¶ms, float reserve_val = NAN) { - params.param1 = 0.f; - params.param2 = 0.f; - params.param3 = 0.f; - params.param4 = 0.f; + params.param1 = reserve_val; + params.param2 = reserve_val; + params.param3 = reserve_val; + params.param4 = reserve_val; params.x = 0; params.y = 0; - params.z = 0.f; + params.z = reserve_val; } }; @@ -75,15 +78,16 @@ class MAVLinkCommands float param7 = NAN; } params; - static void set_as_reserved(Params ¶ms) + static + void set_as_reserved(Params ¶ms, float reserve_val = NAN) { - params.param1 = 0.f; - params.param2 = 0.f; - params.param3 = 0.f; - params.param4 = 0.f; - params.param5 = 0.f; - params.param6 = 0.f; - params.param7 = 0.f; + params.param1 = reserve_val; + params.param2 = reserve_val; + params.param3 = reserve_val; + params.param4 = reserve_val; + params.param5 = reserve_val; + params.param6 = reserve_val; + params.param7 = reserve_val; } }; diff --git a/plugins/logging/logging_impl.cpp b/plugins/logging/logging_impl.cpp index e467f99a91..942d4aacc9 100644 --- a/plugins/logging/logging_impl.cpp +++ b/plugins/logging/logging_impl.cpp @@ -43,7 +43,7 @@ Logging::Result LoggingImpl::start_logging() const MAVLinkCommands::CommandLong cmd {}; cmd.command = MAV_CMD_LOGGING_START; - MAVLinkCommands::CommandLong::set_as_reserved(cmd.params); + MAVLinkCommands::CommandLong::set_as_reserved(cmd.params, 0.f); cmd.target_component_id = _parent->get_autopilot_id(); return logging_result_from_command_result(_parent->send_command(cmd)); @@ -54,7 +54,7 @@ Logging::Result LoggingImpl::stop_logging() const MAVLinkCommands::CommandLong cmd {}; cmd.command = MAV_CMD_LOGGING_STOP; - MAVLinkCommands::CommandLong::set_as_reserved(cmd.params); + MAVLinkCommands::CommandLong::set_as_reserved(cmd.params, 0.f); cmd.target_component_id = _parent->get_autopilot_id(); return logging_result_from_command_result(_parent->send_command(cmd)); @@ -65,7 +65,7 @@ void LoggingImpl::start_logging_async(const Logging::result_callback_t &callback MAVLinkCommands::CommandLong cmd {}; cmd.command = MAV_CMD_LOGGING_START; - MAVLinkCommands::CommandLong::set_as_reserved(cmd.params); + MAVLinkCommands::CommandLong::set_as_reserved(cmd.params, 0.f); cmd.target_component_id = _parent->get_autopilot_id(); _parent->send_command_async(cmd, std::bind(&LoggingImpl::command_result_callback, @@ -78,7 +78,7 @@ void LoggingImpl::stop_logging_async(const Logging::result_callback_t &callback) MAVLinkCommands::CommandLong cmd {}; cmd.command = MAV_CMD_LOGGING_STOP; - MAVLinkCommands::CommandLong::set_as_reserved(cmd.params); + MAVLinkCommands::CommandLong::set_as_reserved(cmd.params, 0.f); cmd.target_component_id = _parent->get_autopilot_id(); _parent->send_command_async(cmd, std::bind(&LoggingImpl::command_result_callback,