Skip to content

Commit

Permalink
AP_Scripting: Update rockblock and MAVLinkHL with gcs:run_command and…
Browse files Browse the repository at this point in the history 
… MISSION_SET_CURRENT

AP_Scripting: Add bindings for command_long

GCS_Mavlink: Add lua method for command_long
  • Loading branch information
@stephendade
stephendade committed Jan 30, 2025
1 parent d529601 commit 9a4ea8a
Show file tree
Hide file tree
Showing 8 changed files with 116 additions and 97 deletions.
73 changes: 25 additions & 48 deletions libraries/AP_Scripting/applets/RockBlock.lua
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,8 @@ local function MAVLinkProcessor()
COMMAND_INT = 75,
HIGH_LATENCY2 = 235,
MISSION_ITEM_INT = 73,
SET_MODE = 11
SET_MODE = 11,
MISSION_SET_CURRENT = 41
}

-- private fields
Expand All @@ -121,6 +122,7 @@ local function MAVLinkProcessor()
_crc_extra[235] = 0xb3
_crc_extra[73] = 0x26
_crc_extra[11] = 0x59
_crc_extra[41] = 0x1c

local _messages = {}
_messages[75] = { -- COMMAND_INT
Expand Down Expand Up @@ -156,6 +158,9 @@ local function MAVLinkProcessor()
_messages[11] = { -- SET_MODE
{ "custom_mode", "<I4" }, { "target_system", "<B" }, { "base_mode", "<B" },
}
_messages[41] = { -- MISSION_SET_CURRENT
{ "seq", "<I2" }, { "target_system", "<B" }, { "target_component", "<B" },
}
function self.getSeqID() return _txseqid end

function self.generateCRC(buffer)
Expand Down Expand Up @@ -269,53 +274,25 @@ local function MAVLinkProcessor()
end
elseif _mavresult.msgid == self.SET_MODE then
vehicle:set_mode(_mavresult.custom_mode)
elseif _mavresult.msgid == self.COMMAND_LONG or _mavresult.msgid ==
self.COMMAND_INT then
if _mavresult.command == 400 then -- MAV_CMD_COMPONENT_ARM_DISARM
if _mavresult.param1 == 1 then
arming:arm()
elseif _mavresult.param1 == 0 then
arming:disarm()
end
elseif _mavresult.command == 176 then -- MAV_CMD_DO_SET_MODE
vehicle:set_mode(_mavresult.param2)
elseif _mavresult.command == 20 then -- MAV_CMD_NAV_RETURN_TO_LAUNCH (Mode RTL) may vary depending on frame
if FWVersion:type() == 2 then -- copter
vehicle:set_mode(6)
elseif FWVersion:type() == 3 then -- plane
vehicle:set_mode(11)
elseif FWVersion:type() == 1 then -- rover
vehicle:set_mode(11)
end
elseif _mavresult.command == 21 then -- MAV_CMD_NAV_LAND (Mode LAND) may vary depending on frame
if FWVersion:type() == 2 then -- copter
vehicle:set_mode(9)
elseif FWVersion:type() == 12 then -- blimp
vehicle:set_mode(0)
end
elseif _mavresult.command == 22 then -- MAV_CMD_NAV_TAKEOFF
vehicle:start_takeoff(_mavresult.param7)
elseif _mavresult.command == 84 then -- MAV_CMD_NAV_VTOL_TAKEOFF
vehicle:start_takeoff(_mavresult.param7)
elseif _mavresult.command == 85 then -- MAV_CMD_NAV_VTOL_LAND (Mode QLAND)
vehicle:set_mode(20)
elseif _mavresult.command == 300 then -- MAV_CMD_MISSION_START --mode auto and then start mission
if FWVersion:type() == 2 then -- copter
vehicle:set_mode(3)
elseif FWVersion:type() == 3 then -- plane
vehicle:set_mode(10)
elseif FWVersion:type() == 1 then -- rover
vehicle:set_mode(10)
elseif FWVersion:type() == 7 then -- sub
vehicle:set_mode(3)
end
elseif _mavresult.command == 2600 then -- MAV_CMD_CONTROL_HIGH_LATENCY
if _mavresult.param1 == 1 then
gcs:enable_high_latency_connections(true)
else
gcs:enable_high_latency_connections(false)
end
end
elseif _mavresult.msgid == self.COMMAND_LONG then
gcs:run_command_long(_mavresult.command, { p1 = _mavresult.param1,
p2 = _mavresult.param2,
p3 = _mavresult.param3,
p4 = _mavresult.param4,
p5 = _mavresult.param5,
p6 = _mavresult.param6,
p7 = _mavresult.param7})
elseif _mavresult.msgid == self.COMMAND_INT then
gcs:run_command_int(_mavresult.command, { p1 = _mavresult.param1,
p2 = _mavresult.param2,
p3 = _mavresult.param3,
p4 = _mavresult.param4,
x = _mavresult.x,
y = _mavresult.y,
z = _mavresult.z,
frame = _mavresult.frame })
elseif _mavresult.msgid == self.MISSION_SET_CURRENT then
mission:set_current_cmd(_mavresult.seq)
end
_mavbuffer = ""
return true
Expand Down
6 changes: 6 additions & 0 deletions libraries/AP_Scripting/docs/docs.lua
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2981,6 +2981,12 @@ function gcs:last_seen() end
---@return integer -- MAV_RESULT
function gcs:run_command_int(command, params) end

-- call a MAVLink MAV_CMD_xxx command via command_long interface
---@param command integer -- MAV_CMD_xxx
---@param params table -- parameters of p1, p2, p3, p4, p5, p6 and p7. Any not specified taken as zero
---@return integer -- MAV_RESULT
function gcs:run_command_long(command, params) end

-- The relay library provides access to controlling relay outputs.
relay = {}

Expand Down
73 changes: 25 additions & 48 deletions libraries/AP_Scripting/examples/MAVLinkHL.lua
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,8 @@ local function MAVLinkProcessor()
COMMAND_INT = 75,
HIGH_LATENCY2 = 235,
MISSION_ITEM_INT = 73,
SET_MODE = 11
SET_MODE = 11,
MISSION_SET_CURRENT = 41
}

