Add support for Rockblock 9704 satellite modem

libraries/AP_Scripting/applets/RockBlock-9704.md

@@ -0,0 +1,70 @@
+# RockBlock 9704 Lua Script
+
+A Lua script for ArduPilot that enables satellite communication
+using the RockBlock 9704 SBD modem to send and receive basic MAVLink telemetry.
+
+Requires https://github.com/stephendade/rockblock2mav (MQTT client) at the GCS end
+
+Messages will be send or received if High Latency Mode is enabled, or as configured
+by ``RK9_FORCEHL``.
+
+Setup:
+This script requires:
+- A SERIALn port, set to Scripting. Connect to the RXD/TXD lines of the Rockblock's header
+- A SCR_SDEVn (virtual) port, set to MAVLink High Latency. Requires SCR_SDEV_EN=1
+- A relay output connected to the I_EN pin of the Rockblock's header
+- A GPIO input connected to the I_BTD pin of the Rockblock's header
+
+The Rockblock's V_IN- and V_IN+ header pins can be connected to the flight controller's GND and +5V pins respectively.
+
+Caveats:
+- This will *only* send HIGH_LATENCY2 packets via the SBD modem. No heartbeats, no command acknowledgements, no statustexts, no parameters, etc
+- A single HIGH_LATENCY2 packet will be sent every RK9_PERIOD sec
+
+# Parameters
+
+The script has the following parameters:
+
+## RK9_FORCEHL
+
+Mode of operation:
+- 0 = start disabled, can be enabled via MAV_CMD_CONTROL_HIGH_LATENCY (default)
+- 1 = start enabled, can be disabled via MAV_CMD_CONTROL_HIGH_LATENCY
+- 2 = enabled on loss of telemetry (GCS) link for RK9_TIMEOUT seconds
+
+## RK9_PERIOD
+
+When in High Latency mode, send Rockblock updates every RK9_PERIOD seconds. Defaults to 30 seconds.
+
+## RK9_DEBUG
+
+Sends Rockblock debug text to GCS via statustexts. Defaults to 0 (disabled).
+
+## RK9_ENABLE
+
+Enables the modem transmission. Defaults to 1 (enabled).
+
+## RK9_TIMEOUT
+
+If RK9_FORCEHL=2, this is the number of seconds of no-messages from the GCS until High Latency mode is auto-enabled.
+Defaults to 5 seconds.
+
+## RK9_SERPORT
+
+Serial port number to which the Rockblock is connected.
+This is the index of the SERIALn_ ports that are set to 28 for "scripting". Defaults to 0.
+
+## RK9_SCRPORT
+
+Scripting Serial port number to which the Rockblock is connected for HL2 messages.
+This is the index of the SCR_SDEV ports that are set to 2 for "MavlinkHL". Defaults to 0.
+
+## RK9_RELAY
+
+RELAYn output to control Rockblock power. This connects to I_EN on the Rockblock header. Defaults to RELAY1.
+
+## RK9_BOOTED
+
+SERVOn GPIO pin number (usually 50 or greater) that reads the Rockblock booted state.
+This connects to I_BTD on the Rockblock header. Requires SERVOn_FUNCTION=-1. Defaults to 52 (SERVO3).
+

libraries/AP_Scripting/docs/docs.lua

@@ -104,6 +104,16 @@ function mission_receive() end
 ---@param text string|number|integer
 function print(text) end
 
+-- compute CRC-16/XMODEM checksum of text
+---@param text string
+---@return integer -- checksum
+function crc_xmodem(text) end
+
+-- compute CRC-16/MODBUS checksum of text
+---@param text string
+---@return integer -- checksum
+function crc_modbus(text) end
+
 -- data flash logging to SD card
 logger = {}
 

libraries/AP_Scripting/examples/MAVLinkHL.lua

@@ -13,11 +13,13 @@ Use the "hl_mode" variable to control the behaviour of the script:
 Use the MAVLink High Latency Control ("link hl on|off" in MAVProxy) to control
 whether to send or not
 
+Change the use_mavlink2 variable to true to use MAVLink 2 instead of MAVLink 1
+
 Caveats:
 -This will send HIGH_LATENCY2 packets in place of HEARTBEAT packets
 -A single HIGH_LATENCY2 packet will be send every 5 sec
