diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index eb358f4f082..0bd2bc86cc5 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/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 { diff --git a/libraries/AP_Scripting/AP_Scripting.h b/libraries/AP_Scripting/AP_Scripting.h index fc8f5ba47fb..df37eb7bbc8 100644 --- a/libraries/AP_Scripting/AP_Scripting.h +++ b/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); diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index ebe38414c7f..71e9ff2dba5 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/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 diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 65c97aac7d6..3304b672e40 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/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 diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index 5ff19db42e0..f52b4ab085d 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/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); diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index 2b9f6f34c5e..07b5403ec32 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/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);