--[[ 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) ]]-- local PARAM_TABLE_KEY = 10 local PARAM_TABLE_PREFIX = "RCK_" local port = serial:find_serial(0) if not port or baud == 0 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", "<f"}, {"param2", "<f"}, {"param3", "<f"}, {"param4", "<f"}, {"x", "<i4"}, {"y", "<i4"}, {"z", "<f"}, {"command", "<I2"}, {"target_system", "<B"}, {"target_component", "<B"}, {"frame", "<B"}, {"current", "<B"}, {"autocontinue", "<B"} } _messages[76] = { -- COMMAND_LONG {"param1", "<f"}, {"param2", "<f"}, {"param3", "<f"}, {"param4", "<f"}, {"param5", "<f"}, {"param6", "<f"}, {"param7", "<f"}, {"command", "<I2"}, {"target_system", "<B"}, {"target_component", "<B"}, {"confirmation", "<B"} } _messages[235] = { -- HIGH_LATENCY2 {"timestamp", "<I4"}, {"latitude", "<i4"}, {"longitude", "<i4"}, {"custom_mode", "<I2"}, {"altitude", "<i2"}, {"target_altitude", "<i2"}, {"target_distance", "<I2"}, {"wp_num", "<I2"}, {"failure_flags", "<I2"}, {"type", "<B"}, {"autopilot", "<B"}, {"heading", "<B"}, {"target_heading", "<B"}, {"throttle", "<B"}, {"airspeed", "<B"}, {"airspeed_sp", "<B"}, {"groundspeed", "<B"}, {"windspeed", "<B"}, {"wind_heading", "<B"}, {"eph", "<B"}, {"epv", "<B"}, {"temperature_air", "<b"}, {"climb_rate", "<b"}, {"battery", "<b"}, {"custom0", "<B"}, -- should be <b (int8), but we're hacking this into a uint8 instead {"custom1", "<b"}, {"custom2", "<b"} } _messages[73] = { -- MISSION_ITEM_INT {"param1", "<f"}, {"param2", "<f"}, {"param3", "<f"}, {"param4", "<f"}, {"x", "<i4"}, {"y", "<i4"}, {"z", "<f"}, {"seq", "<I2"}, {"command", "<I2"}, {"target_system", "<B"}, {"target_component", "<B"}, {"frame", "<B"}, {"current", "<B"}, {"autocontinue", "<B"} } _messages[11] = { -- SET_MODE { "custom_mode", "<I4" }, { "target_system", "<B" }, { "base_mode", "<B" }, } function self.getSeqID() return _txseqid end function self.generateCRC(buffer) -- generate the x25crc for a given buffer. Make sure to include crc_extra! local crc = 0xFFFF for i = 1, #buffer do local tmp = string.byte(buffer, i, i) ~ (crc & 0xFF) tmp = (tmp ~ (tmp << 4)) & 0xFF crc = (crc >> 8) ~ (tmp << 8) ~ (tmp << 3) ~ (tmp >> 4) crc = crc & 0xFFFF end return string.pack("<H", crc) end function self.parseMAVLink(byte) -- parse a new byte and see if we've got MAVLink message -- returns true if a packet was decoded, false otherwise _mavbuffer = _mavbuffer .. string.char(byte) -- parse buffer to find MAVLink packets if #_mavbuffer == 1 and string.byte(_mavbuffer, 1) == PROTOCOL_MARKER_V1 and _mavdecodestate == 0 then -- we have a packet start _mavdecodestate = 1 return end -- if we have a full header, try parsing if #_mavbuffer == HEADER_LEN_V1 and _mavdecodestate == 1 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 -- fetch seq/sysid/compid _mavresult.seq, _mavresult.sysid, _mavresult.compid, read_marker = string.unpack("<BBB", _mavbuffer, read_marker) -- fetch the message id _mavresult.msgid, _ = string.unpack("<B", _mavbuffer, read_marker) _mavdecodestate = 2 return end -- get payload if _mavdecodestate == 2 and #_mavbuffer == (_payload_len + HEADER_LEN_V1) then _mavdecodestate = 3 _mavresult.payload = string.sub(_mavbuffer, HEADER_LEN_V1 + 1) return end -- get crc, then process if CRC ok if _mavdecodestate == 3 and #_mavbuffer == (_payload_len + HEADER_LEN_V1 + 2) then _mavdecodestate = 0 _mavresult.crc = string.sub(_mavbuffer, -2, -1) local message_map = _messages[_mavresult.msgid] if not message_map then -- we don't know how to decode this message, bail on it _mavbuffer = "" return true end -- check CRC, if message defined local crc_extra_msg = _crc_extra[_mavresult.msgid] if crc_extra_msg ~= nil then local calccrc = self.generateCRC( string.sub(_mavbuffer, 2, -3) .. string.char(crc_extra_msg)) if _mavresult.crc ~= calccrc then gcs:send_text(3, "Rockblock: Bad Mavlink CRC: " .. self.bytesToString(_mavbuffer, -2, -1) .. ", " .. self.bytesToString(calccrc, 1, 2)) _mavbuffer = "" return end end -- map all the fields out local offset = 1 for _, v in ipairs(message_map) do if v[3] then _mavresult[v[1]] = {} for j = 1, v[3] do _mavresult[v[1]][j], offset = string.unpack(v[2], _mavresult.payload, offset) end else _mavresult[v[1]], offset = string.unpack(v[2], _mavresult.payload, 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) if _mavresult.command == 16 then -- MAV_CMD_NAV_WAYPOINT local loc = Location() loc:lat(_mavresult.x) loc:lng(_mavresult.y) loc:alt(_mavresult.z * 100) if _mavresult.frame == 10 then -- MAV_FRAME_GLOBAL_TERRAIN_ALT loc:terrain_alt(true) elseif _mavresult.frame == 3 then -- MAV_FRAME_GLOBAL_RELATIVE_ALT loc:relative_alt(true) end vehicle:set_target_location(loc) 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 end end _mavbuffer = "" return true end -- packet too big ... start again if #_mavbuffer > 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('<BBBBBB', PROTOCOL_MARKER_V1, #payload, _txseqid, param:get('SYSID_THISMAV'), 1, msgid) -- generate the CRC local crc_extra_msg = _crc_extra[msgid] local crc = self.generateCRC(string.sub(header, 2) .. payload .. string.char(crc_extra_msg)) -- iterate sequence id _txseqid = (_txseqid + 1) % 255 return header .. payload .. crc end -- return the instance return self end --[[ Lua Object for managing the RockBlock modem --]] local function RockblockModem() -- public fields local self = { is_transmitting = false, first_sucessful_mailbox_check = false, time_last_tx = millis():tofloat() * 0.001, last_modem_status_check = 0, modem_detected = false, in_read_cycle = false, rx_len = 0 } -- private fields local _AT_query = "AT+CGMM\r" local _AT_mailbox_check = "AT+SBDIXA\r" local _AT_load_tx_buffer = "AT+SBDWB=" local _AT_read_rx_buffer = "AT+SBDRB\r" local _AT_clear_tx_buffer = "AT+SBDD0\r" local _AT_clear_rx_buffer = "AT+SBDD1\r" local _modem_history = {} local _modem_to_send = {} local _str_recieved = "" -- Get any incoming data function self.rxdata(inchar) read = string.char(inchar) local maybepkt = nil if _modem_history[#_modem_history] == 'AT+SBDRB' and self.in_read_cycle == false and self.rx_len > 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()