mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
Andrew Tridgell
acda8428f8
using CMUX allows us to have AT commands run in parallel with PPP or TCP data, allowing for signal strength and other data to be retrieved this also adds auto-bauding, with separate LTE_IBAUD and LTE_BAUD parameters, avoiding the need to configure the hardware baud rate with a terminal program
842 lines
24 KiB
Lua
842 lines
24 KiB
Lua
--[[
|
|
driver for LTE modems with AT command set
|
|
supported chipsets:
|
|
- SIM7600
|
|
--]]
|
|
|
|
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
|
|
|
|
local PARAM_TABLE_KEY = 106
|
|
local PARAM_TABLE_PREFIX = "LTE_"
|
|
|
|
-- local MAVLINK2 = 2
|
|
local PPP = 48
|
|
|
|
-- add a parameter and bind it to a variable
|
|
local 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 Parameter(PARAM_TABLE_PREFIX .. name)
|
|
end
|
|
|
|
-- Setup Parameters
|
|
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 20), 'LTE_modem: could not add param table')
|
|
|
|
--[[
|
|
// @Param: LTE_ENABLE
|
|
// @DisplayName: LTE Enable
|
|
// @Description: Enable or disable the LTE modem driver
|
|
// @Values: 0:Disabled,1:Enabled
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_ENABLE = bind_add_param('ENABLE', 1, 1)
|
|
|
|
--[[
|
|
// @Param: LTE_SERPORT
|
|
// @DisplayName: Serial Port
|
|
// @Description: Serial port to use for the LTE modem. This is the index of the SERIALn_ ports that are set to 28 for "scripting"
|
|
// @Range: 0 8
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_SERPORT = bind_add_param('SERPORT', 2, 0)
|
|
|
|
--[[
|
|
// @Param: LTE_SCRPORT
|
|
// @DisplayName: Scripting Serial Port
|
|
// @Description: Scripting Serial port to use for the LTE modem. This is the index of the SCR_SDEV ports that are set to 2 for "MAVLink2"
|
|
// @Range: 0 8
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_SCRPORT = bind_add_param('SCRPORT', 3, 0)
|
|
|
|
--[[
|
|
// @Param: LTE_SERVER_IP0
|
|
// @DisplayName: Server IP 0
|
|
// @Description: First octet of the server IP address to connect to
|
|
// @Range: 0 255
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_SERVER_IP0 = bind_add_param('SERVER_IP0', 4, 0)
|
|
|
|
--[[
|
|
// @Param: LTE_SERVER_IP1
|
|
// @DisplayName: Server IP 1
|
|
// @Description: Second octet of the server IP address to connect to
|
|
// @Range: 0 255
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_SERVER_IP1 = bind_add_param('SERVER_IP1', 5, 0)
|
|
|
|
--[[
|
|
// @Param: LTE_SERVER_IP2
|
|
// @DisplayName: Server IP 2
|
|
// @Description: Third octet of the server IP address to connect to
|
|
// @Range: 0 255
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_SERVER_IP2 = bind_add_param('SERVER_IP2', 6, 0)
|
|
|
|
--[[
|
|
// @Param: LTE_SERVER_IP3
|
|
// @DisplayName: Server IP 3
|
|
// @Description: Fourth octet of the server IP address to connect to
|
|
// @Range: 0 255
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_SERVER_IP3 = bind_add_param('SERVER_IP3', 7, 0)
|
|
|
|
--[[
|
|
// @Param: LTE_SERVER_PORT
|
|
// @DisplayName: Server Port
|
|
// @Description: IPv4 Port of the server to connect to
|
|
// @Range: 1 65525
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_SERVER_PORT = bind_add_param('SERVER_PORT', 8, 0)
|
|
|
|
--[[
|
|
// @Param: LTE_BAUD
|
|
// @DisplayName: Serial Baud Rate
|
|
// @Description: Baud rate for the serial port to the LTE modem when connected. Initial power on baudrate is in LTE_IBAUD
|
|
// @Values: 19200:19200,38400:38400,57600:57600,115200:115200,230400:230400,460800:460800,921600:921600,3686400:3686400
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_BAUD = bind_add_param('BAUD', 9, 921600)
|
|
|
|
--[[
|
|
// @Param: LTE_TIMEOUT
|
|
// @DisplayName: Timeout
|
|
// @Description: Timeout in seconds for the LTE connection. If no data is received for this time, the connection will be reset.
|
|
// @Range: 1 60
|
|
// @Units: s
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_TIMEOUT = bind_add_param('TIMEOUT', 10, 10)
|
|
|
|
--[[
|
|
// @Param: LTE_PROTOCOL
|
|
// @DisplayName: LTE protocol
|
|
// @Description: The protocol that we will use in communication with the LTE modem. If this is PPP then the LTE_SERVER parameters are not used and instead a PPP connection will be established and you should use the NET_ parameters to enable network ports. If this is MAVLink2 then the LTE_SERVER parameters are used to create a TCP connection to a single TCP server.
|
|
// @Values: 2:MavLink2,48:PPP
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_PROTOCOL = bind_add_param('PROTOCOL', 11, 48)
|
|
|
|
--[[
|
|
// @Param: LTE_OPTIONS
|
|
// @DisplayName: LTE options
|
|
// @Description: Options to control the LTE modem driver. If VerboseSignalInfoGCS is set then additional NAMED_VALUE_FLOAT values are sent with verbose signal information
|
|
// @Bitmask: 0:LogAllData,1:VerboseSignalInfoGCS
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_OPTIONS = bind_add_param('OPTIONS', 12, 0)
|
|
|
|
--[[
|
|
// @Param: LTE_IBAUD
|
|
// @DisplayName: LTE initial baudrate
|
|
// @Description: This is the initial baud rate on power on for the modem. This is set in the modem with the AT+IREX=baud command
|
|
// @Values: 19200:19200,38400:38400,57600:57600,115200:115200,230400:230400,460800:460800,921600:921600,3686400:3686400
|
|
// @User: Standard
|
|
--]]
|
|
local LTE_IBAUD = bind_add_param('IBAUD', 13, 115200)
|
|
|
|
local LTE_OPTIONS_LOGALL = (1<<0)
|
|
local LTE_OPTIONS_SIGNALS = (1<<1)
|
|
|
|
--[[
|
|
return true if an option is enabled
|
|
--]]
|
|
local function option_enabled(option)
|
|
return (LTE_OPTIONS:get() & option) ~= 0
|
|
end
|
|
|
|
if LTE_ENABLE:get() == 0 then
|
|
-- disabled
|
|
return
|
|
end
|
|
|
|
local uart = serial:find_serial(LTE_SERPORT:get())
|
|
if not uart then
|
|
gcs:send_text(MAV_SEVERITY.ERROR, 'LTE_modem: could not find serial port')
|
|
return
|
|
end
|
|
|
|
local ser_device = serial:find_simulated_device(LTE_PROTOCOL:get(), LTE_SCRPORT:get())
|
|
if not ser_device then
|
|
gcs:send_text(MAV_SEVERITY.ERROR, 'LTE_modem: could not find SCR_SDEV device')
|
|
return
|
|
end
|
|
|
|
local step = "ATI"
|
|
|
|
local stats = { bytes_in = 0, bytes_out = 0 }
|
|
|
|
uart:begin(LTE_IBAUD:get())
|
|
|
|
--[[
|
|
Open a log file to log the output from the modem
|
|
This is useful for debugging the connection process
|
|
--]]
|
|
local log_file = io.open('LTE_modem.log', 'w')
|
|
|
|
--[[
|
|
log data to log_file
|
|
--]]
|
|
local function log_data(s, marker)
|
|
if s and #s > 0 and log_file then
|
|
log_file:write(marker .. '[' .. s .. ']\n')
|
|
log_file:flush()
|
|
end
|
|
end
|
|
|
|
--[[
|
|
Function to read from the UART and log the output
|
|
This function reads up to 512 bytes at a time and writes it to the log file
|
|
returns the string read or nil
|
|
--]]
|
|
local function uart_read()
|
|
local s = uart:readstring(512)
|
|
log_data(s, '<<<')
|
|
stats.bytes_in = stats.bytes_in + #s
|
|
return s
|
|
end
|
|
|
|
--[[
|
|
Function to write to the UART and log the command
|
|
--]]
|
|
local function uart_write(s)
|
|
uart:writestring(s)
|
|
log_data(s, '>>>')
|
|
stats.bytes_out = stats.bytes_out + #s
|
|
return #s
|
|
end
|
|
|
|
-- Constants for GSM 07.10 CMUX framing
|
|
local FLAG = 0xF9
|
|
local UIH = 0xEF
|
|
local SABM = 0x2F
|
|
--local UA = 0x63
|
|
local EA = 0x01
|
|
local CR_SEND = 0x02
|
|
|
|
-- CMUX buffer state
|
|
local cmux = {}
|
|
cmux.buffers = {[1] = "", [2] = ""} -- DLC1=AT, DLC2=DATA(PPP or TCP)
|
|
|
|
--[[
|
|
FCS lookup table for polynomial x^8 + x^2 + x^1 + 1 (0x07)
|
|
This is the reverse of the standard CRC-8 table
|
|
--]]
|
|
local fcs_table = {
|
|
0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75,
|
|
0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b,
|
|
0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69,
|
|
0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67,
|
|
0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d,
|
|
0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43,
|
|
0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51,
|
|
0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f,
|
|
0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05,
|
|
0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b,
|
|
0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19,
|
|
0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17,
|
|
0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d,
|
|
0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33,
|
|
0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21,
|
|
0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f,
|
|
0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95,
|
|
0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b,
|
|
0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89,
|
|
0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87,
|
|
0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad,
|
|
0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3,
|
|
0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1,
|
|
0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf,
|
|
0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5,
|
|
0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb,
|
|
0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9,
|
|
0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7,
|
|
0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd,
|
|
0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3,
|
|
0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1,
|
|
0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf
|
|
}
|
|
|
|
--[[
|
|
Calculate FCS for a byte array
|
|
data: table of bytes (numbers 0-255) or string
|
|
Returns: FCS value (0-255)
|
|
--]]
|
|
local function fcs_calc(data)
|
|
local fcs = 0xff -- Initial value
|
|
|
|
for i = 1, #data do
|
|
local byte = string.byte(data, i)
|
|
fcs = fcs_table[((fcs ~ byte) & 0xff) + 1] ~ (fcs >> 8)
|
|
end
|
|
|
|
return (~fcs) & 0xff
|
|
end
|
|
|
|
-- Construct a CMUX frame for a given DLC, data type and data
|
|
function cmux.encode_cmux_frame(dlc, dtype, data)
|
|
local addr = string.char((dlc << 2) | EA | CR_SEND)
|
|
local ctrl = string.char(dtype | 0x10)
|
|
local len = #data
|
|
local len_byte = string.char((len << 1) | EA)
|
|
local header = addr .. ctrl .. len_byte
|
|
local fcs = string.char(fcs_calc(header))
|
|
return string.char(FLAG) .. header .. data .. fcs .. string.char(FLAG)
|
|
end
|
|
|
|
--[[
|
|
send an AT command string with CMUX framing
|
|
--]]
|
|
local function cmux_AT_send(atcmd)
|
|
local s = cmux.encode_cmux_frame(1, UIH, atcmd)
|
|
return uart_write(s) == #s
|
|
end
|
|
|
|
--[[
|
|
send an appropriate data reset for the protocol
|
|
--]]
|
|
local function send_data_reset()
|
|
if LTE_PROTOCOL:get() == PPP then
|
|
cmux_AT_send('ATH\r\n')
|
|
else
|
|
cmux_AT_send('AT+CRESET\r\n')
|
|
end
|
|
end
|
|
|
|
--[[
|
|
Function to handle errors in the response from the modem
|
|
If an error is detected, it resets the modem
|
|
returns true if an error was detected
|
|
--]]
|
|
local function handle_error(s)
|
|
if s and s:find('\nERROR\r\n') then
|
|
gcs:send_text(MAV_SEVERITY.ERROR, 'LTE_modem: error response from modem')
|
|
send_data_reset()
|
|
step = "ATI"
|
|
return true
|
|
end
|
|
return false
|
|
end
|
|
|
|
-- Send SABM (Set Asynchronous Balanced Mode) for all DLCs
|
|
function cmux.send_sabm()
|
|
uart_write(cmux.encode_cmux_frame(0, SABM, ""))
|
|
uart_write(cmux.encode_cmux_frame(1, SABM, ""))
|
|
uart_write(cmux.encode_cmux_frame(2, SABM, ""))
|
|
end
|
|
|
|
--[[
|
|
Parses a single CMUX frame from a byte buffer.
|
|
Returns: DLC number, extracted payload, and remaining buffer (or nils on failure)
|
|
--]]
|
|
function cmux.parse_cmux_frame(buf)
|
|
local start_idx = buf:find(string.char(FLAG))
|
|
if not start_idx then
|
|
--gcs:send_text(MAV_SEVERITY.INFO, "no start idx")
|
|
return nil, nil, nil
|
|
end
|
|
local end_idx = buf:find(string.char(FLAG), start_idx + 1)
|
|
if not end_idx then
|
|
--gcs:send_text(MAV_SEVERITY.INFO, "no end idx")
|
|
return nil, nil, nil, "short"
|
|
end
|
|
|
|
local frame = buf:sub(start_idx + 1, end_idx - 1)
|
|
if #frame < 4 then
|
|
--gcs:send_text(MAV_SEVERITY.INFO, "too short")
|
|
return nil, nil, nil
|
|
end
|
|
|
|
local addr = frame:byte(1)
|
|
local ctrl = frame:byte(2)
|
|
|
|
--gcs:send_text(MAV_SEVERITY.INFO, string.format("addr=0x%02x ctrl=0x%02x", addr, ctrl))
|
|
|
|
if ctrl == SABM then
|
|
return nil, nil, buf:sub(end_idx + 1)
|
|
end
|
|
|
|
if (ctrl & 0xef) ~= UIH then
|
|
return nil, nil, nil
|
|
end
|
|
|
|
local len_byte = frame:byte(3)
|
|
if (len_byte & EA) == 0 then
|
|
gcs:send_text(MAV_SEVERITY.INFO, "mux multibyte")
|
|
return nil, nil, nil -- we don't handle multi-byte length yet
|
|
end
|
|
local len = len_byte >> 1
|
|
if #frame < 3 + len + 1 then return nil, nil, nil end
|
|
|
|
local data = frame:sub(4, 3 + len)
|
|
local fcs_field = frame:byte(3 + len + 1)
|
|
local header = frame:sub(1, 3)
|
|
local calc_fcs = fcs_calc(header)
|
|
if calc_fcs ~= fcs_field then
|
|
gcs:send_text(MAV_SEVERITY.INFO, "FCS mismatch")
|
|
return nil, nil, nil -- FCS mismatch
|
|
end
|
|
|
|
local dlc = (addr >> 2) & 0x3F
|
|
local remainder = buf:sub(end_idx + 1)
|
|
--gcs:send_text(MAV_SEVERITY.INFO, string.format("CMUX got: dlc=%d ldata=%d lrem=%d", dlc, #data, #remainder))
|
|
return dlc, data, remainder
|
|
end
|
|
|
|
-- Feeds raw UART data into CMUX frame parser and routes payloads to DLC buffers
|
|
function cmux.feed_uart_in(raw)
|
|
while #raw > 0 do
|
|
local dlc, data, rest, err = cmux.parse_cmux_frame(raw)
|
|
if not dlc or not data or not rest then
|
|
if err == "short" then
|
|
return raw
|
|
end
|
|
-- discard
|
|
return ""
|
|
end
|
|
if cmux.buffers[dlc] then
|
|
cmux.buffers[dlc] = cmux.buffers[dlc] .. data
|
|
end
|
|
raw = rest
|
|
end
|
|
return raw
|
|
end
|
|
|
|
--[[
|
|
send data with CMUX framing
|
|
--]]
|
|
local function cmux_data_send(data)
|
|
local s = cmux.encode_cmux_frame(2, UIH, data)
|
|
return uart_write(s) == #s
|
|
end
|
|
|
|
--[[
|
|
send data with CMUX framing when connected (logging only if data
|
|
logging enabled)
|
|
--]]
|
|
local function cmux_data_send_connected(data)
|
|
local s = cmux.encode_cmux_frame(2, UIH, data)
|
|
if option_enabled(LTE_OPTIONS_LOGALL) then
|
|
log_data(s, '>>>')
|
|
end
|
|
local n = uart:writestring(s)
|
|
stats.bytes_out = stats.bytes_out + n
|
|
return n == #s
|
|
end
|
|
|
|
local ati_sequence = 0
|
|
|
|
--[[
|
|
Function to confirm the connection to the modem
|
|
it uses AIT command to get the modem info
|
|
|
|
when we enter the ATI step the modem could be in one of several states:
|
|
|
|
- in AT command mode
|
|
- in muxed mode
|
|
- in muxed mode at higher baudrate
|
|
--]]
|
|
local function step_ATI()
|
|
local s = uart_read()
|
|
if s and s:find('IMEI: ') then
|
|
gcs:send_text(MAV_SEVERITY.INFO, 'LTE_modem: found modem')
|
|
if cmux.parse_cmux_frame(s) then
|
|
-- already in CMUX mode
|
|
cmux.send_sabm()
|
|
send_data_reset()
|
|
step = "BAUD"
|
|
else
|
|
step = "CMUX"
|
|
end
|
|
return
|
|
end
|
|
if s and #s >= 4 and s:byte(1) == FLAG and s:byte(-1) == FLAG then
|
|
-- already in mux mode
|
|
send_data_reset()
|
|
cmux_AT_send('ATI\r\n')
|
|
return
|
|
end
|
|
if ati_sequence % 2 == 1 then
|
|
uart_write('+++')
|
|
else
|
|
uart_write('\r\nATI\r\n')
|
|
end
|
|
if ati_sequence % 10 == 5 then
|
|
uart:begin(LTE_BAUD:get())
|
|
end
|
|
if ati_sequence % 10 == 9 then
|
|
uart:begin(LTE_IBAUD:get())
|
|
end
|
|
ati_sequence = ati_sequence + 1
|
|
end
|
|
|
|
local change_baud = nil
|
|
|
|
--[[
|
|
change baud rate
|
|
--]]
|
|
local function step_BAUD()
|
|
cmux_AT_send(string.format('AT+IPR=%u\r\n', LTE_BAUD:get()))
|
|
step = "CREG"
|
|
change_baud = LTE_BAUD:get()
|
|
end
|
|
|
|
--[[
|
|
confirm we are registered on the network
|
|
--]]
|
|
local function step_CREG()
|
|
local s = uart_read()
|
|
if handle_error(s) then
|
|
return
|
|
end
|
|
if s and s:find('CREG: 0,1\r\n') then
|
|
gcs:send_text(MAV_SEVERITY.INFO, 'LTE_modem: CREG OK')
|
|
if LTE_PROTOCOL:get() == PPP then
|
|
step = "PPPOPEN"
|
|
else
|
|
step = "CIPMODE"
|
|
end
|
|
return
|
|
end
|
|
cmux_AT_send('AT+CREG?\r\n')
|
|
end
|
|
|
|
--[[
|
|
set the modem to transparent mode
|
|
--]]
|
|
local function step_CIPMODE()
|
|
local s = uart_read()
|
|
if handle_error(s) then
|
|
return
|
|
end
|
|
if s and s:find('CIPMODE=1\r') and s:find('\r\r\nOK\r') then
|
|
gcs:send_text(MAV_SEVERITY.INFO, 'LTE_modem: transparent mode set')
|
|
step = "NETOPEN"
|
|
return
|
|
end
|
|
cmux_data_send('AT+CIPMODE=1\r\n')
|
|
end
|
|
|
|
--[[
|
|
setup CMUX multiplexing mode
|
|
--]]
|
|
local function step_CMUX()
|
|
local s = uart_read()
|
|
if handle_error(s) then
|
|
return
|
|
end
|
|
if s and s:find('CMUX=0\r\r\nOK\r') then
|
|
gcs:send_text(MAV_SEVERITY.INFO, 'LTE_modem: CMUX mode set')
|
|
-- send SABM frames to establish the DLCs
|
|
send_data_reset()
|
|
cmux.send_sabm()
|
|
step = "BAUD"
|
|
return
|
|
end
|
|
uart_write('AT+CMUX=0\r\n')
|
|
end
|
|
|
|
--[[
|
|
open the network stack
|
|
needed to be able to open a TCP connection
|
|
--]]
|
|
local function step_NETOPEN()
|
|
local s = uart_read()
|
|
if handle_error(s) then
|
|
return
|
|
end
|
|
if s and s:find('NETOPEN\r') and s:find('\r\r\nOK\r\n') then
|
|
gcs:send_text(MAV_SEVERITY.INFO, 'LTE_modem: network opened')
|
|
step = "CIPOPEN"
|
|
return
|
|
end
|
|
cmux_data_send('AT+NETOPEN\r\n')
|
|
end
|
|
|
|
local last_data_ms = millis()
|
|
local pending_to_modem = ""
|
|
local pending_to_fc = ""
|
|
local pending_to_parse = ""
|
|
|
|
--[[
|
|
open PPP mode
|
|
--]]
|
|
local function step_PPPOPEN()
|
|
local s = uart_read()
|
|
if handle_error(s) then
|
|
return
|
|
end
|
|
|
|
if s and s:find('CONNECT ') then
|
|
gcs:send_text(MAV_SEVERITY.INFO, 'LTE_modem: connected')
|
|
last_data_ms = millis()
|
|
pending_to_modem = ""
|
|
pending_to_fc = ""
|
|
pending_to_parse = ""
|
|
step = "CONNECTED"
|
|
return
|
|
end
|
|
cmux_data_send('AT+CGDATA="PPP",1\r\n')
|
|
end
|
|
|
|
--[[
|
|
open a TCP connection to the server
|
|
the server IP and port are defined in the parameters
|
|
--]]
|
|
local function step_CIPOPEN()
|
|
local s = uart_read()
|
|
if handle_error(s) then
|
|
return
|
|
end
|
|
|
|
if s and s:find('CONNECT ') then
|
|
gcs:send_text(MAV_SEVERITY.INFO, 'LTE_modem: connected')
|
|
last_data_ms = millis()
|
|
pending_to_modem = ""
|
|
pending_to_fc = ""
|
|
pending_to_parse = ""
|
|
step = "CONNECTED"
|
|
return
|
|
end
|
|
if LTE_SERVER_PORT:get() <= 0 then
|
|
gcs:send_text(MAV_SEVERITY.ERROR, "Must set LTE_SERVER_PORT")
|
|
return
|
|
end
|
|
cmux_data_send(string.format('AT+CIPOPEN=0,"TCP","%d.%d.%d.%d",%d\r\n',
|
|
LTE_SERVER_IP0:get(), LTE_SERVER_IP1:get(), LTE_SERVER_IP2:get(), LTE_SERVER_IP3:get(),
|
|
LTE_SERVER_PORT:get()))
|
|
end
|
|
|
|
--[[
|
|
handle AT replies in CMUX mode
|
|
--]]
|
|
local function handle_AT_reply(s)
|
|
-- check for CSQ reply
|
|
local rssi_raw, ber_raw = s:match("%+CSQ:%s*(%d+),(%d+)")
|
|
if rssi_raw then
|
|
gcs:send_named_float('LTE_RSSI', rssi_raw)
|
|
logger:write("LTE",'RSSI,BER,Bin,Bout','iiII',
|
|
rssi_raw,
|
|
ber_raw,
|
|
stats.bytes_in,
|
|
stats.bytes_out)
|
|
-- gcs:send_text(MAV_SEVERITY.INFO, string.format("RSSI:%d BER:%d", rssi_raw, ber_raw))
|
|
return
|
|
end
|
|
-- check for CSPI reply
|
|
-- example: +CPSI: LTE,Online,505-02,0xCBE8,36519691,101,EUTRAN-BAND3,1800,5,5,-147,-1143,-764,11
|
|
local system_mode, operation_mode, mcc_mnc, tac_str, scell_id_str, pcid_str, earfcn_band, ul_freq_str, dl_freq_str, tdd_cfg_str, rsrp_str, rsrq_str, rssi_str, sinr_str =
|
|
s:match("%+CPSI:%s*([^,]+),([^,]+),([^,]+),([^,]+),([^,]+),([^,]+),([^,]+),([^,]+),([^,]+),([^,]+),([%-]?%d+),([%-]?%d+),([%-]?%d+),([%-]?%d+)")
|
|
|
|
if system_mode then
|
|
-- Convert strings to numbers
|
|
local tac = tonumber(tac_str:match("0x(%w+)"), 16) or tonumber(tac_str) or 0
|
|
local scell_id = tonumber(scell_id_str) or 0
|
|
local pcid = tonumber(pcid_str) or 0
|
|
local ul_freq = tonumber(ul_freq_str) or 0
|
|
local dl_freq = tonumber(dl_freq_str) or 0
|
|
local tdd_cfg = tonumber(tdd_cfg_str) or 0
|
|
local rsrp = tonumber(rsrp_str) or 0
|
|
local rsrq = tonumber(rsrq_str) or 0
|
|
local rssi = tonumber(rssi_str) or 0
|
|
local sinr = tonumber(sinr_str) or 0
|
|
local band = earfcn_band:match("[^%d]+(%d+)") or -1
|
|
logger:write("LTES",'Md,Op,MCC,TAC,CID,PID,BND,F,DF,TDD,RP,RQ,RS,SR','nNNIIINHhhhhhh',
|
|
system_mode, operation_mode, mcc_mnc, tac, scell_id, pcid, earfcn_band,
|
|
ul_freq, dl_freq, tdd_cfg, rsrp, rsrq, rssi, sinr)
|
|
if option_enabled(LTE_OPTIONS_SIGNALS) then
|
|
gcs:send_named_float('LTE_RSRP', rsrp)
|
|
gcs:send_named_float('LTE_RSRQ', rsrq)
|
|
gcs:send_named_float('LTE_SINR', sinr)
|
|
gcs:send_named_float('LTE_BAND', band)
|
|
gcs:send_named_float('LTE_FREQ', ul_freq)
|
|
gcs:send_named_float('LTE_CID', scell_id)
|
|
end
|
|
return
|
|
end
|
|
|
|
if s:find("PPPD: DISCONNECTED") then
|
|
step = "PPPOPEN"
|
|
end
|
|
end
|
|
|
|
local last_CSQ_ms = millis()
|
|
local last_CSQ_reply_ms = uint32_t(0)
|
|
local last_parse_ms = uint32_t(0)
|
|
|
|
--[[
|
|
handle data while connected
|
|
--]]
|
|
local function step_CONNECTED()
|
|
local s = uart:readstring(512)
|
|
stats.bytes_in = stats.bytes_in + #s
|
|
if option_enabled(LTE_OPTIONS_LOGALL) then
|
|
log_data(s, '<<<')
|
|
end
|
|
if s and s:find('\r\nCLOSED\r\n') then
|
|
gcs:send_text(MAV_SEVERITY.INFO, 'LTE_modem: connection closed, reconnecting')
|
|
step = "CIPOPEN"
|
|
return
|
|
end
|
|
if s and s:find('PPPD: DISCONNECTED\r\n') then
|
|
gcs:send_text(MAV_SEVERITY.INFO, 'LTE_modem: PPP closed, reconnecting')
|
|
step = "PPPOPEN"
|
|
return
|
|
end
|
|
local now_ms = millis()
|
|
if s and #s > 0 then
|
|
pending_to_parse = pending_to_parse .. s
|
|
pending_to_parse = cmux.feed_uart_in(pending_to_parse)
|
|
if now_ms - last_parse_ms > 1000 then
|
|
pending_to_parse = ""
|
|
end
|
|
if #cmux.buffers[1] > 0 then
|
|
last_parse_ms = now_ms
|
|
--gcs:send_text(MAV_SEVERITY.INFO, string.format("AT reply %d", #cmux.buffers[1]))
|
|
handle_AT_reply(cmux.buffers[1])
|
|
cmux.buffers[1] = ""
|
|
end
|
|
if #cmux.buffers[2] > 0 then
|
|
last_data_ms = now_ms
|
|
-- gcs:send_text(MAV_SEVERITY.INFO, string.format("data input %d", #cmux.buffers[2]))
|
|
last_parse_ms = now_ms
|
|
pending_to_fc = pending_to_fc .. cmux.buffers[2]
|
|
cmux.buffers[2] = ""
|
|
end
|
|
elseif now_ms - last_data_ms > uint32_t(LTE_TIMEOUT:get() * 1000) then
|
|
gcs:send_text(MAV_SEVERITY.ERROR, 'LTE_modem: timeout')
|
|
step = "ATI"
|
|
return
|
|
end
|
|
s = ser_device:readstring(512)
|
|
if s then
|
|
pending_to_modem = pending_to_modem .. s
|
|
end
|
|
|
|
--[[
|
|
going via these pending buffers allows for rapid bursts of data and takes advantage
|
|
of the hardware flow control
|
|
--]]
|
|
local buffer_limit = 10240 -- so we don't run out of memory
|
|
if #pending_to_modem > buffer_limit then
|
|
pending_to_modem = ""
|
|
end
|
|
if #pending_to_fc > buffer_limit then
|
|
pending_to_fc = ""
|
|
end
|
|
|
|
while #pending_to_modem > 0 do
|
|
local n = #pending_to_modem
|
|
if n > 127 then
|
|
n = 127
|
|
end
|
|
-- gcs:send_text(MAV_SEVERITY.INFO, string.format("data output %d", n))
|
|
if not cmux_data_send_connected(pending_to_modem:sub(1, n)) then
|
|
break
|
|
end
|
|
pending_to_modem = pending_to_modem:sub(n + 1)
|
|
end
|
|
if #pending_to_fc > 0 then
|
|
local nwritten = ser_device:writestring(pending_to_fc)
|
|
if nwritten > 0 then
|
|
pending_to_fc = pending_to_fc:sub(nwritten + 1)
|
|
end
|
|
end
|
|
-- request CSQ signal strength at 1Hz
|
|
if now_ms - last_CSQ_ms > 1000 then
|
|
last_CSQ_ms = now_ms
|
|
cmux_AT_send("AT+CSQ\r\n")
|
|
cmux_AT_send("AT+CPSI?\r\n")
|
|
end
|
|
if now_ms - last_CSQ_reply_ms > 5000 then
|
|
last_CSQ_reply_ms = now_ms
|
|
gcs:send_named_float('LTE_RSSI', -1)
|
|
end
|
|
end
|
|
|
|
local step_count = 0
|
|
local last_step = nil
|
|
|
|
local function update()
|
|
if LTE_ENABLE:get() == 0 then
|
|
return update, 500
|
|
end
|
|
|
|
if change_baud then
|
|
uart:begin(change_baud)
|
|
change_baud = nil
|
|
end
|
|
|
|
if step == "CONNECTED" then
|
|
-- run the connected step at 200Hz
|
|
step_CONNECTED()
|
|
step_count = 0
|
|
return update, 5
|
|
end
|
|
|
|
-- prevent getting stuck
|
|
if step == last_step and step ~= "ATI" then
|
|
step_count = step_count + 1
|
|
if step_count > 50 then
|
|
gcs:send_text(MAV_SEVERITY.INFO, "LTE_modem: step reset")
|
|
step = "ATI"
|
|
end
|
|
else
|
|
step_count = 0
|
|
end
|
|
last_step = step
|
|
|
|
gcs:send_text(MAV_SEVERITY.INFO, string.format('LTE_modem: step %s', step))
|
|
|
|
if step == "ATI" then
|
|
step_ATI()
|
|
return update, 1100
|
|
end
|
|
|
|
if step == "BAUD" then
|
|
step_BAUD()
|
|
return update, 200
|
|
end
|
|
|
|
if step == "CREG" then
|
|
step_CREG()
|
|
return update, 500
|
|
end
|
|
|
|
if step == "CIPMODE" then
|
|
step_CIPMODE()
|
|
return update, 200
|
|
end
|
|
|
|
if step == "NETOPEN" then
|
|
step_NETOPEN()
|
|
return update, 200
|
|
end
|
|
|
|
if step == "CMUX" then
|
|
step_CMUX()
|
|
return update, 200
|
|
end
|
|
|
|
if step == "PPPOPEN" then
|
|
step_PPPOPEN()
|
|
return update, 200
|
|
end
|
|
|
|
if step == "CIPOPEN" then
|
|
step_CIPOPEN()
|
|
return update, 200
|
|
end
|
|
|
|
gcs:send_text(MAV_SEVERITY.ERROR, string.format("LTE_modem: bad step %s", step))
|
|
step = "ATI"
|
|
end
|
|
|
|
gcs:send_text(MAV_SEVERITY.INFO, 'LTE_modem: starting')
|
|
|
|
return update,500
|