Skip to content

Commit

Permalink
AP_Scripting: Update rockblock and MAVLinkHL with additional commands…
Browse files Browse the repository at this point in the history 
… and gcs timeout
  • Loading branch information
@stephendade
stephendade committed Feb 6, 2025
1 parent 8c7afb3 commit 1b0c89c
Show file tree
Hide file tree
Showing 3 changed files with 194 additions and 106 deletions.
145 changes: 92 additions & 53 deletions libraries/AP_Scripting/applets/RockBlock.lua
View file
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
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

0 comments on commit 1b0c89c

Please sign in to comment.