diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index 93cd4e9e96..525eaf14b7 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -206,6 +206,30 @@ void AP_Scripting::thread(void) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Scripting has stopped"); } +void AP_Scripting::handle_mission_command(const AP_Mission::Mission_Command& cmd_in) +{ + if (!_enable) { + return; + } + + if (mission_data == nullptr) { + // load buffer + mission_data = new ObjectBuffer(mission_cmd_queue_size); + if (mission_data == nullptr) { + gcs().send_text(MAV_SEVERITY_INFO, "scripting: unable to receive mission command"); + return; + } + } + + struct scripting_mission_cmd cmd {cmd_in.p1, + cmd_in.content.scripting.p1, + cmd_in.content.scripting.p2, + cmd_in.content.scripting.p3, + AP_HAL::millis()}; + + mission_data->push(cmd); +} + 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 29cbcd6f35..54ab39e4fb 100644 --- a/libraries/AP_Scripting/AP_Scripting.h +++ b/libraries/AP_Scripting/AP_Scripting.h @@ -46,6 +46,8 @@ public: MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet); + void handle_mission_command(const AP_Mission::Mission_Command& cmd); + // User parameters for inputs into scripts AP_Float _user[4]; @@ -65,6 +67,17 @@ public: uint8_t num_i2c_devices; AP_HAL::OwnPtr *_i2c_dev[SCRIPTING_MAX_NUM_I2C_DEVICE]; + // mission item buffer + static const int mission_cmd_queue_size = 5; + struct scripting_mission_cmd { + uint16_t p1; + float content_p1; + float content_p2; + float content_p3; + uint32_t time_ms; + }; + ObjectBuffer * mission_data; + private: bool repl_start(void); diff --git a/libraries/AP_Scripting/examples/Mission_test.lua b/libraries/AP_Scripting/examples/Mission_test.lua index b918bca414..d72593e357 100644 --- a/libraries/AP_Scripting/examples/Mission_test.lua +++ b/libraries/AP_Scripting/examples/Mission_test.lua @@ -4,6 +4,12 @@ local last_mission_index = mission:get_current_nav_index() function update() -- this is the loop which periodically runs + -- check for scripting DO commands in the mission + local time_ms, param1, param2, param3, param4 = mission_receive() + if time_ms then + gcs:send_text(0, string.format("Scripting CMD @ %u ms, %i, %0.2f, %0.2f, %0.2f", time_ms:tofloat(), param1, param2, param3, param4)) + end + local mission_state = mission:state() -- make sure the mission is running diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index 15a6394b2b..e5bdfcc97a 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -47,10 +47,39 @@ static int lua_micros(lua_State *L) { return 1; } +static int lua_mission_receive(lua_State *L) { + check_arguments(L, 0, "mission_receive"); + + ObjectBuffer *input = AP::scripting()->mission_data; + + if (input == nullptr) { + // no mission items ever received + return 0; + } + + struct AP_Scripting::scripting_mission_cmd cmd; + + if (!input->pop(cmd)) { + // no new item + return 0; + } + + new_uint32_t(L); + *check_uint32_t(L, -1) = cmd.time_ms; + + lua_pushinteger(L, cmd.p1); + lua_pushnumber(L, cmd.content_p1); + lua_pushnumber(L, cmd.content_p2); + lua_pushnumber(L, cmd.content_p3); + + return 5; +} + static const luaL_Reg global_functions[] = { {"millis", lua_millis}, {"micros", lua_micros}, + {"mission_receive", lua_mission_receive}, {NULL, NULL} };