From 72bfdff604f309e960eb5699fb30a84f730715a7 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Fri, 27 Sep 2024 18:26:30 +0100
Subject: [PATCH 1/2] Generator: Lua: add support for checksum on decode

---
 generator/mavgen_lua.py | 89 ++++++++++++++++++++++++++++++++++-------
 1 file changed, 75 insertions(+), 14 deletions(-)

diff --git a/generator/mavgen_lua.py b/generator/mavgen_lua.py
index d563de8b5..459fad9af 100644
--- a/generator/mavgen_lua.py
+++ b/generator/mavgen_lua.py
@@ -53,6 +53,7 @@ def generate(basename, xml):
         mavlink_msg_file = open("%s/mavlink_msg_%s.lua" % (basename, m.name), "w")
         mavlink_msg_file.write("local %s = {}\n" % m.name)
         mavlink_msg_file.write("%s.id = %u\n" % (m.name, m.id))
+        mavlink_msg_file.write("%s.crc_extra = %u\n" % (m.name, m.crc_extra))
         mavlink_msg_file.write("%s.fields = {\n" % m.name)
         for i in range(0, len(m.ordered_fields)):
             field = m.ordered_fields[i]
@@ -70,6 +71,9 @@ def generate(basename, xml):
 """-- 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("{module_root_rel}mavlink_msg_" .. msgname)
   if not message_map then
@@ -78,45 +82,99 @@ def generate(basename, xml):
   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("{module_root_rel}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]] = {{}}
@@ -133,11 +191,14 @@ def generate(basename, xml):
     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("{module_root_rel}mavlink_msg_" .. msgname)
   if not message_map then

From 70c0e22bb928b97890ad90bd6460e77f08eca2e5 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Fri, 27 Sep 2024 18:40:28 +0100
Subject: [PATCH 2/2] Generator: Lua: swap nul character `\0` for
 `string.char(0)`

---
 generator/mavgen_lua.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/generator/mavgen_lua.py b/generator/mavgen_lua.py
index 459fad9af..4c02ca2c9 100644
--- a/generator/mavgen_lua.py
+++ b/generator/mavgen_lua.py
@@ -186,7 +186,7 @@ def generate(basename, xml):
       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]], "\0", "")
+        result[v[1]] = string.gsub(result[v[1]], string.char(0), "")
       end
     end
   end