diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index 51484e81534..184ad774076 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -140,6 +140,8 @@ 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) { @@ -338,6 +340,54 @@ void AP_Scripting::restart_all() _stop = true; } +void AP_Scripting::handle_message(const mavlink_message_t &msg, const mavlink_channel_t chan) { + if (mavlink_data.input == nullptr) { + return; + } + + struct mavlink_msg data {msg, chan}; + + WITH_SEMAPHORE(mavlink_data.sem); + + for (int32_t id : mavlink_data.accept_msg_ids) { + if (id == -1) { + return; + } + if (id == msg.msgid) { + mavlink_data.input->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 b3e86798d30..8a2039f7758 100644 --- a/libraries/AP_Scripting/AP_Scripting.h +++ b/libraries/AP_Scripting/AP_Scripting.h @@ -43,6 +43,10 @@ public: bool enabled(void) const { return _enable != 0; }; bool should_run(void) const { return enabled() && !_stop; } + 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[]; @@ -97,6 +101,27 @@ public: int get_current_ref() { return current_ref; } void set_current_ref(int ref) { current_ref = ref; } + struct mavlink_msg { + mavlink_message_t msg; + 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]; + HAL_Semaphore sem; + } mavlink_data; + private: bool repl_start(void); diff --git a/libraries/AP_Scripting/examples/mavlink.lua b/libraries/AP_Scripting/examples/mavlink.lua new file mode 100644 index 00000000000..d85c1450759 --- /dev/null +++ b/libraries/AP_Scripting/examples/mavlink.lua @@ -0,0 +1,134 @@ +function decode_header(message) + -- build up a map of the result + local result = {} + + local read_marker = 3 + + -- id the MAVLink version + result.protocol_version, read_marker = string.unpack(" #include +extern const AP_HAL::HAL& hal; + extern "C" { #include "lua/src/lmem.h" } @@ -36,6 +38,108 @@ int lua_micros(lua_State *L) { return 1; } +static int lua_mavlink_receive(lua_State *L) { + check_arguments(L, 0, "receive"); + + struct AP_Scripting::mavlink_msg msg; + ObjectBuffer *input = AP::scripting()->mavlink_data.input; + + if (input == nullptr) { + luaL_error(L, "Never subscribed to a message"); + } + + if (input->pop(msg)) { + luaL_Buffer b; + luaL_buffinit(L, &b); + luaL_addlstring(&b, (char *)&msg.msg, sizeof(msg.msg)); + luaL_pushresult(&b); + lua_pushinteger(L, msg.chan); + return 2; + } else { + // no MAVLink to handle, just return no results + return 0; + } +} + +static int lua_mavlink_register_msgid(lua_State *L) { + check_arguments(L, 1, "register_msgid"); + luaL_checkstack(L, 1, "Out of stack"); + + 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) { + lua_pushboolean(L, false); + return 1; + } + } + + int i = 0; + for (i = 0; i < (int)ARRAY_SIZE(data.accept_msg_ids); i++) { + if (data.accept_msg_ids[i] == -1) { + break; + } + } + + if (i >= (int)ARRAY_SIZE(data.accept_msg_ids)) { + return luaL_error(L, "Out of MAVLink ID's to monitor"); + } + + { + WITH_SEMAPHORE(data.sem); + data.accept_msg_ids[i] = msgid; + } + + lua_pushboolean(L, true); + return 1; +} + +static int lua_mavlink_send(lua_State *L) { + check_arguments(L, 3, "send"); + + 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"); + + const int msgid = luaL_checkinteger(L, 2); + luaL_argcheck(L, ((msgid >= 0) && (msgid < (1 << 24))), 2, "msgid out of range"); + + 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"); + } + } + + 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); + } + + return 1; +} int lua_mission_receive(lua_State *L) { binding_argcheck(L, 0); @@ -407,6 +511,25 @@ 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);