AP_Scripting: add new mavlink binding to block commands

libraries/AP_Scripting/AP_Scripting.cpp

@@ -259,6 +259,16 @@ void AP_Scripting::thread(void) {
         }
         num_pwm_source = 0;
 
+        // Clear blocked commands
+        {
+            WITH_SEMAPHORE(mavlink_command_block_list_sem);
+            while (mavlink_command_block_list != nullptr) {
+                command_block_list *next_item = mavlink_command_block_list->next;
+                delete mavlink_command_block_list;
+                mavlink_command_block_list = next_item;
+            }
+        }
+
         bool cleared = false;
         while(true) {
             // 1hz check if we should restart
@@ -361,6 +371,22 @@ void AP_Scripting::handle_message(const mavlink_message_t &msg, const mavlink_ch
     }
 }
 
+bool AP_Scripting::is_handling_command(uint16_t cmd_id)
+{
+    WITH_SEMAPHORE(mavlink_command_block_list_sem);
+
+    // Look in linked list to see if id is registered
+    if (mavlink_command_block_list != nullptr) {
+        for (command_block_list *item = mavlink_command_block_list; item; item = item->next) {
+            if (item->id == cmd_id) {
+                return true;
+            }
+        }
+    }
+
+    return false;
+}
+
 AP_Scripting *AP_Scripting::_singleton = nullptr;
 
 namespace AP {

libraries/AP_Scripting/AP_Scripting.h

@@ -45,6 +45,9 @@ public:
 
     void handle_message(const mavlink_message_t &msg, const mavlink_channel_t chan);
 
+    // Check if command ID is blocked
+    bool is_handling_command(uint16_t cmd_id);
+
     static AP_Scripting * get_singleton(void) { return _singleton; }
 
     static const struct AP_Param::GroupInfo var_info[];
@@ -112,6 +115,13 @@ public:
         HAL_Semaphore sem;
     } mavlink_data;
 
+    struct command_block_list {
+        uint16_t id;
+        command_block_list *next;
+    };
+    command_block_list *mavlink_command_block_list;
+    HAL_Semaphore mavlink_command_block_list_sem;
+
 private:
 
     bool repl_start(void);

libraries/AP_Scripting/docs/docs.lua

@@ -2894,3 +2894,7 @@ function mavlink:receive_chan() end
 ---@param msgid integer
 ---@param message string
 function mavlink:send_chan(chan, msgid, message) end
+
+-- Block a given MAV_CMD from being procceced by ArduPilot
+---@param comand_id integer
+function mavlink:block_command(comand_id) end

libraries/AP_Scripting/generator/description/bindings.desc

@@ -730,3 +730,4 @@ singleton mavlink manual init lua_mavlink_init 2
 singleton mavlink manual register_rx_msgid lua_mavlink_register_rx_msgid 1
 singleton mavlink manual send_chan lua_mavlink_send_chan 3
 singleton mavlink manual receive_chan lua_mavlink_receive_chan 0
+singleton mavlink manual block_command lua_mavlink_block_command 1

libraries/AP_Scripting/lua_bindings.cpp

@@ -180,6 +180,39 @@ int lua_mavlink_send_chan(lua_State *L) {
     return 1;
 }
 
+int lua_mavlink_block_command(lua_State *L) {
+
+    // Allow : and . access
+    const int arg_offset = (luaL_testudata(L, 1, "mavlnk") != NULL) ? 1 : 0;
+
+    binding_argcheck(L, 1+arg_offset);
+
+    const uint16_t id = get_uint16_t(L, 1+arg_offset);
+
+    // Check if ID is already registered
+    if (AP::scripting()->is_handling_command(id)) {
+        lua_pushboolean(L, true);
+        return 1;
+    }
+
+    // Add new list item
+    AP_Scripting::command_block_list *new_item = new AP_Scripting::command_block_list;
+    if (new_item == nullptr) {
+        lua_pushboolean(L, false);
+        return 1;
+    }
+    new_item->id = id;
+
+    {
+        WITH_SEMAPHORE(AP::scripting()->mavlink_command_block_list_sem);
+        new_item->next = AP::scripting()->mavlink_command_block_list;
+        AP::scripting()->mavlink_command_block_list = new_item;
+    }
+
+    lua_pushboolean(L, true);
+    return 1;
+}
+
 int lua_mission_receive(lua_State *L) {
     binding_argcheck(L, 0);
 

libraries/AP_Scripting/lua_bindings.h

@@ -18,3 +18,4 @@ int lua_mavlink_init(lua_State *L);
 int lua_mavlink_receive_chan(lua_State *L);
 int lua_mavlink_register_rx_msgid(lua_State *L);
 int lua_mavlink_send_chan(lua_State *L);
+int lua_mavlink_block_command(lua_State *L);