--[[ Lua script to send a recieve very basic MAVLink telemetry over a Rockblock SBD satellite modem Requires https://github.com/stephendade/rockblock2mav at the GCS end Setup: This script requires 1 serial port: A "Script" to connect the RockBlock modem 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 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 send every RCK_PERIOD seconds MAVLink 1 will be used, as it's slightly more efficient (50 vs 52 bytes for a HL2 message) Any incoming packets on the first mailbox check will be ignored (as these may be from a long time in the past) Only 1 command can be sent at a time from the GCS. Any subsequent commands will overwrite the previous command The param SCR_VM_I_COUNT may need to be increased in some circumstances Written by Stephen Dade (stephen_dade@hotmail.com) ]]-- ---@diagnostic disable: cast-local-type local PARAM_TABLE_KEY = 10 local PARAM_TABLE_PREFIX = "RCK_" local port = serial:find_serial(0) if not port then gcs:send_text(0, "Rockblock: No Scripting Serial Port") return end port:begin(19200) port:set_flow_control(0) -- 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 RCK specific parameters assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 4), 'could not add param table') --[[ // @Param: RCK_FORCEHL // @DisplayName: Force enable High Latency mode // @Description: Automatically enables High Latency mode if not already enabled // @Values: 0:Disabled,1:Enabled // @User: Standard --]] RCK_FORCEHL = bind_add_param('FORCEHL', 1, 0) --[[ // @Param: RCK_PERIOD // @DisplayName: Update rate // @Description: When in High Latency mode, send Rockblock updates every N seconds // @Range: 0 600 // @Units: s // @User: Standard --]] RCK_PERIOD = bind_add_param('PERIOD', 2, 30) --[[ // @Param: RCK_DEBUG // @DisplayName: Display Rockblock debugging text // @Description: Sends Rockblock debug text to GCS via statustexts // @Values: 0:Disabled,1:Enabled // @User: Standard --]] RCK_DEBUG = bind_add_param('DEBUG', 3, 0) --[[ // @Param: RCK_ENABLE // @DisplayName: Enable Message transmission // @Description: Enables the Rockblock sending and recieving // @Values: 0:Disabled,1:Enabled // @User: Standard --]] RCK_ENABLE = bind_add_param('ENABLE', 4, 1) --[[ Lua Object for decoding and encoding MAVLink (V1 only) messages --]] local function MAVLinkProcessor() -- public fields local self = { -- define MAVLink message id's COMMAND_LONG = 76, COMMAND_INT = 75, HIGH_LATENCY2 = 235, MISSION_ITEM_INT = 73, SET_MODE = 11 } -- private fields local _mavbuffer = "" local _mavresult = {} local _payload_len = 0 local _mavdecodestate = 0 -- 0=looking for marker, 1=getting header,2=getting payload,3=getting crc PROTOCOL_MARKER_V1 = 0xFE HEADER_LEN_V1 = 6 local _txseqid = 0 -- AUTOGEN from MAVLink generator local _crc_extra = {} _crc_extra[75] = 0x9e _crc_extra[76] = 0x98 _crc_extra[235] = 0xb3 _crc_extra[73] = 0x26 _crc_extra[11] = 0x59 local _messages = {} _messages[75] = { -- COMMAND_INT {"param1", "> 8) ~ (tmp << 8) ~ (tmp << 3) ~ (tmp >> 4) crc = crc & 0xFFFF end return string.pack(" 263 then _mavbuffer = "" end return false end function self.bytesToString(buf, start, stop) local ret = "" for idx = start, stop do ret = ret .. string.format("0x%x ", buf:byte(idx), 1, -1) .. " " end return ret end function self.createMAVLink(message, msgid) -- generate a mavlink message (V1 only) -- create the payload local message_map = _messages[msgid] if not message_map then -- we don't know how to encode this message, bail on it gcs:send_text(3, "Rockblock: Unknown MAVLink message " .. msgid) return nil end local packString = "<" local packedTable = {} local packedIndex = 1 for i, v in ipairs(message_map) do if v[3] then packString = (packString .. string.rep(string.sub(v[2], 2), v[3])) for j = 1, v[3] do packedTable[packedIndex] = message[message_map[i][1]][j] packedIndex = packedIndex + 1 end else packString = (packString .. string.sub(v[2], 2)) packedTable[packedIndex] = message[message_map[i][1]] packedIndex = packedIndex + 1 end end local payload = string.pack(packString, table.unpack(packedTable)) -- create the header. Assume componentid of 1 local header = string.pack(' 0 then -- read buffer may include /r and /n, so need a special cycle to capture all up to the self.rx_len self.in_read_cycle = true _str_recieved = _str_recieved .. read elseif self.in_read_cycle and #_str_recieved == self.rx_len + 3 then -- get last byte in read cycle _str_recieved = _str_recieved .. read self.in_read_cycle = false self.rx_len = 0 table.insert(_modem_history, _str_recieved) _str_recieved = "" if RCK_DEBUG:get() == 1 then gcs:send_text(3, "Rockblock: Modem rx msg: " .. self.nicestring(_modem_history[#_modem_history])) end elseif (read == '\r' or read == '\n') and not self.in_read_cycle then if #_str_recieved > 0 then table.insert(_modem_history, _str_recieved) if RCK_DEBUG:get() == 1 then gcs:send_text(3, "Rockblock: Modem response: " .. self.nicestring(_modem_history[#_modem_history])) end maybepkt = self.check_cmd_return() end _str_recieved = "" else _str_recieved = _str_recieved .. read end return maybepkt end -- Parse any incoming text from the modem -- returns any payload data, nil otherwise function self.check_cmd_return() -- modem detection (response to AT_query) if #_modem_history == 3 and _modem_history[1] == 'AT+CGMM' and _modem_history[3] == 'OK' then gcs:send_text(3, "Rockblock modem detected - " .. self.nicestring(_modem_history[2])) _modem_history = nil _modem_history = {} self.modem_detected = true table.insert(_modem_to_send, _AT_clear_rx_buffer) table.insert(_modem_to_send, _AT_clear_tx_buffer) -- enable high latency mode, if desired if RCK_FORCEHL:get() == 1 then gcs:enable_high_latency_connections(true) end return nil end if self.modem_detected then -- TX Buffer clear (response to AT_query) if #_modem_history >= 3 and _modem_history[#_modem_history - 2] == 'AT+SBDD0' and _modem_history[#_modem_history] == 'OK' then if RCK_DEBUG:get() == 1 then gcs:send_text(3, "Rockblock cleared modem TX buffer") end _modem_history = nil _modem_history = {} return nil end -- RX buffer clear (response to AT_query) if #_modem_history >= 3 and _modem_history[#_modem_history - 2] == 'AT+SBDD1' and _modem_history[#_modem_history] == 'OK' then if RCK_DEBUG:get() == 1 then gcs:send_text(3, "Rockblock cleared modem RX buffer") end _modem_history = nil _modem_history = {} return nil end -- Tx buffer loaded (response to AT command) if #_modem_history >= 4 and string.find(_modem_history[#_modem_history - 3], _AT_load_tx_buffer, 1, true) and _modem_history[#_modem_history] == 'OK' then if RCK_DEBUG:get() == 1 then gcs:send_text(3, "Rockblock loaded packet into tx buffer, " .. tostring(_modem_history[#_modem_history - 3])) end if _modem_history[#_modem_history - 1] ~= '0' then gcs:send_text(3, "Rockblock Error loading packet into buffer") end _modem_history = nil _modem_history = {} return nil end -- Got received data (response to AT command) if _modem_history[#_modem_history - 2] == 'AT+SBDRB' and _modem_history[#_modem_history] == 'OK' then -- Message format is { 2 byte msg length} + {message} + {2 byte checksum} local totallen = #(_modem_history[#_modem_history - 1]) local len = string.unpack(">i2", _modem_history[#_modem_history - 1]) local msg = string.sub(_modem_history[#_modem_history - 1], 3, totallen - 2) local checksumrx = string.sub(_modem_history[#_modem_history - 1], totallen - 1, totallen) local highByte, lowByte = self.checksum(msg) -- check that received message is OK, then send to MAVLink processor if len ~= #msg then gcs:send_text(3, "Rockblock: Bad RX message length " .. tostring(len) .. " vs actual " .. tostring(#msg)) elseif checksumrx ~= (tostring(highByte) .. tostring(lowByte)) then gcs:send_text(3, "Rockblock: Bad RX CRC " .. checksumrx .. " vs " .. tostring(highByte) .. tostring(lowByte)) else if RCK_DEBUG:get() == 1 then gcs:send_text(3, "totmsg=" .. self.nicestring( _modem_history[#_modem_history - 1])) end end _modem_history = nil _modem_history = {} return msg end -- Mailbox check (response to AT command) (can be a fail or success) if _modem_history[#_modem_history - 2] == 'AT+SBDIXA' and _modem_history[#_modem_history] == 'OK' then -- Parse response (comma and : delimited, trimming whitespace) local statusReponse = {} for w in _modem_history[#_modem_history - 1]:gmatch("[^,:]+") do table.insert(statusReponse, w) end if #statusReponse == 7 then if tonumber(statusReponse[2]) < 5 then gcs:send_text(3, "Rockblock Packet Transmitted successfully") -- check for rx packet if tonumber(statusReponse[4]) == 1 and self.first_sucessful_mailbox_check then self.rx_len = tonumber(statusReponse[6]) gcs:send_text(3, "Rockblock Packet Received of len " .. tostring(self.rx_len)) -- read messages, if not first mailbox check table.insert(_modem_to_send, _AT_read_rx_buffer) elseif RCK_DEBUG:get() == 1 then gcs:send_text(3, "Rockblock: No message to receive") end self.first_sucessful_mailbox_check = true elseif tonumber(statusReponse[2]) == 32 then gcs:send_text(3, "Rockblock Error: No network service") else gcs:send_text(3, "Rockblock Error: Unable to send") end end _modem_history = nil _modem_history = {} self.is_transmitting = false return nil end end end function self.checksum(bytes) -- Checksum calculation for SBDWB -- The checksum is the least significant 2-bytes of the summation of the entire SBD message local SUM = 0 for idx = 1, #bytes do SUM = SUM + bytes:byte(idx) end local SUM_H = (SUM & 0xFF << 8) >> 8 local SUM_L = SUM & 0xFF if RCK_DEBUG:get() == 1 then gcs:send_text(3, "Rockblock: Modem CRC: " .. string.char(SUM_H) .. " " .. string.char(SUM_L)) end return string.char(SUM_H), string.char(SUM_L) end function self.nicestring(instr) -- make any strings printable to GCS (constrain to ASCII range) local retstr = "" for i = 1, #instr do local c = string.byte(instr:sub(i, i)) if c < 0x20 or c > 0x7E then c = 0x5F end retstr = retstr .. string.char(c) end return tostring(retstr) end function self.checkmodem() --- send detect command to modem every 10 sec if not detected if (millis():tofloat() * 0.001) - self.last_modem_status_check > 10 then table.insert(_modem_to_send, _AT_query) self.last_modem_status_check = millis():tofloat() * 0.001 end end function self.getnextcommandtosend() --- get the next string to send to the modem if #_modem_to_send == 0 then return nil end local ret = _modem_to_send[1] if RCK_DEBUG:get() == 1 then gcs:send_text(3, "Rockblock: Sent to modem: " .. self.nicestring(_modem_to_send[1])) end table.remove(_modem_to_send, 1) return ret end function self.loadtxbuffer(pkt) -- Load the transmit buffer of the modem -- Send as binary data, plus checksum local highByte, lowByte = self.checksum(pkt) table.insert(_modem_to_send, _AT_load_tx_buffer .. tostring(#pkt) .. '\r') table.insert(_modem_to_send, pkt) table.insert(_modem_to_send, highByte .. lowByte .. "\r") self.time_last_tx = millis():tofloat() * 0.001 if RCK_ENABLE:get() == 1 then table.insert(_modem_to_send, _AT_mailbox_check) self.is_transmitting = true end end -- return the instance return self end -- Transmitted HIGH_LATENCY2 packet local hl2 = {} hl2.timestamp = 0 hl2.latitude = 0 hl2.longitude = 0 hl2.custom_mode = 0 hl2.altitude = 0 hl2.target_altitude = 0 hl2.target_distance = 0 hl2.wp_num = 0 hl2.failure_flags = 0 hl2.type = gcs:frame_type() hl2.autopilot = 3 -- MAV_AUTOPILOT_ARDUPILOTMEGA hl2.heading = 0 hl2.target_heading = 0 hl2.throttle = 0 hl2.airspeed = 0 hl2.airspeed_sp = 0 hl2.groundspeed = 0 hl2.windspeed = 0 hl2.wind_heading = 0 hl2.eph = 0 hl2.epv = 0 hl2.temperature_air = 0 hl2.climb_rate = 0 hl2.battery = 0 hl2.custom0 = 1 -- MAV_MODE_FLAG_CUSTOM_MODE_ENABLED hl2.custom1 = 0 hl2.custom2 = 0 function wrap_360(angle) local res = angle % 360 if res < 0 then res = res + 360 end return res end -- Define the MAVLink processor local mavlink = MAVLinkProcessor() -- Define the RockBlock interface local rockblock = RockblockModem() function HLSatcom() -- read in any bytes from rockblock and form into received commands local n_bytes = port:available() while n_bytes > 0 do read = port:read() pkt = rockblock.rxdata(read) n_bytes = n_bytes - 1 -- we've got a MAVLink message from the GCS, parse if pkt ~= nil then for idx = 1, #pkt do mavlink.parseMAVLink(pkt:byte(idx)) end end end -- write out commands from send list. one cmd per loop local cmd = rockblock.getnextcommandtosend() if cmd ~= nil then for idx = 1, #cmd do port:write(cmd:byte(idx)) end end --- check if modem is there if not rockblock.modem_detected then gcs:send_text(3, "Rockblock: Trying to detect modem") rockblock.checkmodem() 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 (millis():tofloat() * 0.001) - rockblock.time_last_tx > RCK_PERIOD:get() and not rockblock.is_transmitting then -- update HL2 packet hl2.timestamp = millis():tofloat() local position = ahrs:get_location() local wind = ahrs:wind_estimate() if position then hl2.latitude = tonumber(position:lat()) hl2.longitude = tonumber(position:lng()) hl2.altitude = math.floor(tonumber(position:alt()) * 0.01) end if wind then wind_xy = Vector2f() wind_xy:x(wind:x()) wind_xy:y(wind:y()) hl2.windspeed = math.abs(math.floor(wind_xy:length() * 5)) hl2.wind_heading = math.floor(wrap_360(wind_xy:angle()) / 2) end hl2.custom_mode = vehicle:get_mode() if vehicle:get_wp_distance_m() ~= nil then hl2.target_distance = math.floor(vehicle:get_wp_distance_m() / 10) end if mission:get_current_nav_index() ~= nil then hl2.wp_num = mission:get_current_nav_index() end if vehicle:get_wp_bearing_deg() ~= nil then hl2.target_heading = math.floor(wrap_360( vehicle:get_wp_bearing_deg()) / 2) end -- failure flags hl2.failure_flags = 0 if not ahrs:healthy() then hl2.failure_flags = hl2.failure_flags + 4096 -- HL_FAILURE_FLAG_ESTIMATOR end if battery:num_instances() > 0 and not battery:healthy(0) then hl2.failure_flags = hl2.failure_flags + 128 -- HL_FAILURE_FLAG_BATTERY end if gps:num_sensors() > 0 and gps:status(0) <= gps.NO_FIX then hl2.failure_flags = hl2.failure_flags + 1 -- HL_FAILURE_FLAG_GPS end if (FWVersion:type() == 2 or FWVersion:type() == 3) and terrain:status() == terrain.TerrainStatusUnhealthy then -- only for copter and plane hl2.failure_flags = hl2.failure_flags + 64 -- HL_FAILURE_FLAG_TERRAIN end if not rc:has_valid_input() then hl2.failure_flags = hl2.failure_flags + 256 -- HL_FAILURE_FLAG_RC_RECEIVER end hl2.heading = math.floor(wrap_360(math.deg(ahrs:get_yaw())) / 2) hl2.throttle = math.floor(gcs:get_hud_throttle()) if ahrs:airspeed_estimate() ~= nil then hl2.airspeed = math.abs(math.floor(ahrs:airspeed_estimate() * 5)) end -- hl2.airspeed_sp = 0 hl2.groundspeed = math.abs(math.floor( ahrs:groundspeed_vector():length() * 5)) hl2.temperature_air = math.floor(baro:get_external_temperature()) if battery:num_instances() > 0 and battery:capacity_remaining_pct(0) ~= nil then hl2.battery = battery:capacity_remaining_pct(0) else hl2.battery = 0 end -- just sending armed state here for simplicity. Flight mode is in the custom_mode field if arming:is_armed() then hl2.custom0 = 129 -- MAV_MODE_FLAG_SAFETY_ARMED + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED else hl2.custom0 = 1 -- MAV_MODE_FLAG_CUSTOM_MODE_ENABLED end local newpkt = mavlink.createMAVLink(hl2, mavlink.HIGH_LATENCY2) if #newpkt > 50 then gcs:send_text(3, "Rockblock: Tx packet > 50 bytes: " .. tostring(#newpkt)) end -- send packet rockblock.loadtxbuffer(newpkt) 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(3, "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(1000 / 10) end -- start running HLSatcom loop return protected_wrapper()