--MAVLink 1 will be used, as it's slightly more efficient (50 vs 52 bytes for a HL2 message)
 -The param SCR_VM_I_COUNT may need to be increased in some circumstances
+-This script does not handle MAVLink 2 signing, so it will not work with GCS that require signed packets
 
 Written by Stephen Dade (stephen_dade@hotmail.com)
 --]]
@@ -40,6 +42,9 @@ local time_last_tx = millis():tofloat() * 0.001
 local hl_mode = 0
 local link_lost_for = 0
 
+-- set to true to use MAVLink 2 instead of MAVLink 1
+local use_mavlink2 = false
+
 -- enable high latency mode from here, instead of having to enable from GCS
 if hl_mode == 0 then
     gcs:enable_high_latency_connections(true)
@@ -74,6 +79,8 @@ local function MAVLinkProcessor()
     local _mavdecodestate = 0 -- 0=looking for marker, 1=getting header,2=getting payload,3=getting crc
     PROTOCOL_MARKER_V1 = 0xFE
     HEADER_LEN_V1 = 6
+    PROTOCOL_MARKER_V2 = 0xFD
+    HEADER_LEN_V2 = 10
     local _txseqid = 0
 
     -- AUTOGEN from MAVLink generator
@@ -85,6 +92,14 @@ local function MAVLinkProcessor()
     _crc_extra[11] = 0x59
     _crc_extra[41] = 0x1c
 