-- private fields
Expand All @@ -67,6 +68,7 @@ local function MAVLinkProcessor()
_crc_extra[235] = 0xb3
_crc_extra[73] = 0x26
_crc_extra[11] = 0x59
_crc_extra[41] = 0x1c

local _messages = {}
_messages[75] = { -- COMMAND_INT
Expand Down Expand Up @@ -102,6 +104,9 @@ local function MAVLinkProcessor()
_messages[11] = { -- SET_MODE
{ "custom_mode", "<I4" }, { "target_system", "<B" }, { "base_mode", "<B" },
}
_messages[41] = { -- MISSION_SET_CURRENT
{ "seq", "<I2" }, { "target_system", "<B" }, { "target_component", "<B" },
}
function self.getSeqID() return _txseqid end

function self.generateCRC(buffer)
Expand Down Expand Up @@ -216,53 +221,25 @@ local function MAVLinkProcessor()
end
elseif _mavresult.msgid == self.SET_MODE then
vehicle:set_mode(_mavresult.custom_mode)
elseif _mavresult.msgid == self.COMMAND_LONG or _mavresult.msgid ==
self.COMMAND_INT then
if _mavresult.command == 400 then -- MAV_CMD_COMPONENT_ARM_DISARM
if _mavresult.param1 == 1 then
arming:arm()
elseif _mavresult.param1 == 0 then
arming:disarm()
end
elseif _mavresult.command == 176 then -- MAV_CMD_DO_SET_MODE
vehicle:set_mode(_mavresult.param2)
elseif _mavresult.command == 20 then -- MAV_CMD_NAV_RETURN_TO_LAUNCH (Mode RTL) may vary depending on frame
if FWVersion:type() == 2 then -- copter
vehicle:set_mode(6)
elseif FWVersion:type() == 3 then -- plane
vehicle:set_mode(11)
elseif FWVersion:type() == 1 then -- rover
vehicle:set_mode(11)
end
elseif _mavresult.command == 21 then -- MAV_CMD_NAV_LAND (Mode LAND) may vary depending on frame
if FWVersion:type() == 2 then -- copter
vehicle:set_mode(9)
elseif FWVersion:type() == 12 then -- blimp
vehicle:set_mode(0)
end
elseif _mavresult.command == 22 then -- MAV_CMD_NAV_TAKEOFF
vehicle:start_takeoff(_mavresult.param7)
elseif _mavresult.command == 84 then -- MAV_CMD_NAV_VTOL_TAKEOFF
vehicle:start_takeoff(_mavresult.param7)
elseif _mavresult.command == 85 then -- MAV_CMD_NAV_VTOL_LAND (Mode QLAND)
vehicle:set_mode(20)
elseif _mavresult.command == 300 then -- MAV_CMD_MISSION_START --mode auto and then start mission
if FWVersion:type() == 2 then -- copter
vehicle:set_mode(3)
elseif FWVersion:type() == 3 then -- plane
vehicle:set_mode(10)
elseif FWVersion:type() == 1 then -- rover
vehicle:set_mode(10)
elseif FWVersion:type() == 7 then -- sub
vehicle:set_mode(3)
end
elseif _mavresult.command == 2600 then -- MAV_CMD_CONTROL_HIGH_LATENCY
if _mavresult.param1 == 1 then
gcs:enable_high_latency_connections(true)
else
gcs:enable_high_latency_connections(false)
end
end
elseif _mavresult.msgid == self.COMMAND_LONG then
gcs:run_command_long(_mavresult.command, { p1 = _mavresult.param1,
p2 = _mavresult.param2,
p3 = _mavresult.param3,
p4 = _mavresult.param4,
p5 = _mavresult.param5,
p6 = _mavresult.param6,
p7 = _mavresult.param7})
elseif _mavresult.msgid == self.COMMAND_INT then
gcs:run_command_int(_mavresult.command, { p1 = _mavresult.param1,
p2 = _mavresult.param2,
p3 = _mavresult.param3,
p4 = _mavresult.param4,
x = _mavresult.x,
y = _mavresult.y,
z = _mavresult.z,
frame = _mavresult.frame })
elseif _mavresult.msgid == self.MISSION_SET_CURRENT then
mission:set_current_cmd(_mavresult.seq)
end
_mavbuffer = ""
return true
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_Scripting/generator/description/bindings.desc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -308,6 +308,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
singleton GCS manual run_command_long lua_GCS_command_long 2 1

include AP_ONVIF/AP_ONVIF.h depends ENABLE_ONVIF == 1
singleton AP_ONVIF depends ENABLE_ONVIF == 1
Expand Down
42 changes: 42 additions & 0 deletions libraries/AP_Scripting/lua_bindings.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1235,6 +1235,48 @@ int lua_GCS_command_int(lua_State *L)

return 1;
}

