I made an electronic circuit what measures several parameters of combustion motor and generate standard MAVLink2 frame EFI_STATUS with measured data. I wanted to display those data in “Quick” panel of Mission Planner. On this forum I get suggestion to write LUA script to decode MAVLink2 data. I did it and finally have script what decodes data and creates variables what can be displayed in Quick, but stil have 2 problems:
-
On empty Pixhawk it works OK, but on other device with some other configuration I get message: “insufficient memory loading APM/scripts/Manager_Silnika.lua”. On my empty pixhawk with running script I have Status → fremem = 497000. When delete script have Status → freemem = 497736. It looks the script use 736 bytes. On pixhawk with “oher configuration” I can read Status → freemem = 528216. It is even more, but script do not run. Don’t understand why?
-
I can connect my device to TELEM1, TELEM2 or GPS2 port. In Mission Planner I configure port protocol to “Scripting”. In my script I create variable “port” and assign to it handler(?) to one of the ports for example:
local port = serial:find_serial(1)
I had problem with opening port, but finally discover when few other ports is configured to “scripting” protocol, it works. It is OK for empty device, but when other devices are connected to other ports it become a problem. I get information the port what I can use is GPS2 so tried several configuration to find configuration what will work. Finally found 2 configurations, but one require “scripting” has to be set on TELEM2, Serial3 and Serial4. Other working configuration is as above + Serial5. Below is table with most of possible configurations for GPS2 port. In script I open serial_port 2
I can get 3 kind of behaviors described in legend. Port can’t be opened, can be opened but script don’t work, or all works OK (green colour)
To understand it I did some more work. In script I was able to open port with numbers 0, 1 and 2 (probably TELEM1, TELEM2 and GPS). Just did test with opening port 1 when device is connected to one of 3 possible ports and try to open all 3 ports with script by simple plugging plug into another socket and reset pixhawk. Results are below:
Completely don’t understand this logic. It must be a bug but where? In my script or somewhere other?
Include my script:
--[[
Skrypt LUA dekodujący ramkę MAVLink2 przychodzącą z managera silnika.
Zdekodowane dane są deklarowane w Mission Planner jako zmienne użytkownika i są wyswietlane
z przedrostkiem MAV_ w oknie gdzie wybiera się źródło danych dla panelu Quick
w Quick trzeba kliklnąć w jedną z liczb, wtedy otwiera się powyższe okno z naszymi danymi.
Ustawienie:
Wybieramy wolny port UART w pixhawku np. Telem2
1) CONFIG -> Standard Param -> Telemetry 2 Baud Rate = 57600
2) CONFIG -> Standard Param -> Telemetry 2 protocol selection = Scripting (28)
(c) Piotr Laskowski
Opracowano na bazie kodu napisanego przez Stephen Dade (stephen_dade@hotmail.com)
https://ardupilot.org/plane/docs/common-serial-options.html !Uwaga - numery portów są blędne
Serial 0 USB port
Serial 1 Telemetry port 1
Serial 2 Telemetry port 2
Serial 3 GPS1 port
Serial 4 GPS2 port
Serial 5 USER port
Serial 6 USER port
Serial 7 USER port
--]]
gcs:send_text(0, "Start MAVLink LUA script")
--local port = serial:find_serial(0) -- 0=Telem1
local port = serial:find_serial(1) -- 1=Telem2
--local port = serial:find_serial(2) -- 2=GPS2
--local port = serial:find_serial(3) -- 3=GPS1 (sprawdzić)
if not port == 0 then
gcs:send_text(0, "No Scriping Serial Port: " .. tostring(port))
return
end
port:begin(57600)
port:set_flow_control(0)
local function MAVLinkProcessor()
-- public fields
local self = {
-- define MAVLink message id's
EFI_STATUS = 225
}
-- private fields
local _mavbuffer = "" -- bufer for incoming data
local _mavresult = {} -- buffer for parts of frame body
local _payload_len = 0
local _mavdecodestate = 0 -- 0=looking for marker, 1=getting header,2=getting payload,3=getting crc
PROTOCOL_MARKER_V2 = 0xFD
HEADER_LEN_V2 = 10
EFI_FRAME_LEN = 73
--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
local _messages = {}
_messages[225] = { -- EFI_STATUS
{"ecu_index", "<f"}, {"rpm", "<f"}, {"fuel_consumed", "<f"}, {"fuel_flow", "<f"}, {"engine_load", "<f"}, {"throttle_position", "<f"},
{"spark_dwell_time", "<f"}, {"barometric_pressure", "<f"}, {"intake_manifold_pressure", "<f"}, {"intake_manifold_temperature", "<f"},
{"cylinder_head_temperature", "<f"}, {"ignition_timing", "<f"}, {"injection_time", "<f"}, {"exhaust_gas_temperature", "<f"},
{"throttle_out", "<f"}, {"pt_compensation", "<f"}, {"health", "<B"}, {"ignition_voltage", "<f"}, {"fuel_pressure", "<f"}
}
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
--------------------------------------------------------------------------------
-- parse a new byte and see if we've got MAVLink 2 message
-- returns true if a packet was decoded, false otherwise
function self.parseMAVLink(byte)
_mavbuffer = _mavbuffer .. string.char(byte)
--gcs:send_text(0, "ds:" .. tostring(_mavdecodestate))
--gcs:send_text(0, "mbuf size: " .. tostring(#_mavbuffer) .. "ds: " .. tostring(_mavdecodestate))
-- parse buffer to find MAVLink packets
--if #_mavbuffer == 1 and string.byte(_mavbuffer, 1) == PROTOCOL_MARKER_V2 and _mavdecodestate == 0 then
if _mavdecodestate == 0 then
if #_mavbuffer == 1 and string.byte(_mavbuffer, 1) == PROTOCOL_MARKER_V2 then
_mavdecodestate = 1
--gcs:send_text(0, "Header")
return false
else
_mavbuffer = ""
end
end
-- if we have a full header, try parsing
if #_mavbuffer == HEADER_LEN_V2 and _mavdecodestate == 1 then
-- wartosc, reszta = string.unpack("format", string, pozycja=1)
_payload_len, _ = string.unpack("<B", _mavbuffer, 2)
_mavresult.seq, _mavresult.sysid, _mavresult.compid, _ = string.unpack("<BBB", _mavbuffer, 5)
_mavresult.msgid, _ = string.unpack("I3", _mavbuffer, 8)
--gcs:send_text(0, "sys:" .. tostring(_mavresult.sysid) ..", comp:" .. tostring(_mavresult.compid) ..", msgid:" .. tostring(_mavresult.msgid) ..", seq:" .. tostring(_mavresult.seq))
_mavdecodestate = 2
return false
end
-- get payload
if _mavdecodestate == 2 and #_mavbuffer == (_payload_len + HEADER_LEN_V2) then
_mavdecodestate = 3
_mavresult.payload = string.sub(_mavbuffer, HEADER_LEN_V2 + 1)
--gcs:send_text(0, "pay: " .. tostring(_payload_len) ..", len: " .. tostring(#_mavresult.payload))
return false
end
-- get crc, then process if CRC ok
if _mavdecodestate == 3 and #_mavbuffer == (_payload_len + HEADER_LEN_V2 + 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
-- ignoruj ramki EFI_STATUS o rozmiarze innym niż domyślny
if _mavresult.msgid == 225 and _payload_len ~= EFI_FRAME_LEN then
_mavbuffer = ""
gcs:send_text(3, "Wrong frame len")
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, "Bad 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
_mavresult[v[1]], offset = string.unpack(v[2], _mavresult.payload, offset)
end
_mavbuffer = ""
gcs:send_text(0, "Decoded")
gcs:send_named_float("Head1_temp", _mavresult.cylinder_head_temperature)
gcs:send_named_float("Head2_temp", _mavresult.exhaust_gas_temperature)
gcs:send_named_float("Local_temp", _mavresult.ignition_timing)
gcs:send_named_float("Motor_RPM", _mavresult.rpm)
gcs:send_named_float("Fuel_Lev", _mavresult.fuel_consumed)
gcs:send_named_float("Health", _mavresult.health)
gcs:send_named_float("Throttle", _mavresult.throttle_out)
gcs:send_named_float("Ign_volt", _mavresult.ignition_voltage)
--zmienne debugujące
gcs:send_named_float("Cap_sens1", _mavresult.spark_dwell_time)
gcs:send_named_float("Cap_sens2", _mavresult.barometric_pressure)
gcs:send_named_float("Cap_sens3", _mavresult.intake_manifold_pressure)
gcs:send_named_float("Cap_sens4", _mavresult.intake_manifold_temperature)
gcs:send_named_float("Test", _mavresult.fuel_pressure)
gcs:send_named_float("Fuel_Min", _mavresult.engine_load)
gcs:send_named_float("Fuel_Max", _mavresult.throttle_position)
return true
end
-- packet too big ... start again
if #_mavbuffer > 263 then
_mavbuffer = ""
gcs:send_text(0, "To big:" .. tostring(_mavdecodestate))
_mavdecodestate = 0
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
-- return the instance
return self
end
-- Define the MAVLink processor
local mavlink = MAVLinkProcessor()
function HLSatcom()
-- read in any bytes from UART and and send to MAVLink processor
-- only read in 1 packet at a time to avoid time overruns
while port:available() > 0 do
-- local byte = port:read()
-- if mavlink.parseMAVLink(byte) then break end
if mavlink.parseMAVLink(port:read()) then
break
end
end
return HLSatcom, 100
end
return HLSatcom, 100