From d4bb4e4526d6c81964f01611cd8e3ae1e3a246d5 Mon Sep 17 00:00:00 2001 From: Stephen Dade Date: Tue, 24 Jan 2023 16:47:01 +1100 Subject: [PATCH] AP_Scripting: Add lua scripts for Rockblock modem --- libraries/AP_Scripting/applets/RockBlock.lua | 814 +++++++++++++++++++ libraries/AP_Scripting/applets/RockBlock.md | 41 + 2 files changed, 855 insertions(+) create mode 100644 libraries/AP_Scripting/applets/RockBlock.lua create mode 100644 libraries/AP_Scripting/applets/RockBlock.md diff --git a/libraries/AP_Scripting/applets/RockBlock.lua b/libraries/AP_Scripting/applets/RockBlock.lua new file mode 100644 index 0000000000..b70abea24d --- /dev/null +++ b/libraries/AP_Scripting/applets/RockBlock.lua @@ -0,0 +1,814 @@ +--[[ 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 + } + + -- 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 + _crc_extra = {} + _crc_extra[75] = 0x9e + _crc_extra[76] = 0x98 + _crc_extra[235] = 0xb3 + _crc_extra[73] = 0x26 + + _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()) + hl2.battery = battery:capacity_remaining_pct(0) + + -- 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(MAV_SEVERITY_ERROR, "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() \ No newline at end of file diff --git a/libraries/AP_Scripting/applets/RockBlock.md b/libraries/AP_Scripting/applets/RockBlock.md new file mode 100644 index 0000000000..edd89643da --- /dev/null +++ b/libraries/AP_Scripting/applets/RockBlock.md @@ -0,0 +1,41 @@ +# RockBlock Lua Script + +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 + +Note that this uses MAVLink1 messaging, due to it's smaller message size compared +to MAVLink2. + +Messages will only be send or received if High Latency Mode is enabled. + +Setup: +This script requires 1 serial port: +- A "Script" port to connect the modem to + +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 RCK_PERIOD sec, in addition to receiving + a single MAVLink packet from the GCS. This is known as a mailbox check. +- 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 per mailbox check from the GCS. Any additional commands will overwrite the previous command + +# Parameters + +The script adds the following parameters: + +## RCK_FORCEHL + +Automatically enables High Latency mode if not already enabled + +## RCK_PERIOD + +When in High Latency mode, send Rockblock updates every RCK_PERIOD seconds + +## RCK_DEBUG + +Sends Rockblock debug text to GCS via statustexts + +## RCK_ENABLE + +Enables the modem transmission