ardupilot/libraries/AP_Scripting/examples/benewakeH30_can_rangefinder...

107 lines
3.0 KiB
Lua

-- Lua Can Driver for Benewake CAN Rangefinder
-- User settable parameters
local update_rate_ms = 10 -- update rate (in ms) of the driver
local debug_enable = false -- true to enable debug messages
-- Global variables (DO NOT CHANGE)
local param_num_lua_rfnd_backend = 36 -- parameter number for lua rangefinder
local lua_rfnd_backend -- store lua backend here
local lua_rfnd_driver_found = false -- true if user has configured lua backend
local can_driver = CAN:get_device(5)
gcs:send_text(0,"Scripting started")
-- -------------------------------- RFND DRIVER --------------------------------
function setup_rfnd_sensor()
if not can_driver then
gcs:send_text(0,"No scripting CAN interfaces found")
return
end
local sensor_count = rangefinder:num_sensors() -- number of sensors connected
for j = 0, sensor_count -1 do
local device = rangefinder:get_backend(j)
if ((not lua_rfnd_driver_found) and device and (device:type() == param_num_lua_rfnd_backend)) then
-- this is a lua driver
lua_rfnd_driver_found = true
lua_rfnd_backend = device
end
end
if not lua_rfnd_driver_found then
-- We can't use this script if user hasn't setup a lua backend
gcs:send_text(0, string.format("Configure Lua RFND Sensor"))
return
end
end
function show_rfnd_frame(frame_rfnd)
gcs:send_text(0,string.format("RFND msg from " .. tostring(frame_rfnd:id())))
end
function parse_rfnd_can_frame(frame_rfnd)
local height_cm = (frame_rfnd:data(0)*256 + frame_rfnd:data(1))
return height_cm*0.01
end
function handle_rfnd_frame(frame_rfnd)
if debug_enable then
show_rfnd_frame(frame_rfnd)
end
rfnd_dist = parse_rfnd_can_frame(frame_rfnd)
if (rfnd_dist > 0) then
local sent_successfully = lua_rfnd_backend:handle_script_msg(rfnd_dist)
if not sent_successfully then
-- This should never happen as we already checked for a valid configured lua backend above
gcs:send_text(0, string.format("RFND Lua Script Error"))
return
end
end
end
-- -------------------------------- MAIN --------------------------------
function update()
if not lua_rfnd_driver_found then
setup_rfnd_sensor()
end
if (not lua_rfnd_driver_found) then
-- We can't use this script if user hasn't setup a lua backend
return
end
frame = can_driver:read_frame()
if not frame then
-- no frame to parse
return
end
handle_rfnd_frame(frame)
end
-- wrapper around update(). This calls update() and if update faults
-- then an error is displayed, but the script is not stopped
function protected_wrapper()
local success, err = pcall(update)
if not success then
gcs:send_text(MAV_SEVERITY_ERROR, "Internal Error: " .. err)
-- when we fault we run the update function again after 1s, slowing it
-- down a bit so we don't flood the console with errors
return protected_wrapper, 1000
end
return protected_wrapper, update_rate_ms
end
-- start running update loop
return protected_wrapper()