mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-02-04 12:25:35 -06:00
128 lines
3.8 KiB
Lua
128 lines
3.8 KiB
Lua
-- Script decodes and logs wind sensor data for Trisonica LI-550 mini
|
|
-- https://www.licor.com/products/trisonica/LI-550-mini
|
|
|
|
-- Parameters:
|
|
-- SCR_ENABLE 1
|
|
-- SERIAL5_PROTOCOL 28
|
|
|
|
-- In SITL, you can enable serial ports to connect to the real device.
|
|
-- https://ardupilot.org/dev/docs/learning-ardupilot-uarts-and-the-console.html#the-8-uarts
|
|
-- ./Tools/autotest/sim_vehicle.py -v Plane --console --map -A "--serial5=uart:/dev/ttyUSB0" -D
|
|
|
|
-- Remember to change baud to 230k in the sensor setup and enable the fields you want.
|
|
-- Also, enable 10Hz output instead of the default 5Hz.
|
|
|
|
-- Example data string (excluding quotes, excluding the carriage return line feed ending)
|
|
-- "S 00.08 S2 00.07 D 245 DV 033 U 00.06 V 00.03 W 00.05 T 55889220.00 C 346.68 H 17.92 DP 03.68 P -099.70 AD 0.0000000 AX -2913 AY -3408 AZ -16600 PI 011.4 RO 009.8 MX -619 MY 845 MZ 782 MD 337 TD 337"
|
|
|
|
-- Log severities
|
|
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
|
|
|
|
-- find the serial first (0) scripting serial port instance
|
|
local port = serial:find_serial(0)
|
|
|
|
if not port then
|
|
gcs:send_text(MAV_SEVERITY.EMERGENCY, "no trisonica scripting port")
|
|
return
|
|
end
|
|
|
|
-- begin the serial port
|
|
port:begin(230400)
|
|
port:set_flow_control(0)
|
|
|
|
local function parse_wind_data(buf)
|
|
-- Split the string up into key and values splitting on the default space delimiter.
|
|
|
|
local parsed_values = {}
|
|
|
|
-- Match any key-value pair where key is a string and value is a number
|
|
for key, value in buf:gmatch("(%a+)%s*([%-%.%d]+)") do
|
|
parsed_values[key] = tonumber(value) -- Store key-value pair in the table
|
|
end
|
|
|
|
return parsed_values
|
|
end
|
|
|
|
local tag_ids = nil
|
|
local tag_id_str = ""
|
|
local value_format = ""
|
|
local last_keys = nil
|
|
|
|
local function log_wind_data(parsed)
|
|
-- Given a table of parsed data, log it.
|
|
|
|
-- Collect keys from parsed and store as a list
|
|
local current_keys = {}
|
|
for k in pairs(parsed) do
|
|
table.insert(current_keys, k)
|
|
end
|
|
table.sort(current_keys)
|
|
|
|
-- First packet, we must set last_keys.
|
|
if not last_keys then
|
|
last_keys = current_keys
|
|
return
|
|
end
|
|
|
|
-- Bail early if the first packet parsed only contained some of the keys.
|
|
-- After the 2nd packet, it will stabilize.
|
|
if #current_keys ~= #last_keys then
|
|
last_keys = current_keys
|
|
return
|
|
end
|
|
|
|
for i = 1, #current_keys do
|
|
if current_keys[i] ~= last_keys[i] then
|
|
last_keys = current_keys
|
|
return
|
|
end
|
|
end
|
|
|
|
-- Keys are now stable, so store them as tag_ids
|
|
if not tag_ids then
|
|
tag_ids = current_keys
|
|
tag_id_str = table.concat(tag_ids, ',')
|
|
value_format = string.rep('f', #tag_ids)
|
|
gcs:send_text(MAV_SEVERITY.INFO, "Using tag_ids: " .. tag_id_str)
|
|
end
|
|
|
|
|
|
-- Build ordered values from tag_ids
|
|
local values = {}
|
|
for _, tag_id in ipairs(tag_ids) do
|
|
table.insert(values, parsed[tag_id] or 0)
|
|
end
|
|
|
|
assert(#tag_ids < 15, "#tag_ids=" .. #tag_ids)
|
|
logger:write('W3D', tag_id_str, value_format, table.unpack(values))
|
|
end
|
|
|
|
local buffer = ""
|
|
-- the main update function that is used to read in data from serial port
|
|
local function update()
|
|
|
|
|
|
local n_bytes = port:available()
|
|
while n_bytes > 0 do
|
|
local byte = port:read()
|
|
|
|
if byte > -1 then
|
|
local c = string.char(byte)
|
|
buffer = buffer .. c
|
|
|
|
--ignore the \n because \r is sufficient to end parsing.
|
|
if c == "\r" then
|
|
local result = parse_wind_data(buffer)
|
|
log_wind_data(result)
|
|
buffer = ""
|
|
end
|
|
end
|
|
n_bytes = n_bytes - 1
|
|
end
|
|
return update, 100
|
|
end
|
|
|
|
gcs:send_text(MAV_SEVERITY.INFO, "trisonica-mini.lua running...")
|
|
|
|
return update, 100
|