diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index b1c73afce10e4..afd113d5f4c01 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -2605,6 +2605,12 @@ function gcs:send_text(severity, text) end ---@return uint32_t_ud -- system time in milliseconds function gcs:last_seen() end +-- call a MAVLink MAV_CMD_xxx command via command_int interface +---@param command integer -- MAV_CMD_xxx +---@param params table -- parameters of p1, p2, p3, p4, x, y and z and frame. Any not specified taken as zero +---@return boolean +function gcs:run_command_int(command, params) end + -- The relay library provides access to controlling relay outputs. relay = {} diff --git a/libraries/AP_Scripting/examples/command_int.lua b/libraries/AP_Scripting/examples/command_int.lua new file mode 100644 index 0000000000000..b86792c4569ed --- /dev/null +++ b/libraries/AP_Scripting/examples/command_int.lua @@ -0,0 +1,56 @@ +--[[ + demonstrate using the gcs:command_int() interface to send commands from MAVLink MAV_CMD_xxx set +--]] + +local MAV_FRAME_GLOBAL_RELATIVE_ALT = 3 + +local MAV_CMD_DO_SET_MODE = 176 +local MAV_CMD_DO_REPOSITION = 192 + +-- some plane flight modes for testing +local MODE_LOITER = 12 +local MODE_GUIDED = 15 + +local MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 + +--[[ +create a NaN value +--]] +local function NaN() + return 0/0 +end + +--[[ + test API calls. When in LOITER mode change to GUIDED and force flying to a location NE of home +--]] +local function test_command_int() + if vehicle:get_mode() ~= MODE_LOITER then + return + end + local home = ahrs:get_home() + if not home then + return + end + + -- force mode GUIDED + gcs:run_command_int(MAV_CMD_DO_SET_MODE, { p1 = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, p2 = MODE_GUIDED }) + + -- and fly to 200m NE of home and 100m above home + local dest = home:copy() + dest:offset_bearing(45, 200) + + gcs:run_command_int(MAV_CMD_DO_REPOSITION, { frame = MAV_FRAME_GLOBAL_RELATIVE_ALT, + p1 = -1, + p4 = NaN(), + x = dest:lat(), + y = dest:lng(), + z = 100 }) +end + +local function update() + test_command_int() + return update, 1000 +end + +return update, 1000 + diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 1c2deb44d80a8..cc5ecec9cbd7c 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -296,6 +296,7 @@ singleton GCS method get_high_latency_status depends HAL_HIGH_LATENCY2_ENABLED = singleton GCS method enable_high_latency_connections void boolean singleton GCS method enable_high_latency_connections depends HAL_HIGH_LATENCY2_ENABLED == 1 +singleton GCS manual run_command_int lua_GCS_command_int 2 1 include AP_ONVIF/AP_ONVIF.h depends ENABLE_ONVIF == 1 singleton AP_ONVIF depends ENABLE_ONVIF == 1 diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index 7b817821cda17..7199e14e1c6a0 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -1112,4 +1112,78 @@ void lua_abort() #endif } +#if HAL_GCS_ENABLED +/* + implement gcs:command_int() access to MAV_CMD_xxx commands + */ +int lua_GCS_command_int(lua_State *L) +{ + GCS *_gcs = check_GCS(L); + binding_argcheck(L, 3); + + const uint16_t command = get_uint16_t(L, 2); + if (!lua_istable(L, 3)) { + // must have parameter table + return 0; + } + + mavlink_command_int_t pkt {}; + + pkt.command = command; + + float *params = &pkt.param1; + int32_t *xy = &pkt.x; + + // extract the first 4 parameters as floats + for (uint8_t i=0; i<4; i++) { + char pname[3] { 'p' , char('1' + i), 0 }; + lua_pushstring(L, pname); + lua_gettable(L, 3); + if (lua_isnumber(L, -1)) { + params[i] = lua_tonumber(L, -1); + } + lua_pop(L, 1); + } + + // extract the xy values + for (uint8_t i=0; i<2; i++) { + const char *names[] = { "x", "y" }; + lua_pushstring(L, names[i]); + lua_gettable(L, 3); + if (lua_isinteger(L, -1)) { + xy[i] = lua_tointeger(L, -1); + } + lua_pop(L, 1); + } + + // and z + lua_pushstring(L, "z"); + lua_gettable(L, 3); + if (lua_isnumber(L, -1)) { + pkt.z = lua_tonumber(L, -1); + } + lua_pop(L, 1); + + // optional frame + lua_pushstring(L, "frame"); + lua_gettable(L, 3); + if (lua_isinteger(L, -1)) { + pkt.frame = lua_tointeger(L, -1); + } + lua_pop(L, 1); + + // call the interface with scheduler lock + WITH_SEMAPHORE(AP::scheduler().get_semaphore()); + + auto result = _gcs->lua_command_int_packet(pkt); + + // map MAV_RESULT to a boolean + bool ok = result == MAV_RESULT_ACCEPTED; + + lua_pushboolean(L, ok); + + return 1; +} +#endif + #endif // AP_SCRIPTING_ENABLED diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index 53b5f4b5ae656..cf193715c695f 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/libraries/AP_Scripting/lua_bindings.h @@ -30,3 +30,4 @@ int lua_mavlink_send_chan(lua_State *L); int lua_mavlink_block_command(lua_State *L); int lua_print(lua_State *L); int lua_range_finder_handle_script_msg(lua_State *L); +int lua_GCS_command_int(lua_State *L);