/*
implement gcs:command_long() access to MAV_CMD_xxx commands
*/
int lua_GCS_command_long(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_long_t pkt {};

pkt.command = command;

float *params = &pkt.param1;

// extract the 7 parameters as floats
for (uint8_t i=0; i<7; 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);
}

// call the interface with scheduler lock
WITH_SEMAPHORE(AP::scheduler().get_semaphore());

auto result = _gcs->lua_command_long_packet(pkt);

// Return the resulting MAV_RESULT
lua_pushinteger(L, result);

return 1;
}
#endif

#if HAL_ENABLE_DRONECAN_DRIVERS
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_Scripting/lua_bindings.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,4 +34,5 @@ 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);
int lua_GCS_command_long(lua_State *L);
int lua_DroneCAN_get_FlexDebug(lua_State *L);
14 changes: 14 additions & 0 deletions libraries/GCS_MAVLink/GCS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -425,6 +425,20 @@ MAV_RESULT GCS::lua_command_int_packet(const mavlink_command_int_t &packet)

return ch->handle_command_int_packet(packet, msg);
}

MAV_RESULT GCS::lua_command_long_packet(const mavlink_command_long_t &packet)
{
// for now we assume channel 0. In the future we may create a dedicated channel
auto *ch = chan(0);
if (ch == nullptr) {
return MAV_RESULT_UNSUPPORTED;
}
// we need a dummy message for some calls
mavlink_message_t msg {};

return ch->try_command_long_as_command_int(packet, msg);
}

#endif // AP_SCRIPTING_ENABLED

#endif // HAL_GCS_ENABLED
3 changes: 2 additions & 1 deletion libraries/GCS_MAVLink/GCS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1294,8 +1294,9 @@ class GCS
virtual uint8_t sysid_this_mav() const = 0;

#if AP_SCRIPTING_ENABLED
// lua access to command_int
// lua access to command_int and command_long
MAV_RESULT lua_command_int_packet(const mavlink_command_int_t &packet);
MAV_RESULT lua_command_long_packet(const mavlink_command_long_t &packet);
#endif

// Sequence number should be incremented when available modes changes
Expand Down

0 comments on commit 9a4ea8a

Please sign in to comment.