Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

scripts: add extra folder with named_value_float_echo.lua #13

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
55 changes: 55 additions & 0 deletions scripts/ardupilot/extra/named_value_float_echo.lua
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
local NAMED_VALUE_FLOAT = {}
NAMED_VALUE_FLOAT.id = 251
NAMED_VALUE_FLOAT.fields = {
{ "time_boot_ms", "<I4" },
{ "value", "<f" },
{ "name", "<c10" },
}

local function decode_named_value_float(message)
local result = {}
local offset = 2 -- Start from the second byte

for _, field in ipairs(NAMED_VALUE_FLOAT.fields) do
local value
local success, err = pcall(function()
value, offset = string.unpack(field[2], message, offset)
end)
if not success then
return nil
end
result[field[1]] = value
end
return result
end

local function bytes_to_string(str)
return str:match("^%z*(.-)%z*$") -- Remove leading and trailing null bytes
end

local function hex_dump(str)
return (str:gsub('.', function (c) return string.format('%02X ', string.byte(c)) end))
end

mavlink.init(1, 10)
mavlink.register_rx_msgid(NAMED_VALUE_FLOAT.id)

function update()
local msg, _, timestamp_ms = mavlink.receive_chan()
if msg then

local header = msg:sub(1, 10) -- MAVLink v2 header is 10 bytes
local payload = msg:sub(12) -- Payload starts at 11th byte for MAVLink v2

local decoded_msg = decode_named_value_float(payload)
if decoded_msg then
local name = bytes_to_string(decoded_msg.name)
local value = decoded_msg.value
--gcs:send_text(6, string.format("Received NAMED_VALUE_FLOAT: %s = %f", name, value))
gcs:send_named_float(name, value)
end
end
return update, 100
end

return update()
Loading