mirror of https://github.com/ArduPilot/ardupilot
107 lines
3.0 KiB
Lua
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()
|