Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Scripting: Update Rockblock and MAVLinkHL scripts #29192

Merged
merged 1 commit into from
Feb 7, 2025
Merged

Scripting: Update Rockblock and MAVLinkHL scripts #29192

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
145 changes: 92 additions & 53 deletions libraries/AP_Scripting/applets/RockBlock.lua
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ Usage:
Use the MAVLink High Latency Control ("link hl on|off" in MAVProxy) to control
whether to send or not (or use "force_hl_enable")
Use the RCK_DEBUG param to view debugging statustexts at the GCS
Use the RCK_FORCEHL param to force-enable high latency mode, instead of enabling from GCS
Use the RCK_FORCEHL param to control the mode of operation: 0=Disabled, 1=Enabled, 2=Enabled on 5000ms telemetry loss

Caveats:
This will *only* send HIGH_LATENCY2 packets via the SBD modem. No heartbeats,
Expand All @@ -24,8 +24,6 @@ The param SCR_VM_I_COUNT may need to be increased in some circumstances
Written by Stephen Dade ([email protected])
]]--

---@diagnostic disable: cast-local-type

local PARAM_TABLE_KEY = 10
local PARAM_TABLE_PREFIX = "RCK_"

Expand All @@ -39,6 +37,9 @@ end
port:begin(19200)
port:set_flow_control(0)

-- number of millsec that GCS telemetry has been lost for
local link_lost_for = 0

-- bind a parameter to a variable
function bind_param(name)
local p = Parameter()
Expand All @@ -58,7 +59,7 @@ assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 4), 'could not add p
// @Param: RCK_FORCEHL
// @DisplayName: Force enable High Latency mode
// @Description: Automatically enables High Latency mode if not already enabled
// @Values: 0:Disabled,1:Enabled
// @Values: 0:Disabled,1:Enabled,2:Enabled on 5000ms telemetry loss
// @User: Standard
--]]
RCK_FORCEHL = bind_add_param('FORCEHL', 1, 0)
Expand Down Expand Up @@ -91,6 +92,13 @@ RCK_DEBUG = bind_add_param('DEBUG', 3, 0)
--]]
RCK_ENABLE = bind_add_param('ENABLE', 4, 1)

--[[
Returns true if the value is NaN, false otherwise
--]]
local function isNaN (x)
return (x ~= x)
end

--[[
Lua Object for decoding and encoding MAVLink (V1 only) messages
--]]
Expand All @@ -102,7 +110,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 +130,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 +166,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 @@ -190,8 +203,12 @@ local function MAVLinkProcessor()
_payload_len, read_marker = string.unpack("<B", _mavbuffer,
read_marker) -- payload is always the second byte
-- fetch seq/sysid/compid
_mavresult.seq, _mavresult.sysid, _mavresult.compid, read_marker =
string.unpack("<BBB", _mavbuffer, read_marker)
_mavresult.seq, read_marker =
string.unpack("<B", _mavbuffer, read_marker)
_mavresult.sysid, read_marker =
string.unpack("<B", _mavbuffer, read_marker)
_mavresult.compid, read_marker =
string.unpack("<B", _mavbuffer, read_marker)
-- fetch the message id
_mavresult.msgid, _ = string.unpack("<B", _mavbuffer, read_marker)

Expand Down Expand Up @@ -269,53 +286,59 @@ 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
elseif _mavresult.msgid == self.COMMAND_LONG then
--- need to do a little conversion here. Taken from convert_COMMAND_LONG_to_COMMAND_INT()
local command_long_stores_location = 0
if (_mavresult.command == 179 or -- MAV_CMD_DO_SET_HOME
_mavresult.command == 201 or -- MAV_CMD_DO_SET_ROI
_mavresult.command == 195 or -- MAV_CMD_DO_SET_ROI_LOCATION
_mavresult.command == 192 or -- MAV_CMD_DO_REPOSITION
_mavresult.command == 43003) then -- MAV_CMD_EXTERNAL_POSITION_ESTIMATE
command_long_stores_location = 1
end
local int_frame_conv = 0 -- MAV_FRAME_GLOBAL
if (_mavresult.command == 195) then -- MAV_CMD_DO_SET_ROI_LOCATION
int_frame_conv = 3 -- MAV_FRAME_GLOBAL_RELATIVE_ALT
elseif (_mavresult.command == 179) then -- MAV_CMD_DO_SET_HOME
int_frame_conv = 0 -- MAV_FRAME_GLOBAL
elseif (_mavresult.command == 201) then -- MAV_CMD_DO_SET_ROI
int_frame_conv = 3 -- MAV_FRAME_GLOBAL_RELATIVE_ALT
elseif (_mavresult.command == 42006) then -- MAV_CMD_FIXED_MAG_CAL_YAW
int_frame_conv = 3 -- MAV_FRAME_GLOBAL_RELATIVE_ALT
end
local int_x = _mavresult.param5
local int_y = _mavresult.param6
if isNaN(int_x) then
int_x = 0
end
if isNaN(int_y) then
int_y = 0
end
if command_long_stores_location == 1 then
int_x = 1E7 * int_x
int_y = 1E7 * int_y
end
gcs:run_command_int(_mavresult.command, { p1 = _mavresult.param1,
p2 = _mavresult.param2,
p3 = _mavresult.param3,
p4 = _mavresult.param4,
x = int_x,
y = int_y,
z = _mavresult.param7,
int_frame = int_frame_conv,
current = 0,
autocontinue = 0 })
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 Expand Up @@ -719,6 +742,22 @@ function HLSatcom()
gcs:send_text(3, "Rockblock: Trying to detect modem")
rockblock.checkmodem()
end

--- check if GCS telemetry has been lost for 5000 millisec (if param enabled)
if RCK_FORCEHL:get() == 2 then
if last_seen == gcs:last_seen() then
link_lost_for = link_lost_for + 100
else
-- There has been a new heartbeat, update last_seen and reset link_lost_for
last_seen = gcs:last_seen()
link_lost_for = 0
end
if link_lost_for > 5000 and not gcs:get_high_latency_status() then
gcs:enable_high_latency_connections(true)
elseif link_lost_for < 5000 and gcs:get_high_latency_status() then
gcs:enable_high_latency_connections(false)
end
end

-- send HL2 packet every 30 sec, if not aleady in a mailbox check
if rockblock.modem_detected and gcs:get_high_latency_status() and
Expand Down
5 changes: 4 additions & 1 deletion libraries/AP_Scripting/applets/RockBlock.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,10 @@ The script adds the following parameters:

## RCK_FORCEHL

Automatically enables High Latency mode if not already enabled
Mode of operation:
- 0 = start disabled, can be enabled via MAV_CMD_CONTROL_HIGH_LATENCY
- 1 = start enabled, can be disabled via MAV_CMD_CONTROL_HIGH_LATENCY
- 2 = enabled on loss of telemetry (GCS) link for 5 seconds

## RCK_PERIOD

Expand Down
Loading
Loading