Skip to content

Commit

Permalink
AP_Scripting: modules: MAVLink: update to latest version of generated…
Browse files Browse the repository at this point in the history
… code
  • Loading branch information
IamPete1 authored and tridge committed Oct 1, 2024
1 parent 981c500 commit 902f1e6
Show file tree
Hide file tree
Showing 2 changed files with 81 additions and 14 deletions.
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
local COMMAND_LONG = {}
COMMAND_LONG.id = 76
COMMAND_LONG.crc_extra = 152
COMMAND_LONG.fields = {
{ "param1", "<f" },
{ "param2", "<f" },
Expand Down
94 changes: 80 additions & 14 deletions libraries/AP_Scripting/modules/MAVLink/mavlink_msgs.lua
Original file line number Diff line number Diff line change
@@ -1,5 +1,9 @@
-- Auto generated MAVLink parsing script
local mavlink_msgs = {}

---Lookup the message id for a given message name
---@param msgname string
---@return integer -- message id
function mavlink_msgs.get_msgid(msgname)
local message_map = require("MAVLink/mavlink_msg_" .. msgname)
if not message_map then
Expand All @@ -8,45 +12,99 @@ function mavlink_msgs.get_msgid(msgname)
return message_map.id
end

---Return a object containing everything that is not the payload
---@param message any -- encoded message
---@return table
function mavlink_msgs.decode_header(message)
-- build up a map of the result
local result = {}

local read_marker = 3
result.checksum = string.unpack("<H", message, 1)

-- id the MAVLink version
result.protocol_version, read_marker = string.unpack("<B", message, read_marker)
if (result.protocol_version == 0xFE) then -- mavlink 1
local magic = string.unpack("<B", message, 3)
if (magic == 0xFE) then -- mavlink 1
result.protocol_version = 1
elseif (result.protocol_version == 0XFD) then --mavlink 2
elseif (magic == 0XFD) then --mavlink 2
result.protocol_version = 2
else
error("Invalid magic byte")
end

_, read_marker = string.unpack("<B", message, read_marker) -- payload is always the second byte
-- fetch payload length
result.payload_length = string.unpack("<B", message, 4)

-- strip the incompat/compat flags
result.incompat_flags, result.compat_flags, read_marker = string.unpack("<BB", message, read_marker)
-- fetch the incompat/compat flags
result.incompat_flags, result.compat_flags = string.unpack("<BB", message, 5)

-- fetch seq/sysid/compid
result.seq, result.sysid, result.compid, read_marker = string.unpack("<BBB", message, read_marker)
result.seq, result.sysid, result.compid = string.unpack("<BBB", message, 7)

-- fetch the message id
result.msgid, read_marker = string.unpack("<I3", message, read_marker)
result.msgid = string.unpack("<I3", message, 10)

return result, read_marker
return result
end

-- generate the x25crc for a given buffer
---@param buffer string -- buffer to crc
---@return integer -- resulting crc 0 to 0xFFFF
function mavlink_msgs.generateCRC(buffer)
-- generate the x25crc for a given buffer.
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 crc
end

-- Note that this does not parse the serial data, it parses the MAVLink 2 C structure `mavlink_message_t`
-- This structure is passed in by the ArduPilot bindings as a string
---@param message any -- encoded message
---@param msg_map table -- table containing message objects with keys of the message ID
---@return table|nil -- a table representing the contents of the message, or nill if decode failed
function mavlink_msgs.decode(message, msg_map)
local result, offset = mavlink_msgs.decode_header(message)
local result = mavlink_msgs.decode_header(message)
local message_map = require("MAVLink/mavlink_msg_" .. msg_map[result.msgid])
if not message_map then
-- we don't know how to decode this message, bail on it
return nil
end

-- If we have a crc extra for this message then check it
-- This ensures compatibility with message definitions generated before the crc check was added
if message_map.crc_extra then
-- crc of payload and header values
local crc_buffer
if result.protocol_version == 2 then
crc_buffer = string.sub(message, 4, 12 + result.payload_length)

else
-- V1 does not include all fields on the wire
crc_buffer = string.char(result.payload_length)
crc_buffer = crc_buffer .. string.char(result.seq)
crc_buffer = crc_buffer .. string.char(result.sysid)
crc_buffer = crc_buffer .. string.char(result.compid)
crc_buffer = crc_buffer .. string.char(result.msgid)
if result.payload_length > 0 then
crc_buffer = crc_buffer .. string.sub(message, 13, 12 + result.payload_length)
end

end

local crc = mavlink_msgs.generateCRC(crc_buffer .. string.char(message_map.crc_extra))

if crc ~= result.checksum then
-- crc failed
return nil
end
end

-- map all the fields out
local offset = 13
for _,v in ipairs(message_map.fields) do
if v[3] then
result[v[1]] = {}
Expand All @@ -55,14 +113,22 @@ function mavlink_msgs.decode(message, msg_map)
end
else
result[v[1]], offset = string.unpack(v[2], message, offset)
if string.sub(v[2],2,2) == 'c' then
-- Got string, unpack includes 0 values to the set length
-- this is annoying, so remove them
result[v[1]] = string.gsub(result[v[1]], string.char(0), "")
end
end
end

-- ignore the idea of a checksum

return result;
return result
end

---Encode the payload section of a given message
---@param msgname string -- name of message to encode
---@param message table -- table containing key value pairs representing the data fields in the message
---@return integer -- message id
---@return string -- encoded payload
function mavlink_msgs.encode(msgname, message)
local message_map = require("MAVLink/mavlink_msg_" .. msgname)
if not message_map then
Expand Down

0 comments on commit 902f1e6

Please sign in to comment.