From fdb62fd7bb5d5f5de30b069cb51f9f40c13bf9cf Mon Sep 17 00:00:00 2001 From: Stephen Dade Date: Mon, 15 Sep 2025 20:01:01 +1000 Subject: [PATCH 1/2] AP_Scripting: Add bindings for CRC16 --- libraries/AP_Scripting/docs/docs.lua | 10 +++++++ libraries/AP_Scripting/examples/crc.lua | 10 +++++++ .../generator/description/bindings.desc | 3 +++ libraries/AP_Scripting/lua_bindings.cpp | 27 +++++++++++++++++++ libraries/AP_Scripting/lua_bindings.h | 2 ++ 5 files changed, 52 insertions(+) create mode 100644 libraries/AP_Scripting/examples/crc.lua diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 8d09f2e68938b..3120be1d3e0d9 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/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 = {} diff --git a/libraries/AP_Scripting/examples/crc.lua b/libraries/AP_Scripting/examples/crc.lua new file mode 100644 index 0000000000000..ba29be765f490 --- /dev/null +++ b/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)) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 239fb8c1f9fe3..36e989672e0ec 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/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) diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index d7a0fb7a6c324..9104712086334 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/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 diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index 8dfc3e6a0f611..316b9fba64b43 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/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); From e31ce512f7276387deec70fd5e292dddc786dd20 Mon Sep 17 00:00:00 2001 From: Stephen Dade Date: Fri, 27 Jun 2025 15:45:42 +1000 Subject: [PATCH 2/2] AP_Scripting: Add Rockblock 9704 Lua script --- .../AP_Scripting/applets/RockBlock-9704.lua | 694 ++++++++++++++++++ .../AP_Scripting/applets/RockBlock-9704.md | 70 ++ libraries/AP_Scripting/examples/MAVLinkHL.lua | 108 ++- 3 files changed, 850 insertions(+), 22 deletions(-) create mode 100644 libraries/AP_Scripting/applets/RockBlock-9704.lua create mode 100644 libraries/AP_Scripting/applets/RockBlock-9704.md diff --git a/libraries/AP_Scripting/applets/RockBlock-9704.lua b/libraries/AP_Scripting/applets/RockBlock-9704.lua new file mode 100644 index 0000000000000..98d6d1860b1ac --- /dev/null +++ b/libraries/AP_Scripting/applets/RockBlock-9704.lua @@ -0,0 +1,694 @@ +--[[ +Lua script to send a recieve very basic MAVLink telemetry over a +Rockblock 9704 SBD satellite modem +Requires https://github.com/stephendade/rockblock2mav at the GCS end + +Setup: +This script requires 1 serial port, 1 scripting serial port, 1 relay output and 1 GPIO input + +ftp put /home/stephen/Documents/UAVCode/ardupilot/libraries/AP_Scripting/applets/RockBlock-9704.lua APM/Scripts/RockBlock-9704.lua + +Usage: +Use the MAVLink High Latency Control ("link hl on|off" in MAVProxy) to control +whether to send/receive or not (or use "force_hl_enable") + +Written by Stephen Dade (stephen_dade@hotmail.com) + + +]]-- + +-- number of millsec that GCS telemetry has been lost for +local link_lost_for = 0 + +-- Params +local PARAM_TABLE_KEY = 108 +local PARAM_TABLE_PREFIX = "RK9_" + +-- bind a parameter to a variable +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p + end + + -- add a parameter and bind it to a variable + function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) + end + + -- setup RK9 specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 9), 'could not add param table') +--[[ + // @Param: RK9_FORCEHL + // @DisplayName: Force enable High Latency mode + // @Description: Automatically enables High Latency mode if not already enabled + // @Values: 0:Disabled,1:Enabled,2:Enabled on telemetry loss + // @User: Standard +--]] +RK9_FORCEHL = bind_add_param('FORCEHL', 1, 0) + +--[[ + // @Param: RK9_PERIOD + // @DisplayName: Update rate + // @Description: When in High Latency mode, send Rockblock updates every N seconds + // @Range: 0 600 + // @Units: s + // @User: Standard +--]] +RK9_PERIOD = bind_add_param('PERIOD', 2, 30) + +--[[ + // @Param: RK9_DEBUG + // @DisplayName: Display Rockblock debugging text + // @Description: Sends Rockblock debug text to GCS via statustexts + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +RK9_DEBUG = bind_add_param('DEBUG', 3, 0) + +--[[ + // @Param: RK9_ENABLE + // @DisplayName: Enable Message transmission + // @Description: Enables the Rockblock sending and recieving + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +RK9_ENABLE = bind_add_param('ENABLE', 4, 1) + +--[[ + // @Param: RK9_TIMEOUT + // @DisplayName: GCS timeout to start sendin Rockblock messages + // @Description: If RK9_FORCEHL=2, this is the number of seconds of GCS timeout until High Latency mode is auto-enabled + // @Range: 0 600 + // @Units: s + // @User: Standard +--]] +RK9_TIMEOUT = bind_add_param('TIMEOUT', 5, 5) + +--[[ + // @Param: RK9_SERPORT + // @DisplayName: Rockblock Serial Port + // @Description: Serial port number to which the Rockblock is connected.This is the index of the SERIALn_ ports that are set to 28 for "scripting" + // @Range: 0 8 + // @User: Standard +--]] +RK9_SERPORT = bind_add_param('SERPORT', 6, 0) + +--[[ + // @Param: RK9_SCRPORT + // @DisplayName: Rockblock Scripting Serial Port + // @Description: 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" + // @Range: 0 8 + // @User: Standard +--]] +RK9_SCRPORT = bind_add_param('SCRPORT', 7, 0) + +--[[ + // @Param: RK9_RELAY + // @DisplayName: Rockblock Power Relay + // @Description: RELAYn output to control Rockblock power. This connects to I_EN on the Rockblock header. + // @Range: 1 8 + // @User: Standard +--]] +RK9_RELAY = bind_add_param('RELAY', 8, 1) + +--[[ + // @Param: RK9_BOOTED + // @DisplayName: Rockblock booted feedback + // @Description: SERVOn GPIO channel that reads the Rockblock booted state. This connects to I_BTD on the Rockblock header. Requires SERVON_FUNCTION=-1 + // @Range: 50 110 + // @User: Standard +--]] +RK9_BOOTED = bind_add_param('BOOTED', 9, 52) + +local pinboot = RK9_BOOTED:get() +if pinboot then + gpio:pinMode(math.floor(pinboot), 0) -- set AUX 3 (servo11) to input I_BTD SERVO11_FUNCTION=-1 +end + +-- Modem States +local ModemState = { + POWER0 = 0, --not powered + POWER1 = 1, + BOOTED1 = 3, + BOOTED2 = 4, + BOOTED3 = 5, + API_CONFIGURED = 6, + SIM_CONFIGURED = 7, + OPERATIONAL_CONFIGURED = 8, + TOPIC_RECIEVED = 9 +} + +-- The delay between loops +local loop_delay_ms = 200 + +local base64 = 'ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/' +-- Create lookup table for bit patterns to base64 chars +local base64_bit_to_char = {} +for i = 0, 63 do + local c = base64:sub(i + 1, i + 1) + local pattern = '' + local val = i + for j = 5, 0, -1 do + pattern = pattern .. (val >= 2^j and '1' or '0') + if val >= 2^j then val = val - 2^j end + end + base64_bit_to_char[pattern] = c +end + +-- Create inverse lookup table for base64 chars to bit patterns +local base64_char_to_bits = {} +for pattern, char in pairs(base64_bit_to_char) do + base64_char_to_bits[char] = pattern +end + +local function base64_encode(input) + return ((input:gsub('.', function(x) + local r, b_char = '', x:byte() + for i = 8, 1, -1 do r = r .. (b_char % 2 ^ i - b_char % 2 ^ (i - 1) > 0 and '1' or '0') end + return r + end) .. '0000'):gsub('%d%d%d?%d?%d?%d?', function(x) + if (#x < 6) then return '' end + return base64_bit_to_char[x:sub(1,6)] + end) .. ({ '', '==', '=' })[#input % 3 + 1]) +end + +local function base64_decode(input) + -- Strip invalid characters and padding + input = input:gsub('[^'..base64..'=]', '') + input = input:gsub('=', '') + + -- Convert base64 chars to bit sequence + local bits = input:gsub('.', function(x) + return base64_char_to_bits[x] or '' + end) + + -- Convert 8-bit sequences to binary string + local result = "" + for i = 1, #bits, 8 do + local byte = bits:sub(i, i+7) + -- Pad with zeros if needed + byte = byte .. string.rep('0', 8 - #byte) + local value = 0 + for j = 1, 8 do + value = value * 2 + tonumber(byte:sub(j,j)) + end + result = result .. string.char(value) + end + return result +end + +--[[ +Lua Object for managing the RockBlock modem +--]] +local function RockblockModem9704() + -- public fields + local self = { + state = ModemState.POWER0, + raw_topic_id = nil, + message_id = 0, + request_reference = 0, + cur_message = nil, + rxbuffer = "", + txbuffer = nil, + txbufferlen = 0, + bars = 0, + port = nil, + scr_port = nil, + last_status_check = 0, + status_interval = 10000, -- 10 seconds + last_mavlink_send = 0, + scr_string = "" + } + + -- Encode message with CRC + function self.encode_message(message) + local crc = crc_xmodem(message) + local crc_bytes = string.char((crc >> 8) & 0xFF) .. string.char(crc & 0xFF) + local result = message .. crc_bytes + local encoded = base64_encode(result) + return encoded + end + + -- Decode message and verify CRC + function self.decode_message(encoded_message) + local decoded = base64_decode(encoded_message) + local msg_len = #decoded + local crc_received = string.sub(decoded, msg_len-1) + local message_data = string.sub(decoded, 1, msg_len-2) + local crc_calculated = crc_xmodem(message_data) + local crc_calculated_bytes = string.char((crc_calculated >> 8) & 0xFF) .. + string.char(crc_calculated & 0xFF) + -- print out the message data and fields + if RK9_DEBUG:get() == 1 then + gcs:send_text(6, string.format("Rockblock: Received CRC: %02X %02X", crc_received:byte(1), crc_received:byte(2))) + gcs:send_text(6, string.format("Rockblock: Calculated CRC: %02X %02X", crc_calculated_bytes:byte(1), crc_calculated_bytes:byte(2))) + end + return message_data, crc_received, crc_calculated_bytes + end + + + -- Send GET command to the modem + function self.send_get_command(command) + self.cur_message = command + local command_str = string.format("GET %s {}\r", command) + if RK9_DEBUG:get() == 1 then + gcs:send_text(6, "Rockblock: TX: " .. command_str:gsub("\r", "")) + end + if self.port then + self.port:writestring(command_str) + end + end + + -- Send PUT command to the modem + function self.send_put_command(command, options) + local command_str = string.format("PUT %s {%s}\r", command, options) + if RK9_DEBUG:get() == 1 then + gcs:send_text(6, "Rockblock: TX: " .. command_str:gsub("\r", "")) + end + self.cur_message = command + if self.port then + self.port:writestring(command_str) + end + end + + -- Process provisioning message + function self.process_provisioning_message(message, target_name) + -- Find the entire object containing the target_name + local object_str = message:match('{[^}]-"topic_name"%s*:%s*"' .. target_name .. '"[^}]-}') + if not object_str then + return nil + end + + -- Extract topic_id from that object + local topic_id = object_str:match('"topic_id"%s*:%s*(%d+)') + return topic_id and tonumber(topic_id) or nil + end + + -- Process JSON response from the modem + -- This is a simplified JSON parser for ArduPilot Lua + function self.process_json(json_str, target) + local result = {} + + -- Extract key fields using pattern matching + result.active_version_major = tonumber(string.match(json_str, "\"major\":([%d]+)") or "0") + result.constellation_visible = string.match(json_str, "\"constellation_visible\":([%a]+)") == "true" + result.signal_bars = tonumber(string.match(json_str, "\"signal_bars\":([%d]+)") or "0") + result.message_response = string.match(json_str, "\"message_response\":\"([%w_]+)\"") + result.message_id = tonumber(string.match(json_str, "\"message_id\":([%d]+)") or "0") + result.final_mo_status = string.match(json_str, "\"final_mo_status\":\"([%w_]+)\"") + result.data = string.match(json_str, "\"data\":\"([%w+/=]+)\"") + + -- Find RAW topic id + if target == "messageProvisioning" then + local topic_id = self.process_provisioning_message(json_str, "RAW") + if topic_id then + result.raw_topic_id = topic_id + else + gcs:send_text(2, "Rockblock: Failed to find RAW topic ID. Check modem is activated.") + end + end + + return result + end + + -- Process a line received from the modem + function self.process_line(line) + -- Extract response code and target + local response_code = tonumber(string.match(line, "^(%d+)") or "0") + local target = string.match(line, "%d+ ([%w_]+)") + local json_start = line:find("{") + + if not json_start then return end + + local json_str = line:sub(json_start) + local json_response = self.process_json(json_str, target) + + -- Command acknowledgment + if RK9_DEBUG:get() == 1 then + gcs:send_text(6, "Rockblock: Command " .. target .. " acknowledged") + end + if target == self.cur_message then + self.cur_message = nil + end + + -- Handle state transitions + if response_code == 400 then + self.state = ModemState.BOOTED2 + return + end + + if target == "apiVersion" and (response_code == 200 or response_code == 299 or response_code == 402) and + json_response.active_version_major >= 1 then + self.state = ModemState.BOOTED3 + elseif target == "apiVersion" then + self.state = ModemState.BOOTED2 + elseif target == "hwInfo" then + self.state = ModemState.API_CONFIGURED + gcs:send_text(5, "Rockblock: Modem detected") + elseif target == "simConfig" then + self.state = ModemState.SIM_CONFIGURED + elseif target == "operationalState" then + self.state = ModemState.OPERATIONAL_CONFIGURED + gcs:send_text(5, "Rockblock: Modem configured") + elseif target == "constellationState" then + self.bars = json_response.signal_bars or 0 + if json_response.constellation_visible and RK9_DEBUG:get() == 1 then + gcs:send_text(5, "Rockblock: Signal bars: " .. self.bars .. "/5") + end + self.last_status_check = millis() + elseif target == "messageProvisioning" then + if json_response.raw_topic_id then + self.raw_topic_id = json_response.raw_topic_id + if RK9_DEBUG:get() == 1 then + gcs:send_text(5, "Rockblock: RAW topic ID: " .. self.raw_topic_id) + end + self.state = ModemState.TOPIC_RECIEVED + else + gcs:send_text(2, "Rockblock: Failed to find RAW topic ID. Check modem is activated.") + end + elseif target == "messageOriginate" then + if json_response.message_response == "message_accepted" then + self.message_id = json_response.message_id + + -- Send the segment + if self.txbuffer then + self.send_put_command("messageOriginateSegment", + string.format("\"topic_id\":%d, \"message_id\":%d, \"segment_length\":%d, \"segment_start\":0, \"data\":\"%s\"", + self.raw_topic_id, self.message_id, self.txbufferlen, self.txbuffer)) + end + end + elseif target == "messageOriginateStatus" then + if json_response.final_mo_status == "mo_ack_received" and json_response.message_id == self.message_id then + gcs:send_text(5, "Rockblock: Message " .. self.message_id .. " sent successfully") + elseif json_response.message_id == self.message_id then + gcs:send_text(2, "Rockblock: Message " .. self.message_id .. " failed to send" .. + " with status: " .. json_response.final_mo_status) + end + self.txbuffer = nil + elseif target == "messageTerminateSegment" and response_code == 299 then + if json_response.data then + if RK9_DEBUG:get() == 1 then + gcs:send_text(6, "Rockblock: Data received: " .. json_response.data) + end + local message_data, crc_received, crc_calculated = self.decode_message(json_response.data) + + -- Check CRC + local crc_ok = true + for i = 1, #crc_received do + if crc_received:byte(i) ~= crc_calculated:byte(i) then + crc_ok = false + break + end + end + + if not crc_ok then + gcs:send_text(2, "Rockblock: CRC mismatch in received message") + else + self.scr_port:writestring(message_data) + end + end + end + end + + -- Send a message through the modem + function self.send_message(message) + if RK9_ENABLE:get() == 0 then + if RK9_DEBUG:get() == 1 then + gcs:send_text(6, "Rockblock: Rockblock sending disabled by RK9_ENABLE param") + end + return false + end + + if not self.raw_topic_id then + if RK9_DEBUG:get() == 1 then + gcs:send_text(6, "Rockblock: No RAW topic ID available") + end + return false + end + + if self.bars == 0 then + if RK9_DEBUG:get() == 1 then + gcs:send_text(6, "Rockblock: No signal bars available") + end + return false + end + + if self.txbuffer then + if RK9_DEBUG:get() == 1 then + gcs:send_text(6, "Rockblock: Modem already processing a message") + end + return false + end + + -- Send messageOriginate + self.request_reference = self.request_reference + 1 + self.txbuffer = self.encode_message(message) + -- note the buffer len is before the base64 encoding takes place + self.txbufferlen = #message + 2 -- +2 for CRC + self.send_put_command("messageOriginate", + string.format("\"topic_id\":%d, \"message_length\":%d, \"request_reference\":%d", + self.raw_topic_id, self.txbufferlen, self.request_reference)) + + return true + end + + -- Read from the serial port and process data + function self.modem_process() + if not self.port then return end + + -- Read available data from rockblock + local n_bytes = math.min(self.port:available():toint(), 70) + local rxstring = self.port:readstring(n_bytes) + + -- Read available data from scripting serial port + n_bytes = self.scr_port:available():toint() + self.scr_string = self.scr_string .. self.scr_port:readstring(n_bytes) + -- if there's more than 150 bytes in the buffer, just trim to the last 150 bytes + if #self.scr_string > 150 then + self.scr_string = self.scr_string:sub(-150) + end + + -- Send pending messages if we're in HL mode and it's been RK9_PERIOD:get() since last send + if #self.scr_string > 52 then + local now = (millis():tofloat() * 0.001) + if now - self.last_mavlink_send > RK9_PERIOD:get() and gcs:get_high_latency_status() then + if self.state == ModemState.TOPIC_RECIEVED and self.bars > 0 and not self.txbuffer then + --iterate through the string to find 0xFD (MAVLink start of packet) + while #self.scr_string > 10 do + local fd_pos = self.scr_string:find(string.char(0xFD)) + --ensure it's a HL2 packet by looking at the message ID (0xEB) + if fd_pos and #self.scr_string > (fd_pos + 12) and self.scr_string:byte(fd_pos + 7) == 0xEB then + local scr_string_aligned = self.scr_string:sub(fd_pos, fd_pos + 51) + if RK9_DEBUG:get() == 1 then + gcs:send_text(6, "Rockblock: Sending message of len " .. #scr_string_aligned) + end + self.send_message(scr_string_aligned) + self.scr_string = "" + elseif fd_pos then + -- remove up to and including the FD + self.scr_string = self.scr_string:sub(fd_pos + 1) + else + -- no FD found, clear the buffer + self.scr_string = "" + end + end + elseif self.state ~= ModemState.TOPIC_RECIEVED then + gcs:send_text(2, "Rockblock: Cannot send message, modem not configured") + elseif self.bars == 0 then + gcs:send_text(2, "Rockblock: Cannot send message, no signal bars") + end + self.last_mavlink_send = now + end + end + + -- Append to buffer + self.rxbuffer = self.rxbuffer .. rxstring + + -- Process complete lines + while true do + local cr_pos = self.rxbuffer:find("\r") + if not cr_pos then break end + + local line = self.rxbuffer:sub(1, cr_pos - 1) + self.rxbuffer = self.rxbuffer:sub(cr_pos + 1) + + if #line > 0 then + self.process_line(line) + end + end + end + + -- Initialize the modem + function self.initialize_modem() + -- Find and configure the serial port + local port = RK9_SERPORT:get() + if port then + self.port = serial:find_serial(math.floor(port)) + if not self.port then + gcs:send_text(2, "Rockblock: Failed to find serial port") + return false + end + else + gcs:send_text(2, "Rockblock: RK9_SERPORT parameter not set") + return false + end + + --Find and configure scripting serial ports for HL2 (43) messages + port = RK9_SCRPORT:get() + if port then + self.scr_port = serial:find_simulated_device(43, math.floor(port)) + if not self.scr_port then + gcs:send_text(2, 'Rockblock: could not find SCR_SDEV device') + return false + end + else + gcs:send_text(2, "Rockblock: RK9_SCRPORT parameter not set") + return false + end + + -- Configure serial port + self.port:begin(230400) + self.port:set_flow_control(0) + + -- Start modem initialization sequence + self.send_get_command("apiVersion") + + return true + end + + function self.clear_buffers() + -- Clear the RX buffer + self.rxbuffer = "" + + -- Clear the TX buffer + self.txbuffer = nil + self.cur_message = nil + + -- drain the serial port rx buffers + self.port:readstring(self.port:available():toint()) + self.scr_port:readstring(self.scr_port:available():toint()) + + if RK9_DEBUG:get() == 1 then + gcs:send_text(6, "Rockblock: Buffers cleared") + end + end + + -- return the instance + return self +end + +-- Define the RockBlock interface +local rockblock = RockblockModem9704() + +-- Main update function called periodically by ArduPilot +function HLSatcom() + -- boot the modem + if rockblock.state == ModemState.POWER0 then + -- set I_EN to high to power the modem, if not already powered + local pin = RK9_RELAY:get() + if pin then + relay:on(math.floor(pin) - 1) -- turn on I_EN. Note the indexing change from 1 to 0 + end + gcs:send_text(5, "Rockblock: Powering modem") + rockblock.state = ModemState.POWER1 + loop_delay_ms = 1000 -- wait 1 seconds for modem to power up + return + end + + if rockblock.state == ModemState.POWER1 then + local pin = RK9_BOOTED:get() + if pin then + if gpio:read(math.floor(pin)) then + rockblock.state = ModemState.BOOTED1 -- modem is booted + gcs:send_text(5, "Rockblock: Modem booted") + return + end + end + end + + -- enable high latency mode, if desired + if RK9_FORCEHL:get() == 1 and not gcs:get_high_latency_status() then + gcs:enable_high_latency_connections(true) + end + + -- Initialize the port on first run + if not rockblock.port or not rockblock.scr_port then + if not rockblock.initialize_modem() then + -- raise an error to pcall to stop the script + gcs:send_text(2, "Rockblock: Initialization failed, check serial port settings") + loop_delay_ms = 20000 -- try again in 20 seconds + return + end + rockblock.clear_buffers() + end + + --- check if GCS telemetry has been lost for RK9_TIMEOUT sec (if param enabled) + if RK9_FORCEHL:get() == 2 then + -- link lost time = boot time - GCS last seen time + link_lost_for = (millis()- gcs:last_seen()):toint() + -- gcs:last_seen() is set to millis() during boot (on plane). 0 on rover/copter + -- So if it's less than 10000 assume no GCS packet received since boot + if link_lost_for > (RK9_TIMEOUT:get() * 1000) and not gcs:get_high_latency_status() and gcs:last_seen() > 10000 then + gcs:enable_high_latency_connections(true) + elseif link_lost_for < (RK9_TIMEOUT:get() * 1000) and gcs:get_high_latency_status() then + gcs:enable_high_latency_connections(false) + end + end + + -- Read and process data from the serial port + rockblock.modem_process() + + -- Handle state machine + if not rockblock.cur_message then -- Only proceed if no command is pending + if rockblock.state == ModemState.BOOTED2 then + rockblock.send_put_command("apiVersion", "\"active_version\": {\"major\": 1, \"minor\": 6, \"patch\": 1}") + loop_delay_ms = 200 + elseif rockblock.state == ModemState.BOOTED3 then + rockblock.send_get_command("hwInfo") + elseif rockblock.state == ModemState.API_CONFIGURED then + rockblock.send_put_command("simConfig", "\"interface\": \"internal\"") + elseif rockblock.state == ModemState.SIM_CONFIGURED then + rockblock.send_put_command("operationalState", "\"state\": \"active\"") + elseif rockblock.state == ModemState.OPERATIONAL_CONFIGURED then + local now = millis() + if now - rockblock.last_status_check > rockblock.status_interval then + rockblock.last_status_check = now + rockblock.send_get_command("messageProvisioning") + loop_delay_ms = 100 -- once modem is configured, speed up the loop to 100ms + end + elseif rockblock.state == ModemState.TOPIC_RECIEVED and gcs:get_high_latency_status() then + -- Periodic constellation status check + local now = millis() + if now - rockblock.last_status_check > rockblock.status_interval and rockblock.cur_message == nil then + rockblock.last_status_check = now + rockblock.send_get_command("constellationState") + end + end + end + + -- Trying to find modem + if rockblock.state == ModemState.BOOTED1 then + gcs:send_text(5, "Rockblock: Trying to detect modem") + -- Start modem initialization sequence + rockblock.send_get_command("apiVersion") + loop_delay_ms = 5000 -- try again in 5 seconds + end +end + +-- wrapper around HLSatcom(). This calls HLSatcom() and if HLSatcom faults +-- then an error is displayed, but the script is not stopped +function protected_wrapper() + local success, err = pcall(HLSatcom) + if not success then + gcs:send_text(2, "Rockblock: Internal Error: " .. err) + -- when we fault we run the HLSatcom function again after 1s, slowing it + -- down a bit so we don't flood the console with errors + return protected_wrapper, 1000 + end + return protected_wrapper, math.floor(loop_delay_ms) +end + +-- start running HLSatcom loop +return protected_wrapper() \ No newline at end of file diff --git a/libraries/AP_Scripting/applets/RockBlock-9704.md b/libraries/AP_Scripting/applets/RockBlock-9704.md new file mode 100644 index 0000000000000..c4e349cfe51f2 --- /dev/null +++ b/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). + diff --git a/libraries/AP_Scripting/examples/MAVLinkHL.lua b/libraries/AP_Scripting/examples/MAVLinkHL.lua index 5b5d011f510a5..0e3a4cdc14dd4 100644 --- a/libraries/AP_Scripting/examples/MAVLinkHL.lua +++ b/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", " 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(' 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