mavlink_commands: make clear separation between command types structu…

…re in MavlinkCommandReceiver
mavlink · Oct 22, 2020 · c35d2c9 · c35d2c9
1 parent d62c356
commit c35d2c9
Show file tree

Hide file tree

Showing 5 changed files with 116 additions and 51 deletions.
diff --git a/src/core/mavlink_commands.cpp b/src/core/mavlink_commands.cpp
@@ -346,12 +346,12 @@ void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& messag
 {
     mavlink_command_int_t command_int;
     mavlink_msg_command_int_decode(&message, &command_int);
-    MavlinkCommandReceiver::Command cmd(command_int);
+    MavlinkCommandReceiver::CommandInt cmd(command_int);
 
     std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);
 
-    for (auto it = _mavlink_command_handler_table.begin();
-         it != _mavlink_command_handler_table.end();
+    for (auto it = _mavlink_command_int_handler_table.begin();
+         it != _mavlink_command_int_handler_table.end();
          ++it) {
         if (it->cmd_id == command_int.command) {
             it->callback(cmd);
@@ -363,12 +363,12 @@ void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& messa
 {
     mavlink_command_long_t command_long;
     mavlink_msg_command_long_decode(&message, &command_long);
-    MavlinkCommandReceiver::Command cmd(command_long);
+    MavlinkCommandReceiver::CommandLong cmd(command_long);
 
     std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);
 
-    for (auto it = _mavlink_command_handler_table.begin();
-         it != _mavlink_command_handler_table.end();
+    for (auto it = _mavlink_command_long_handler_table.begin();
+         it != _mavlink_command_long_handler_table.end();
          ++it) {
         if (it->cmd_id == command_long.command) {
             it->callback(cmd);
@@ -377,23 +377,44 @@ void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& messa
 }
 
 void MavlinkCommandReceiver::register_mavlink_command_handler(
-    uint16_t cmd_id, mavlink_command_handler_t callback, const void* cookie)
+    uint16_t cmd_id, mavlink_command_int_handler_t callback, const void* cookie)
 {
     std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);
 
-    MAVLinkCommandHandlerTableEntry entry = {cmd_id, callback, cookie};
-    _mavlink_command_handler_table.push_back(entry);
+    MAVLinkCommandIntHandlerTableEntry entry = {cmd_id, callback, cookie};
+    _mavlink_command_int_handler_table.push_back(entry);
+}
+
+void MavlinkCommandReceiver::register_mavlink_command_handler(
+    uint16_t cmd_id, mavlink_command_long_handler_t callback, const void* cookie)
+{
+    std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);
+
+    MAVLinkCommandLongHandlerTableEntry entry = {cmd_id, callback, cookie};
+    _mavlink_command_long_handler_table.push_back(entry);
 }
 
 void MavlinkCommandReceiver::unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie)
 {
     std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);
 
-    for (auto it = _mavlink_command_handler_table.begin();
-         it != _mavlink_command_handler_table.end();
+    // COMMAND_INT
+    for (auto it = _mavlink_command_int_handler_table.begin();
+         it != _mavlink_command_int_handler_table.end();
+         /* no ++it */) {
+        if (it->cmd_id == cmd_id && it->cookie == cookie) {
+            it = _mavlink_command_int_handler_table.erase(it);
+        } else {
+            ++it;
+        }
+    }
+
+    // COMMAND_LONG
+    for (auto it = _mavlink_command_long_handler_table.begin();
+         it != _mavlink_command_long_handler_table.end();
          /* no ++it */) {
         if (it->cmd_id == cmd_id && it->cookie == cookie) {
-            it = _mavlink_command_handler_table.erase(it);
+            it = _mavlink_command_long_handler_table.erase(it);
         } else {
             ++it;
         }
@@ -404,11 +425,23 @@ void MavlinkCommandReceiver::unregister_all_mavlink_command_handlers(const void*
 {
     std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);
 
-    for (auto it = _mavlink_command_handler_table.begin();
-         it != _mavlink_command_handler_table.end();
+    // COMMAND_INT
+    for (auto it = _mavlink_command_int_handler_table.begin();
+         it != _mavlink_command_int_handler_table.end();
+         /* no ++it */) {
+        if (it->cookie == cookie) {
+            it = _mavlink_command_int_handler_table.erase(it);
+        } else {
+            ++it;
+        }
+    }
+
+    // COMMAND_LONG
+    for (auto it = _mavlink_command_long_handler_table.begin();
+         it != _mavlink_command_long_handler_table.end();
          /* no ++it */) {
         if (it->cookie == cookie) {
-            it = _mavlink_command_handler_table.erase(it);
+            it = _mavlink_command_long_handler_table.erase(it);
         } else {
             ++it;
         }

diff --git a/src/core/mavlink_commands.h b/src/core/mavlink_commands.h
@@ -133,34 +133,25 @@ class MavlinkCommandReceiver {
     explicit MavlinkCommandReceiver(SystemImpl& system_impl);
     ~MavlinkCommandReceiver();
 
-    struct Command {
+    struct CommandInt {
         uint8_t target_system_id{0};
         uint8_t target_component_id{0};
+        MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
         uint16_t command{0};
-
-        // Int
-        MAV_FRAME frame{MAV_FRAME_GLOBAL_RELATIVE_ALT};
         bool current{0};
         bool autocontinue{false};
-
-        // Long
-        uint8_t confirmation{0};
-
-        // Mixed
+        // Most of the "Reserved" values in MAVLink spec are NAN.
         struct Params {
-            float param1{NAN};
-            float param2{NAN};
-            float param3{NAN};
-            float param4{NAN};
-            float param5{NAN};
-            float param6{NAN};
-            float param7{NAN};
-            int32_t x{0};
-            int32_t y{0};
-            float z{NAN};
+            float param1 = NAN;
+            float param2 = NAN;
+            float param3 = NAN;
+            float param4 = NAN;
+            int32_t x = 0;
+            int32_t y = 0;
+            float z = NAN;
         } params{};
 
-        Command(mavlink_command_int_t command_int)
+        CommandInt(const mavlink_command_int_t& command_int)
         {
             target_system_id = command_int.target_system;
             target_component_id = command_int.target_component;
@@ -176,8 +167,24 @@ class MavlinkCommandReceiver {
             params.y = command_int.y;
             params.z = command_int.z;
         }
+    };
+
+    struct CommandLong {
+        uint8_t target_system_id{0};
+        uint8_t target_component_id{0};
+        uint16_t command{0};
+        uint8_t confirmation{0};
+        struct Params {
+            float param1 = NAN;
+            float param2 = NAN;
+            float param3 = NAN;
+            float param4 = NAN;
+            float param5 = NAN;
+            float param6 = NAN;
+            float param7 = NAN;
+        } params{};
 
-        Command(mavlink_command_long_t command_long)
+        CommandLong(const mavlink_command_long_t& command_long)
         {
             target_system_id = command_long.target_system;
             target_component_id = command_long.target_component;
@@ -193,9 +200,13 @@ class MavlinkCommandReceiver {
         }
     };
 
-    typedef std::function<void(const Command&)> mavlink_command_handler_t;
+    typedef std::function<void(const CommandInt&)> mavlink_command_int_handler_t;
+    typedef std::function<void(const CommandLong&)> mavlink_command_long_handler_t;
+
+    void register_mavlink_command_handler(
+        uint16_t cmd_id, mavlink_command_int_handler_t callback, const void* cookie);
     void register_mavlink_command_handler(
-        uint16_t cmd_id, mavlink_command_handler_t callback, const void* cookie);
+        uint16_t cmd_id, mavlink_command_long_handler_t callback, const void* cookie);
 
     void unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie);
     void unregister_all_mavlink_command_handlers(const void* cookie);
@@ -205,14 +216,21 @@ class MavlinkCommandReceiver {
     void receive_command_int(const mavlink_message_t& message);
     void receive_command_long(const mavlink_message_t& message);
 
-    struct MAVLinkCommandHandlerTableEntry {
+    struct MAVLinkCommandIntHandlerTableEntry {
+        uint16_t cmd_id;
+        mavlink_command_int_handler_t callback;
+        const void* cookie; // This is the identification to unregister.
+    };
+
+    struct MAVLinkCommandLongHandlerTableEntry {
         uint16_t cmd_id;
-        mavlink_command_handler_t callback;
+        mavlink_command_long_handler_t callback;
         const void* cookie; // This is the identification to unregister.
     };
 
     std::mutex _mavlink_command_handler_table_mutex{};
-    std::vector<MAVLinkCommandHandlerTableEntry> _mavlink_command_handler_table{};
+    std::vector<MAVLinkCommandIntHandlerTableEntry> _mavlink_command_int_handler_table{};
+    std::vector<MAVLinkCommandLongHandlerTableEntry> _mavlink_command_long_handler_table{};
 };
 
 } // namespace mavsdk
diff --git a/src/core/system_impl.cpp b/src/core/system_impl.cpp
@@ -1234,7 +1234,17 @@ void SystemImpl::intercept_outgoing_messages(std::function<bool(mavlink_message_
 }
 
 void SystemImpl::register_mavlink_command_handler(
-    uint16_t cmd_id, MavlinkCommandReceiver::mavlink_command_handler_t callback, const void* cookie)
+    uint16_t cmd_id,
+    MavlinkCommandReceiver::mavlink_command_int_handler_t callback,
+    const void* cookie)
+{
+    _receive_commands.register_mavlink_command_handler(cmd_id, callback, cookie);
+}
+
+void SystemImpl::register_mavlink_command_handler(
+    uint16_t cmd_id,
+    MavlinkCommandReceiver::mavlink_command_long_handler_t callback,
+    const void* cookie)
 {
     _receive_commands.register_mavlink_command_handler(cmd_id, callback, cookie);
 }

diff --git a/src/core/system_impl.h b/src/core/system_impl.h
@@ -238,7 +238,11 @@ class SystemImpl : public Sender {
 
     void register_mavlink_command_handler(
         uint16_t cmd_id,
-        MavlinkCommandReceiver::mavlink_command_handler_t callback,
+        MavlinkCommandReceiver::mavlink_command_int_handler_t callback,
+        const void* cookie);
+    void register_mavlink_command_handler(
+        uint16_t cmd_id,
+        MavlinkCommandReceiver::mavlink_command_long_handler_t callback,
         const void* cookie);
     void unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie);
     void unregister_all_mavlink_command_handlers(const void* cookie);

diff --git a/tools/rename-enums-to-camelcase.sh b/tools/rename-enums-to-camelcase.sh
@@ -236,14 +236,14 @@ sed -i 's/MAVLinkParameters::Result::CONNECTION_ERROR/MAVLinkParameters::Result:
 sed -i 's/MAVLinkParameters::Result::WRONG_TYPE/MAVLinkParameters::Result::WrongType/g' $files
 sed -i 's/MAVLinkParameters::Result::PARAM_NAME_TOO_LONG/MAVLinkParameters::Result::ParamNameTooLong/g' $files
 
-sed -i 's/MavlinkCommandSender::Result::SUCCESS/MavlinkCommandSender::Result::Success/g' $files
-sed -i 's/MavlinkCommandSender::Result::NO_SYSTEM/MavlinkCommandSender::Result::NoSystem/g' $files
-sed -i 's/MavlinkCommandSender::Result::CONNECTION_ERROR/MavlinkCommandSender::Result::ConnectionError/g' $files
-sed -i 's/MavlinkCommandSender::Result::BUSY/MavlinkCommandSender::Result::Busy/g' $files
-sed -i 's/MavlinkCommandSender::Result::COMMAND_DENIED/MavlinkCommandSender::Result::CommandDenied/g' $files
-sed -i 's/MavlinkCommandSender::Result::TIMEOUT/MavlinkCommandSender::Result::Timeout/g' $files
-sed -i 's/MavlinkCommandSender::Result::IN_PROGRESS/MavlinkCommandSender::Result::InProgress/g' $files
-sed -i 's/MavlinkCommandSender::Result::UNKNOWN_ERROR/MavlinkCommandSender::Result::UnknownError/g' $files
+sed -i 's/MavlinkCommands::Result::SUCCESS/MavlinkCommands::Result::Success/g' $files
+sed -i 's/MavlinkCommands::Result::NO_SYSTEM/MavlinkCommands::Result::NoSystem/g' $files
+sed -i 's/MavlinkCommands::Result::CONNECTION_ERROR/MavlinkCommands::Result::ConnectionError/g' $files
+sed -i 's/MavlinkCommands::Result::BUSY/MavlinkCommands::Result::Busy/g' $files
+sed -i 's/MavlinkCommands::Result::COMMAND_DENIED/MavlinkCommands::Result::CommandDenied/g' $files
+sed -i 's/MavlinkCommands::Result::TIMEOUT/MavlinkCommands::Result::Timeout/g' $files
+sed -i 's/MavlinkCommands::Result::IN_PROGRESS/MavlinkCommands::Result::InProgress/g' $files
+sed -i 's/MavlinkCommands::Result::UNKNOWN_ERROR/MavlinkCommands::Result::UnknownError/g' $files
 
 sed -i 's/Color::RED/Color::Red/g' $files
 sed -i 's/Color::GREEN/Color::Green/g' $files