mirror of https://github.com/ArduPilot/ardupilot
253 lines
9.6 KiB
Lua
253 lines
9.6 KiB
Lua
--[[
|
|
Upto 3 CAN devices supported in this script although its easy to extend.
|
|
--]]
|
|
|
|
local sensor_no = 1 -- Sensor ID. Upload a copy of this script to the flight controller with this variable changed if you would like to use multiple of these sensors as serial. Switching to CAN highly recommended in that case
|
|
local update_rate_ms = 10 -- update rate (in ms) of the driver. 10ms was found to be appropriate
|
|
local bytes_to_parse = 100 -- serial bytes to parse in one interation of lua script. Reduce this if script is not able to complete in time
|
|
local debug_enable = false -- helpgul debug GCS prints
|
|
|
|
-- Global variables (DO NOT CHANGE)
|
|
local NOOPLOOP_FRAME_HEADER = 0x57
|
|
local NOOPLOOP_FRAME_HEADER_1 = 0x01
|
|
local param_num_lua_driver_backend = 36 -- parameter number for lua rangefinder
|
|
local param_num_lua_prx_backend = 15 -- parameter number for lua proximity
|
|
local lua_driver_backend -- store lua backend here
|
|
local sensor_driver_found = false -- true if user has configured lua backend
|
|
local num_pixels = 16 -- automatically updated if 64. User can select between 16/64 from NAssistant software
|
|
local total_bytes_to_expect = 112 -- total bytes in one complete packet
|
|
local linebuf = {} -- serial buffer
|
|
local linebuf_len = 0
|
|
local index_row_max = 8
|
|
|
|
local PARAM_TABLE_KEY = 109 + sensor_no
|
|
local PARAM_TABLE_PREFIX = string.format("TOFSENSE_S" .. sensor_no.. "_")
|
|
|
|
-- bind a parameter to a variable
|
|
function bind_param(name)
|
|
local p = Parameter()
|
|
assert(p:init(name), string.format('could not find %s parameter', name))
|
|
return p
|
|
end
|
|
|
|
-- add a parameter and bind it to a variable
|
|
function bind_add_param(name, idx, default_value)
|
|
assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
|
|
return bind_param(PARAM_TABLE_PREFIX .. name)
|
|
end
|
|
|
|
-- setup parameters
|
|
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), 'could not add param table')
|
|
|
|
--[[
|
|
// @Param: TOFSENSE_S1_PRX
|
|
// @DisplayName: TOFSENSE-M to be used as Proximity sensor
|
|
// @Description: Set 0 if sensor is to be used as a 1-D rangefinder (minimum of all distances will be sent, typically used for height detection). Set 1 if it should be used as a 3-D proximity device (Eg. Obstacle Avoidance)
|
|
// @Values: 0:Set as Rangefinder, 1:Set as Proximity sensor
|
|
// @User: Standard
|
|
--]]
|
|
SET_PRX = bind_add_param('PRX', 1, 0)
|
|
|
|
--[[
|
|
// @Param: TOFSENSE_S1_SP
|
|
// @DisplayName: TOFSENSE-M serial port config
|
|
// @Description: UART instance sensor is connected to. Set 1 if sensor is connected to the port with fist SERIALx_PROTOCOL = 28.
|
|
// @Range: 1 4
|
|
// @User: Standard
|
|
--]]
|
|
SERIAL_PORT = bind_add_param('SP', 2, 1)
|
|
|
|
--[[
|
|
// @Param: TOFSENSE_S1_BR
|
|
// @DisplayName: TOFSENSE-M serial port baudrate
|
|
// @Description: Serial Port baud rate. Sensor baud rate can be changed from Nassistant software
|
|
// @User: Standard
|
|
--]]
|
|
SERIAL_BAUD = bind_add_param('BR', 3, 230400)
|
|
|
|
-- find the serial scripting serial port instance. 0 indexed.
|
|
-- SERIALx_PROTOCOL 28
|
|
local port = assert(serial:find_serial(SERIAL_PORT:get() - 1),"Could not find Scripting Serial Port")
|
|
|
|
-- begin the serial port
|
|
port:begin(SERIAL_BAUD:get())
|
|
-- port:set_flow_control(0)
|
|
|
|
function setup_sensor(sensor, param_num)
|
|
local sensor_count = sensor:num_sensors() -- number of sensors connected
|
|
for j = 0, sensor_count -1 do
|
|
local device = sensor:get_backend(j)
|
|
if ((not sensor_driver_found) and device and (device:type() == param_num)) then
|
|
-- this is a lua driver
|
|
sensor_driver_found = true
|
|
lua_driver_backend = device
|
|
end
|
|
end
|
|
if not sensor_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 Sensor"))
|
|
return
|
|
end
|
|
end
|
|
|
|
-- get yaw and pitch of the pixel based message index.
|
|
function convert_to_angle(index)
|
|
-- The distances are sent in either a 4x4 or 8x8 grid. The horizontal and vertical FOV are 45 degrees so we can work out the angles
|
|
local angle_division = 45/index_row_max
|
|
local horizontal_index = (index) % index_row_max
|
|
local vertical_index = math.floor(index / index_row_max)
|
|
local yaw = -22.5 + (horizontal_index*angle_division)
|
|
local pitch = -22.5 + (vertical_index*angle_division)
|
|
return yaw, pitch
|
|
end
|
|
|
|
-- send the message down to proximity library. This needs to be a 3D vector. User of this function needs to ensure prx backend exists
|
|
function sent_prx_message(dist, yaw_deg, pitch_deg, push_to_boundary)
|
|
if dist > 0 then
|
|
lua_driver_backend:set_distance_min_max(0,4)
|
|
lua_driver_backend:handle_script_distance_msg(dist, yaw_deg, pitch_deg, push_to_boundary)
|
|
end
|
|
end
|
|
|
|
-- send the message down to proximity library. This needs to be a single distance. User of this function needs to ensure rngfnd backend exists
|
|
function send_rfnd_message(dist)
|
|
if dist > 0 then
|
|
local sent_successfully = lua_driver_backend:handle_script_msg(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"))
|
|
end
|
|
end
|
|
end
|
|
|
|
function update() -- this is the loop which periodically runs
|
|
|
|
if not sensor_driver_found then
|
|
if SET_PRX:get() == 0 then
|
|
setup_sensor(rangefinder, param_num_lua_driver_backend)
|
|
else
|
|
setup_sensor(proximity, param_num_lua_prx_backend)
|
|
end
|
|
end
|
|
|
|
if (not sensor_driver_found) then
|
|
-- We can't use this script if user hasn't setup a lua backend
|
|
return
|
|
end
|
|
|
|
local nbytes = port:available()
|
|
nbytes = math.min(nbytes, bytes_to_parse)
|
|
while nbytes > 0 do
|
|
nbytes = nbytes - 1
|
|
local r = port:read()
|
|
if r < 0 then
|
|
break
|
|
end
|
|
local c = r
|
|
|
|
-- if buffer is empty and this byte is 0x57, add to buffer
|
|
if linebuf_len == 0 then
|
|
if c == NOOPLOOP_FRAME_HEADER then
|
|
-- lua table indexing starts from 1
|
|
linebuf[linebuf_len + 1] = c
|
|
linebuf_len = linebuf_len + 1
|
|
else
|
|
linebuf_len = 0
|
|
end
|
|
elseif linebuf_len == 1 then
|
|
-- if buffer has 1 element and this byte is 0x00, add it to buffer
|
|
-- if not clear the buffer
|
|
if c == NOOPLOOP_FRAME_HEADER_1 then
|
|
linebuf[linebuf_len + 1] = c
|
|
linebuf_len = linebuf_len + 1
|
|
else
|
|
linebuf_len = 0
|
|
end
|
|
|
|
elseif linebuf_len == 8 then
|
|
-- store the next character as "number of pixels"
|
|
num_pixels = tonumber(c)
|
|
linebuf[linebuf_len + 1] = c
|
|
linebuf_len = linebuf_len + 1
|
|
-- Check if num_pixels is either 64 or 16
|
|
if num_pixels ~= 64 and num_pixels ~= 16 then
|
|
linebuf_len = 0
|
|
end
|
|
--update total bytes to expect
|
|
total_bytes_to_expect = 16 + (num_pixels * 6)
|
|
if num_pixels == 16 then
|
|
index_row_max = 4
|
|
end
|
|
else
|
|
-- add character to buffer
|
|
linebuf[linebuf_len + 1] = c
|
|
linebuf_len = linebuf_len + 1
|
|
if linebuf_len == total_bytes_to_expect then
|
|
-- calculate checksum
|
|
local checksum = 0
|
|
for i = 1, total_bytes_to_expect - 1 do
|
|
checksum = (checksum + linebuf[i]) % 256
|
|
end
|
|
-- if checksum matches extract contents
|
|
if checksum ~= linebuf[total_bytes_to_expect] then
|
|
if debug_enable then
|
|
gcs:send_text(0, "Checksum does not matches")
|
|
end
|
|
else
|
|
local min_distance = 0
|
|
local min_index = 0
|
|
local index = -1
|
|
for i = 10, total_bytes_to_expect - 7, 6 do
|
|
local distance = ((linebuf[i]<<8 | linebuf[i+1]<<16 | linebuf[i+2]<<24)/256) / 1000000
|
|
local status = linebuf[i+3]
|
|
index = index + 1
|
|
if status < 255 then
|
|
if (SET_PRX:get() == 1) then
|
|
local yaw, pitch = convert_to_angle(index)
|
|
if debug_enable then
|
|
gcs:send_text(0, "Distance debug: " .. distance .. " mm, status: " .. status .. "Yaw " .. yaw .. "pitch " .. pitch)
|
|
end
|
|
sent_prx_message(distance, yaw, pitch, false)
|
|
end
|
|
if status == 0 and (min_distance == 0 or distance < min_distance) then
|
|
min_distance = distance
|
|
min_index = index
|
|
end
|
|
end
|
|
end
|
|
|
|
if (SET_PRX:get() == 1) then
|
|
-- update prx boundary now that we have parsed all data
|
|
lua_driver_backend:update_virtual_boundary()
|
|
else if min_distance > 0 then
|
|
if debug_enable then
|
|
local yaw,pitch = convert_to_angle(min_index)
|
|
gcs:send_text(0, "Distance: " .. min_distance .. " m. Yaw " .. yaw .. "pitch " .. pitch)
|
|
end
|
|
send_rfnd_message(min_distance)
|
|
end
|
|
end
|
|
end
|
|
-- clear buffer
|
|
linebuf_len = 0
|
|
end
|
|
end
|
|
end
|
|
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()
|