From 076d00fd1e0f57bd4c6f7d402da52c7a03cd3002 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Fri, 10 Mar 2023 09:45:30 +1100 Subject: [PATCH] AP_Scripting: add mavlink support to scripting --- libraries/AP_Scripting/AP_Scripting.cpp | 40 +---- libraries/AP_Scripting/AP_Scripting.h | 14 +- libraries/AP_Scripting/examples/mavlink.lua | 153 ++++-------------- .../generator/description/bindings.desc | 5 + libraries/AP_Scripting/lua_bindings.cpp | 111 +++++++------ libraries/AP_Scripting/lua_bindings.h | 4 + 6 files changed, 100 insertions(+), 227 deletions(-) diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index 184ad77407..eba9469a16 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -140,8 +140,6 @@ AP_Scripting::AP_Scripting() { } #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL _singleton = this; - - memset(mavlink_data.accept_msg_ids, -1, sizeof(mavlink_data.accept_msg_ids)); } void AP_Scripting::init(void) { @@ -341,7 +339,7 @@ void AP_Scripting::restart_all() } void AP_Scripting::handle_message(const mavlink_message_t &msg, const mavlink_channel_t chan) { - if (mavlink_data.input == nullptr) { + if (mavlink_data.rx_buffer == nullptr) { return; } @@ -349,45 +347,17 @@ void AP_Scripting::handle_message(const mavlink_message_t &msg, const mavlink_ch WITH_SEMAPHORE(mavlink_data.sem); - for (int32_t id : mavlink_data.accept_msg_ids) { - if (id == -1) { + for (uint16_t i = 0; i < mavlink_data.accept_msg_ids_size; i++) { + if (mavlink_data.accept_msg_ids[i] == -1) { return; } - if (id == msg.msgid) { - mavlink_data.input->push(data); + if (mavlink_data.accept_msg_ids[i] == msg.msgid) { + mavlink_data.rx_buffer->push(data); return; } } } -void AP_Scripting::send_message(const mavlink_channel_t chan) { - if (mavlink_data.output == nullptr) { - return; - } - - struct mavlink_output data; - if (mavlink_data.output->peek(data) && (data.chan == chan)) { - // FIXME: The data thats in this mavlink_msg_entry_t should be provided from the script, which allows - // sending entirely new messages as outputs. At the moment we can only encode messages that - // are known at compile time. This is fine as a starting point as this is symmetrical to the - // decoding side of the scripting support - const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(data.msgid); - if (entry == nullptr) { - return; - } - if (comm_get_txspace(chan) >= (GCS_MAVLINK::packet_overhead_chan(chan) + entry->max_msg_len)) { - _mav_finalize_message_chan_send(chan, - entry->msgid, - data.data, - entry->min_msg_len, - entry->max_msg_len, - entry->crc_extra); - - UNUSED_RESULT(mavlink_data.output->pop(data)); - } - } -} - 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 8a2039f775..fefc0072ae 100644 --- a/libraries/AP_Scripting/AP_Scripting.h +++ b/libraries/AP_Scripting/AP_Scripting.h @@ -45,8 +45,6 @@ public: void handle_message(const mavlink_message_t &msg, const mavlink_channel_t chan); - void send_message(const mavlink_channel_t chan); - static AP_Scripting * get_singleton(void) { return _singleton; } static const struct AP_Param::GroupInfo var_info[]; @@ -106,19 +104,13 @@ public: mavlink_channel_t chan; }; - struct mavlink_output { - char data[256]; // maximum payload size - int32_t msgid; - mavlink_channel_t chan; - }; - static const int mavlink_input_queue_size = 5; static const int mavlink_output_queue_size = 3; struct mavlink { - ObjectBuffer * input; - ObjectBuffer * output; - int32_t accept_msg_ids[16]; + ObjectBuffer *rx_buffer; + int *accept_msg_ids; + uint16_t accept_msg_ids_size; HAL_Semaphore sem; } mavlink_data; diff --git a/libraries/AP_Scripting/examples/mavlink.lua b/libraries/AP_Scripting/examples/mavlink.lua index d85c145075..0f0b6dafca 100644 --- a/libraries/AP_Scripting/examples/mavlink.lua +++ b/libraries/AP_Scripting/examples/mavlink.lua @@ -1,134 +1,37 @@ -function decode_header(message) - -- build up a map of the result - local result = {} +mavlink_msgs = require("mavlink_msgs") - local read_marker = 3 +msg_map = {} - -- id the MAVLink version - result.protocol_version, read_marker = string.unpack("mavlink_data.sem); + // get the depth of receive queue + const int queue_size = luaL_checkinteger(L, -1); + // get number of msgs to accept + const int num_msgs = luaL_checkinteger(L, -2); + + struct AP_Scripting::mavlink &data = AP::scripting()->mavlink_data; + if (data.rx_buffer == nullptr) { + data.rx_buffer = new ObjectBuffer(queue_size); + if (data.rx_buffer == nullptr) { + return luaL_error(L, "Failed to allocate mavlink rx buffer"); + } + } + if (data.accept_msg_ids == nullptr) { + data.accept_msg_ids = new int[num_msgs]; + if (data.accept_msg_ids == nullptr) { + return luaL_error(L, "Failed to allocate mavlink rx registry"); + } + data.accept_msg_ids_size = num_msgs; + memset(data.accept_msg_ids, -1, sizeof(int) * num_msgs); + } + return 0; +} + +int lua_mavlink_receive(lua_State *L) { + binding_argcheck(L, 0); struct AP_Scripting::mavlink_msg msg; - ObjectBuffer *input = AP::scripting()->mavlink_data.input; + ObjectBuffer *rx_buffer = AP::scripting()->mavlink_data.rx_buffer; - if (input == nullptr) { - luaL_error(L, "Never subscribed to a message"); + if (rx_buffer == nullptr) { + return luaL_error(L, "Never subscribed to a message"); } - if (input->pop(msg)) { + if (rx_buffer->pop(msg)) { luaL_Buffer b; luaL_buffinit(L, &b); luaL_addlstring(&b, (char *)&msg.msg, sizeof(msg.msg)); @@ -61,41 +88,30 @@ static int lua_mavlink_receive(lua_State *L) { } } -static int lua_mavlink_register_msgid(lua_State *L) { - check_arguments(L, 1, "register_msgid"); - luaL_checkstack(L, 1, "Out of stack"); +int lua_mavlink_receive_msgid(lua_State *L) { + binding_argcheck(L, 1); const int msgid = luaL_checkinteger(L, -1); luaL_argcheck(L, ((msgid >= 0) && (msgid < (1 << 24))), 1, "msgid out of range"); struct AP_Scripting::mavlink &data = AP::scripting()->mavlink_data; - if (data.input == nullptr) { - { // WITH_SEMAPHORE cannot be used in the same scope as a luaL_error (or any thing else that jumps away) - WITH_SEMAPHORE(data.sem); - data.input = new ObjectBuffer(AP_Scripting::mavlink_input_queue_size); - } - if (data.input == nullptr) { - return luaL_error(L, "Unable to allocate MAVLink buffer"); - } - } - // check that we aren't currently watching this ID - for (int id : data.accept_msg_ids) { - if (id == msgid) { + for (uint8_t i = 0; i < data.accept_msg_ids_size; i++) { + if (data.accept_msg_ids[i] == msgid) { lua_pushboolean(L, false); return 1; } } int i = 0; - for (i = 0; i < (int)ARRAY_SIZE(data.accept_msg_ids); i++) { + for (i = 0; i < data.accept_msg_ids_size; i++) { if (data.accept_msg_ids[i] == -1) { break; } } - if (i >= (int)ARRAY_SIZE(data.accept_msg_ids)) { + if (i >= data.accept_msg_ids_size) { return luaL_error(L, "Out of MAVLink ID's to monitor"); } @@ -108,8 +124,8 @@ static int lua_mavlink_register_msgid(lua_State *L) { return 1; } -static int lua_mavlink_send(lua_State *L) { - check_arguments(L, 3, "send"); +int lua_mavlink_send(lua_State *L) { + binding_argcheck(L, 3); const mavlink_channel_t chan = (mavlink_channel_t)luaL_checkinteger(L, 1); luaL_argcheck(L, (chan < MAVLINK_COMM_NUM_BUFFERS), 1, "channel out of range"); @@ -119,20 +135,22 @@ static int lua_mavlink_send(lua_State *L) { const char *packet = luaL_checkstring(L, 3); - struct AP_Scripting::mavlink &mavlink_data = AP::scripting()->mavlink_data; - - if (mavlink_data.output == nullptr) { - mavlink_data.output = new ObjectBuffer(AP_Scripting::mavlink_output_queue_size); - if (mavlink_data.output == nullptr) { - return luaL_error(L, "Unable to allocate MAVLink output queue"); - } + // FIXME: The data that's in this mavlink_msg_entry_t should be provided from the script, which allows + // sending entirely new messages as outputs. At the moment we can only encode messages that + // are known at compile time. This is fine as a starting point as this is symmetrical to the + // decoding side of the scripting support + const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid); + if (entry == nullptr) { + return luaL_error(L, "Unknown MAVLink message ID (%d)", msgid); } + if (comm_get_txspace(chan) >= (GCS_MAVLINK::packet_overhead_chan(chan) + entry->max_msg_len)) { + _mav_finalize_message_chan_send(chan, + entry->msgid, + packet, + entry->min_msg_len, + entry->max_msg_len, + entry->crc_extra); - AP_Scripting::mavlink_output data {}; - memcpy(data.data, packet, MIN(sizeof(data.data), lua_rawlen(L, 3))); - data.chan = chan; - data.msgid = msgid; - if (mavlink_data.output->push(data)) { lua_pushboolean(L, true); } else { lua_pushboolean(L, false); @@ -511,25 +529,6 @@ int lua_get_i2c_device(lua_State *L) { return 1; } -static const luaL_Reg mavlink_functions[] = -{ - {"receive", lua_mavlink_receive}, - {"register_msgid", lua_mavlink_register_msgid}, - {"send", lua_mavlink_send}, - {NULL, NULL} -}; - -void load_lua_bindings(lua_State *L) { - lua_pushstring(L, "logger"); - luaL_newlib(L, AP_Logger_functions); - lua_settable(L, -3); - - lua_pushstring(L, "mavlink"); - luaL_newlib(L, mavlink_functions); - lua_settable(L, -3); - - luaL_setfuncs(L, global_functions, 0); -} int AP_HAL__I2CDevice_read_registers(lua_State *L) { const int args = lua_gettop(L); diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index 7e363b8a4a..4c9153af48 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/libraries/AP_Scripting/lua_bindings.h @@ -14,3 +14,7 @@ int lua_dirlist(lua_State *L); int lua_removefile(lua_State *L); int SRV_Channels_get_safety_state(lua_State *L); int lua_get_PWMSource(lua_State *L); +int lua_mavlink_init(lua_State *L); +int lua_mavlink_receive(lua_State *L); +int lua_mavlink_receive_msgid(lua_State *L); +int lua_mavlink_send(lua_State *L);