Hello, I am trying to integrate a custom range finder with the orange cube+. This sensor uses CAN-FD protocol and I am trying to read the data using Lua script.
The example I am following is can_read.lua and benewakeH30_can_rangefinder.lua
I am able to read the data for couple of seconds but after that the messages stops showing up but if I reset the sensor, the data comes back for another couple of seconds.
The FDBitrate I am using is 8M and I don;t see any errors on the messages section.
Could someone please guide me into solving this problem.
here the script that I am using.
-- This script is an example of reading from the CAN bus
-- local default params
local update_rate_ms = 1
-- 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(25)
type = 0
length = 0
if not driver1 then
gcs:send_text(0, "No scripting CAN interfaces found")
return
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 driver1 then
local frame = driver1:read_frame()
if not frame then
return
end
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 < 64 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
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
-- 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()