I am trying to perform obstacle avoidance with a custom proximity sensor which transmits data over CAN.
I am using lua script to read data and sending a vector_3f object to proximity backend.
The proximity status shows Green but the proximity viewer shows nothing.
I’ve set AVOID_ENABLE to 3
OA_TYPE is Dijkstra’s
PRX1_TYPE is set to Scripting
Here is the script I am using:
-- This script is an example of reading from the CAN bus
-- local default params
local update_rate_ms = 1
local param_num_lua_prx_backend = 15
local lua_prx_backend
local lua_prx_driver_found = false
-- Load CAN driver1. The first will attach to a protocol of 10, the 2nd to a protocol of 12
-- this allows the script to distinguish packets on two CAN interfaces
local driver1 = CAN:get_device(5)
type = 0
length = 0
-- if not driver1 then
-- gcs:send_text(0, "No scripting CAN interfaces found")
-- return
-- end
function setup_prx_sensor()
if not driver1 then
gcs:send_text(0,"No scripting CAN interfaces found")
return
end
local sensor_count = proximity:num_sensors() -- number of sensors connected
gcs:send_text(0,string.format("found %d sensors", sensor_count))
for j = 0, sensor_count -1 do
local device = proximity:get_backend(j)
if ((not lua_prx_driver_found) and device and (device:type() == param_num_lua_prx_backend)) then
-- this is a lua driver
lua_prx_driver_found = true
lua_prx_backend = device
end
end
if not lua_prx_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 prx Sensor"))
return
end
end
function show_frame(dnum, frame)
gcs:send_text(0, string.format("CAN[%u] msg from %i", dnum, frame:id_signed()))
end
-- Function to convert a 32-bit unsigned integer to a floating-point number
function uint32_to_float(uint)
-- Extracting sign bit, exponent bits, and mantissa bits
local sign = (uint >> 31) == 0 and 1 or -1
local exponent = (uint >> 23) & 0xFF
local mantissa = uint & 0x7FFFFF
-- Handling special cases
if exponent == 0 and mantissa == 0 then
return 0.0
elseif exponent == 0 and mantissa ~= 0 then
return sign * math.ldexp(mantissa / 0x800000, -126)
elseif exponent == 0xFF and mantissa == 0 then
return sign * math.huge
elseif exponent == 0xFF and mantissa ~= 0 then
return 0.0 / 0.0
end
-- Computing the exponent (bias subtracted from the exponent bits)
exponent = exponent - 127
-- Computing the mantissa (including the implicit leading bit)
local fraction = 1.0 + mantissa / 0x800000
-- Constructing the floating-point number
return sign * fraction * (2 ^ exponent)
end
function extract_and_convert(frame, index)
return uint32_to_float((frame:data(index + 3) << 24) | (frame:data(index + 2) << 16) | (frame:data(index + 1) << 8) |
frame:data(index))
end
function update()
-- see if we got any frames
if not lua_prx_driver_found then
setup_prx_sensor()
end
if driver1 then
local frame = driver1:read_frame()
if frame then
if frame:id_signed() == 0 then
type = frame:data(3) * 256 ^ 3 + frame:data(2) * 256 ^ 2 + frame:data(1) * 256 + frame:data(0)
length = frame:data(7) * 256 ^ 7 + frame:data(6) * 256 ^ 6 + frame:data(5) * 256 + frame:data(4)
elseif frame:id_signed() == 209 and length > 0 then
local index = 0
while index < 8 do
-- x = uint32_to_float((frame:data(3) << 24) | (frame:data(2) << 16) | (frame:data(1) << 8) | frame:data(0))
-- y = uint32_to_float((frame:data(7) << 24) | (frame:data(6) << 16) | (frame:data(5) << 8) | frame:data(4))
-- z = uint32_to_float((frame:data(11) << 24) | (frame:data(10) << 16) | (frame:data(9) << 8) | frame:data(8))
-- velocity = uint32_to_float((frame:data(15) << 24) | (frame:data(14) << 16) | (frame:data(13) << 8) | frame:data(12))
-- Extracting and converting x, y, z, and velocity from the frame
local x = extract_and_convert(frame, index)
local y = extract_and_convert(frame, index + 4)
local z = extract_and_convert(frame, index + 8)
local velocity = extract_and_convert(frame, index + 12)
index = index + 16
if not (x == 0.00 and y == 0.00 and z == 0.00 and velocity == 0.00) then
local point = Vector3f()
point:x(x)
point:y(y)
point:z(z)
lua_prx_backend:handle_script_3d_msg(point, true)
gcs:send_text(0, string.format("Point %d: X=%.2f, Y=%.2f, Z=%.2f, Velocity=%.2f", 0, x, y, z, velocity))
end
end
end
end
end
return update, 2
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()