+    local _payload_size = {}
+    _payload_size[75] = 35
+    _payload_size[76] = 33
+    _payload_size[235] = 42
+    _payload_size[73] = 38
+    _payload_size[11] = 6
+    _payload_size[41] = 4
+
     local _messages = {}
     _messages[75] = { -- COMMAND_INT
         {"param1", "<f"}, {"param2", "<f"}, {"param3", "<f"}, {"param4", "<f"},
@@ -142,19 +157,23 @@ local function MAVLinkProcessor()
         _mavbuffer = _mavbuffer .. string.char(byte)
 
         -- check if this is a start of packet
-        if _mavdecodestate == 0 and byte == PROTOCOL_MARKER_V1 then
+        if _mavdecodestate == 0 and ((use_mavlink2 and byte == PROTOCOL_MARKER_V2) or (not use_mavlink2 and byte == PROTOCOL_MARKER_V1)) then
             -- we have a packet start, discard the buffer before this byte
             _mavbuffer = string.char(byte)
             _mavdecodestate = 1
-            return
+            return false
         end
 
         -- if we have a full header, try parsing
-        if #_mavbuffer == HEADER_LEN_V1 and _mavdecodestate == 1 then
+        if _mavdecodestate == 1 and ((not use_mavlink2 and #_mavbuffer == HEADER_LEN_V1) or (use_mavlink2 and #_mavbuffer == HEADER_LEN_V2)) then
             local read_marker = 1
             _, read_marker = string.unpack("<B", _mavbuffer, read_marker)
             _payload_len, read_marker = string.unpack("<B", _mavbuffer,
                                                       read_marker) -- payload is always the second byte
+            if use_mavlink2 then
+                -- ignore the 2 bytes of flags
+                read_marker = read_marker + 2
+            end
             -- fetch seq/sysid/compid
             _mavresult.seq, read_marker =
                 string.unpack("<B", _mavbuffer, read_marker)
@@ -163,24 +182,35 @@ local function MAVLinkProcessor()
             _mavresult.compid, read_marker =
                 string.unpack("<B", _mavbuffer, read_marker)
             -- fetch the message id
-            _mavresult.msgid, _ =
-                string.unpack("<B", _mavbuffer, read_marker)
+            if use_mavlink2 then
+                _mavresult.msgid, _ =
+                    string.unpack("<I3", _mavbuffer, read_marker)
+            else
+                _mavresult.msgid, _ =
+                    string.unpack("<B", _mavbuffer, read_marker)
+            end
 
             _mavdecodestate = 2
-            return
+            return false
         end
 
         -- get payload
-        if _mavdecodestate == 2 and #_mavbuffer ==
+        if _mavdecodestate == 2 and not use_mavlink2 and #_mavbuffer ==
             (_payload_len + HEADER_LEN_V1) then
             _mavdecodestate = 3
             _mavresult.payload = string.sub(_mavbuffer, HEADER_LEN_V1 + 1)
-            return
+            return false
+        elseif _mavdecodestate == 2 and use_mavlink2 and  #_mavbuffer ==
+            (_payload_len + HEADER_LEN_V2) then
+            _mavdecodestate = 3
+            _mavresult.payload = string.sub(_mavbuffer, HEADER_LEN_V2 + 1)
+            return false
         end
 
         -- get crc, then process if CRC ok
-        if _mavdecodestate == 3 and #_mavbuffer ==
-            (_payload_len + HEADER_LEN_V1 + 2) then
+        if _mavdecodestate == 3 and ((not use_mavlink2 and #_mavbuffer ==
+            (_payload_len + HEADER_LEN_V1 + 2)) or (use_mavlink2 and
+            #_mavbuffer == (_payload_len + HEADER_LEN_V2 + 2))) then
             _mavdecodestate = 0
             _mavresult.crc = string.sub(_mavbuffer, -2, -1)
 
@@ -203,7 +233,20 @@ local function MAVLinkProcessor()
                                       self.bytesToString(_mavbuffer, -2, -1) ..
                                       ", " .. self.bytesToString(calccrc, 1, 2))
                     _mavbuffer = ""
-                    return
+                    return true
+                end
+            end
+
+            -- verify payload length before unpacking
+            if #_mavresult.payload ~= _payload_size[_mavresult.msgid] then
+                if use_mavlink2 then
+                    -- re-add the truncated 0 bytes
+                    _mavresult.payload = _mavresult.payload ..
+                        string.rep("\0", _payload_size[_mavresult.msgid] - #_mavresult.payload)
+                else
+                    _mavbuffer = ""
+                    _mavdecodestate = 0
+                    return true
                 end
             end
 
@@ -223,6 +266,7 @@ local function MAVLinkProcessor()
                                                              offset)
                 end
             end
+
             -- only process COMMAND_LONG and COMMAND_INT and  MISSION_ITEM_INT messages
             if _mavresult.msgid == self.MISSION_ITEM_INT then
                 -- goto somewhere (guided mode target)
@@ -299,9 +343,10 @@ local function MAVLinkProcessor()
         end
 
         -- packet too big ... start again
-        if #_mavbuffer > 263 then 
+        if (not use_mavlink2 and #_mavbuffer > 263) or (use_mavlink2 and #_mavbuffer > 280) then 
             _mavbuffer = ""
             _mavdecodestate = 0
+            return true
         end
         return false
     end
@@ -315,7 +360,7 @@ local function MAVLinkProcessor()
     end
 
     function self.createMAVLink(message, msgid)
-        -- generate a mavlink message (V1 only)
+        -- generate a mavlink message
 
         -- create the payload
         local message_map = _messages[msgid]
@@ -345,10 +390,28 @@ local function MAVLinkProcessor()
 
         local payload = string.pack(packString, table.unpack(packedTable))
 
+        --if mavlink2, truncate any trailing 0 bytes
+        if use_mavlink2 then
+            local payload_len = #payload
+            while payload_len > 0 and payload:byte(payload_len) == 0 do
+                payload = string.sub(payload, 1, payload_len - 1)
+                payload_len = payload_len - 1
+            end
+        end
+
         -- create the header. Assume componentid of 1
-        local header = string.pack('<BBBBBB', PROTOCOL_MARKER_V1, #payload,
-                                   _txseqid, param:get('MAV_SYSID'), 1,
-                                   msgid)
+        local header
+        if use_mavlink2 then
+            -- MAVLink 2 header
+            header = string.pack('<BBBBBBBI3', PROTOCOL_MARKER_V2, #payload,
+                                0, 0, _txseqid, param:get('MAV_SYSID'), 1,
+                                msgid)
+        else
+            -- MAVLink 1 header
+            header = string.pack('<BBBBBB', PROTOCOL_MARKER_V1, #payload,
+                                _txseqid, param:get('MAV_SYSID'), 1,
+                                msgid)
+        end
 
         -- generate the CRC
         local crc_extra_msg = _crc_extra[msgid]
@@ -405,11 +468,12 @@ end
 local mavlink = MAVLinkProcessor()
 
 function HLSatcom()
-    -- read in any bytes from GCS and and send to MAVLink processor
-    -- only read in 1 packet at a time to avoid time overruns
-    while port:available() > 0 do
-        local byte = port:read()
-        if mavlink.parseMAVLink(byte) then break end
+    -- read in any bytes from GCS and and send to MAVLink processor, up to max of 100 bytes at a time to prevent timeouts
+    local n_bytes = math.min(port:available():toint(), 100)
+    while n_bytes > 0 do
+        read = port:read()
+        n_bytes = n_bytes - 1
+        mavlink.parseMAVLink(read)
     end
 
     -- if mode 1 and there's been no mavlink traffic for 5000 ms, enable high latency

libraries/AP_Scripting/examples/crc.lua

@@ -0,0 +1,10 @@
+-- Show various CRC calculations
+
+--CRC16 Xmodem
+local inString = "Hello, world!"
+crc = crc_xmodem(inString)
+gcs:send_text(0, "CRC16 Xmodem of '" .. inString .. "' is: " .. string.format("0x%04X", crc))
+
+--CRC16 Modbus
+crc = crc_modbus(inString)
+gcs:send_text(0, "CRC16 Modbus of '" .. inString .. "' is: " .. string.format("0x%04X", crc))

libraries/AP_Scripting/generator/description/bindings.desc

@@ -208,6 +208,9 @@ userdata Quaternion method to_axis_angle void Vector3f
 userdata Quaternion method from_axis_angle  void Vector3f float'skip_check
 userdata Quaternion method from_angular_velocity void Vector3f float'skip_check
 
+global manual crc_xmodem lua_crc_xmodem 1 1
+global manual crc_modbus lua_crc_modbus 1 1
+
 include AP_Notify/AP_Notify.h
 singleton AP_Notify rename notify
 singleton AP_Notify depends (!defined(HAL_BUILD_AP_PERIPH) || AP_PERIPH_NOTIFY_ENABLED)

libraries/AP_Scripting/lua_bindings.cpp

@@ -1296,4 +1296,31 @@ int lua_gps_inject_data(lua_State *L)
 
 #endif  // AP_GPS_ENABLED
 
+int lua_crc_xmodem(lua_State *L) {
+    binding_argcheck(L, 1);
+
+    size_t len = 0;
+    const char *str = luaL_checklstring(L, 1, &len);
+
+    uint16_t crc = 0;
+    for (size_t i=0; i < len; i++) {
+        crc = crc_xmodem_update(crc, (uint8_t)str[i]);
+    }
+
+    lua_pushinteger(L, crc);
+    return 1;
+}
+
+int lua_crc_modbus(lua_State *L) {
+    binding_argcheck(L, 1);
+
+    size_t len = 0;
+    const char *str = luaL_checklstring(L, 1, &len);
+
+    uint16_t crc = calc_crc_modbus((const uint8_t*)str, len);
+
+    lua_pushinteger(L, crc);
+    return 1;
+}
+
 #endif  // AP_SCRIPTING_ENABLED

libraries/AP_Scripting/lua_bindings.h

@@ -36,3 +36,5 @@ int lua_range_finder_handle_script_msg(lua_State *L);
 int lua_GCS_command_int(lua_State *L);
 int lua_DroneCAN_get_FlexDebug(lua_State *L);
 int lua_gps_inject_data(lua_State *L);
+int lua_crc_xmodem(lua_State *L);
+int lua_crc_modbus(lua_State *L);

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -7014,17 +7014,21 @@ void GCS_MAVLINK::initialise_message_intervals_from_config_files()
 
 void GCS_MAVLINK::initialise_message_intervals_from_streamrates()
 {
-    // this is O(n^2), but it's once at boot and across a 10-entry list...
-    for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) {
-        initialise_message_intervals_for_stream(all_stream_entries[i].stream_id);
-    }
 #if HAL_HIGH_LATENCY2_ENABLED
     if (!is_high_latency_link) {
+        // this is O(n^2), but it's once at boot and across a 10-entry list...
+        for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) {
+            initialise_message_intervals_for_stream(all_stream_entries[i].stream_id);
+        }
         set_mavlink_message_id_interval(MAVLINK_MSG_ID_HEARTBEAT, 1000);
     } else {
         set_mavlink_message_id_interval(MAVLINK_MSG_ID_HIGH_LATENCY2, 5000);
     }
 #else
+    // this is O(n^2), but it's once at boot and across a 10-entry list...
+    for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) {
+        initialise_message_intervals_for_stream(all_stream_entries[i].stream_id);
+    }
     set_mavlink_message_id_interval(MAVLINK_MSG_ID_HEARTBEAT, 1000);
 #endif
 }