AP_Scripting: Add drivers for NoopLoop TOFSense-M CAN and Serial sensors
This commit is contained in:
rishabsingh3003
Randy Mackay
parent
b6a54bbef9
commit
153ff58bd9
291
libraries/AP_Scripting/drivers/TOFSense-M/TOFSense-M_CAN.lua
Normal file
291
libraries/AP_Scripting/drivers/TOFSense-M/TOFSense-M_CAN.lua
Normal file
@ -0,0 +1,291 @@
|
||||
--[[
|
||||
Driver for NoopLoop TOFSense-M CAN Version. Can be used as a 1-D RangeFidner or 3-D proximity sensor. Upto 3 CAN devices supported in this script although its easy to extend.
|
||||
--]]
|
||||
|
||||
local update_rate_ms = 10 -- update rate (in ms) of the driver. 10ms was found to be appropriate
|
||||
|
||||
-- Global variables (DO NOT CHANGE)
|
||||
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 sensor_setup_done = false
|
||||
|
||||
-- Table contains the following info for 3 sensors. If more sensors are needed, this table will need to be increased
|
||||
-- approportate scritping backend from rngfnd/prx library, true if backend exists, index parsed last from sensor, minimum distance found since index was 0, Param to decide which rngfnd/prx backednd will match to this sensor, param to decide CAN ID of this sensor
|
||||
local backend_driver = {
|
||||
{lua_driver_backend = nil, sensor_driver_found = false, last_index = 0, min_distance = 0, INSTANCE, CAN_ID},
|
||||
{lua_driver_backend = nil, sensor_driver_found = false, last_index = 0, min_distance = 0, INSTANCE, CAN_ID},
|
||||
{lua_driver_backend = nil, sensor_driver_found = false, last_index = 0, min_distance = 0, INSTANCE, CAN_ID}
|
||||
}
|
||||
|
||||
local PARAM_TABLE_KEY = 104
|
||||
local PARAM_TABLE_PREFIX = "TOFSENSE_"
|
||||
|
||||
-- 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, 15), 'could not add param table')
|
||||
|
||||
--[[
|
||||
// @Param: TOFSENSE_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_NO
|
||||
// @DisplayName: TOFSENSE-M Connected
|
||||
// @Description: Number of TOFSENSE-M CAN sensors connected
|
||||
// @Range: 1 3
|
||||
// @User: Standard
|
||||
--]]
|
||||
MAX_SENSORS = bind_add_param('NO', 2, 1)
|
||||
|
||||
--[[
|
||||
// @Param: TOFSENSE_MODE
|
||||
// @DisplayName: TOFSENSE-M mode to be used
|
||||
// @Description: TOFSENSE-M mode to be used. 0 for 8x8 mode. 1 for 4x4 mode
|
||||
// @Values: 0: 8x8 mode, 1: 4x4 mode
|
||||
// @User: Standard
|
||||
--]]
|
||||
MODE = bind_add_param('MODE', 3, 0)
|
||||
|
||||
-- first sensor
|
||||
--[[
|
||||
// @Param: TOFSENSE_INST1
|
||||
// @DisplayName: TOFSENSE-M First Instance
|
||||
// @Description: First TOFSENSE-M sensors backend Instance. Setting this to 1 will pick the first backend from PRX_ or RNG_ Parameters (Depending on TOFSENSE_PRX)
|
||||
// @Range: 1 3
|
||||
// @User: Standard
|
||||
--]]
|
||||
backend_driver[1].INSTANCE = bind_add_param('INST1', 4, 1)
|
||||
|
||||
--[[
|
||||
// @Param: TOFSENSE_ID1
|
||||
// @DisplayName: TOFSENSE-M First ID
|
||||
// @Description: First TOFSENSE-M sensor ID. Leave this at 0 to accept all IDs and if only one sensor is present. You can change ID of sensor from NAssistant Software
|
||||
// @Range: 1 255
|
||||
// @User: Standard
|
||||
--]]
|
||||
backend_driver[1].CAN_ID = bind_add_param('ID1', 5, 0)
|
||||
|
||||
-- second sensor
|
||||
--[[
|
||||
// @Param: TOFSENSE_INST2
|
||||
// @DisplayName: TOFSENSE-M Second Instance
|
||||
// @Description: Second TOFSENSE-M sensors backend Instance. Setting this to 2 will pick the second backend from PRX_ or RNG_ Parameters (Depending on TOFSENSE_PRX)
|
||||
// @Range: 1 3
|
||||
// @User: Standard
|
||||
--]]
|
||||
backend_driver[2].INSTANCE = bind_add_param('INST2', 6, 2)
|
||||
|
||||
--[[
|
||||
// @Param: TOFSENSE_ID2
|
||||
// @DisplayName: TOFSENSE-M Second ID
|
||||
// @Description: Second TOFSENSE-M sensor ID. This cannot be 0. You can change ID of sensor from NAssistant Software
|
||||
// @Range: 1 255
|
||||
// @User: Standard
|
||||
--]]
|
||||
backend_driver[2].CAN_ID = bind_add_param('ID2', 7, 2)
|
||||
|
||||
--third sensor
|
||||
--[[
|
||||
// @Param: TOFSENSE_INST3
|
||||
// @DisplayName: TOFSENSE-M Third Instance
|
||||
// @Description: Third TOFSENSE-M sensors backend Instance. Setting this to 3 will pick the second backend from PRX_ or RNG_ Parameters (Depending on TOFSENSE_PRX)
|
||||
// @Range: 1 3
|
||||
// @User: Standard
|
||||
--]]
|
||||
backend_driver[2].INSTANCE = bind_add_param('INST3', 8, 2)
|
||||
|
||||
--[[
|
||||
// @Param: TOFSENSE_ID3
|
||||
// @DisplayName: TOFSENSE-M Thir ID
|
||||
// @Description: Third TOFSENSE-M sensor ID. This cannot be 0. You can change ID of sensor from NAssistant Software
|
||||
// @Range: 1 255
|
||||
// @User: Standard
|
||||
--]]
|
||||
backend_driver[2].CAN_ID = bind_add_param('ID3', 9, 3)
|
||||
|
||||
|
||||
-- check both CAN device for scripting backend. CAN Buffer length set to fixed 5
|
||||
local driver = CAN:get_device(5)
|
||||
if not driver then
|
||||
driver = CAN:get_device2(5)
|
||||
end
|
||||
if not driver then
|
||||
error("No scripting CAN interfaces found")
|
||||
return
|
||||
end
|
||||
|
||||
function setup_sensor(sensor, param_num)
|
||||
local sensor_count = sensor:num_sensors() -- number of sensors connected
|
||||
if MAX_SENSORS:get() > 3 then
|
||||
error("TOFSENSE: Only 3 devices supported")
|
||||
end
|
||||
|
||||
for i = 1, MAX_SENSORS:get() do
|
||||
local backends_found = 0
|
||||
local sensor_driver_found = false
|
||||
local lua_driver_backend
|
||||
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
|
||||
backends_found = backends_found + 1
|
||||
if backends_found == backend_driver[i].INSTANCE:get() then
|
||||
-- get the correct instance as we may have multile scripting backends doing different things
|
||||
sensor_driver_found = true
|
||||
lua_driver_backend = device
|
||||
end
|
||||
end
|
||||
end
|
||||
if not sensor_driver_found then
|
||||
-- We can't use this script if user hasn't setup a lua backend
|
||||
error(string.format("TOFSENSE: Could not find SCR Backend ".. tostring(i)))
|
||||
return
|
||||
end
|
||||
backend_driver[i].sensor_driver_found = true
|
||||
backend_driver[i].lua_driver_backend = lua_driver_backend
|
||||
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 index_row_max = 8
|
||||
if (MODE:get() ~= 0) then
|
||||
index_row_max = 4
|
||||
end
|
||||
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
|
||||
function sent_prx_message(prx_backend, dist, yaw_deg, pitch_deg, push_to_boundary)
|
||||
if (dist > 0) then
|
||||
prx_backend:set_distance_min_max(0,4)
|
||||
prx_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
|
||||
function send_rfnd_message(rfnd_backend, dist)
|
||||
if dist > 0 and (SET_PRX:get() == 0) then
|
||||
local sent_successfully = rfnd_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
|
||||
|
||||
-- get the correct instance from parameters according to the CAN ID received
|
||||
function get_instance_from_CAN_ID(frame)
|
||||
for i = 1, MAX_SENSORS:get() do
|
||||
if ((uint32_t(frame:id() - 0x200)) == uint32_t(backend_driver[i].CAN_ID:get())) then
|
||||
return i
|
||||
end
|
||||
end
|
||||
return 0
|
||||
end
|
||||
|
||||
-- this is the loop which periodically runs
|
||||
function update()
|
||||
|
||||
-- setup the sensor according to user preference of using proximity library or rangefinder
|
||||
if not sensor_setup_done 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
|
||||
sensor_setup_done = true
|
||||
end
|
||||
|
||||
-- read frame if available
|
||||
local frame = driver:read_frame()
|
||||
if not frame then
|
||||
return
|
||||
end
|
||||
|
||||
local instance
|
||||
if ((backend_driver[1].CAN_ID:get() ~= 0)) then
|
||||
instance = get_instance_from_CAN_ID(frame)
|
||||
if (instance == 0) then
|
||||
-- wrong ID
|
||||
return
|
||||
end
|
||||
else
|
||||
-- Simply accept any ID
|
||||
instance = 1
|
||||
end
|
||||
|
||||
-- Correct ID, so parse the data
|
||||
local distance = ((frame:data(0)<<8 | frame:data(1)<<16 | frame:data(2)<<24)/256) / 1000
|
||||
local status = frame:data(3)
|
||||
local index = frame:data(6)
|
||||
local update_rfnd = false
|
||||
if (index < backend_driver[instance].last_index) then
|
||||
-- One cycle of data has come. Lets update all backends involved
|
||||
if SET_PRX:get() == 1 then
|
||||
backend_driver[instance].lua_driver_backend:update_virtual_boundary()
|
||||
else
|
||||
update_rfnd = true
|
||||
end
|
||||
end
|
||||
backend_driver[instance].last_index = index
|
||||
|
||||
if status < 255 then
|
||||
-- Status is healthy
|
||||
if (SET_PRX:get() == 1) then
|
||||
-- Send 3D data to Proximity Library
|
||||
local yaw, pitch = convert_to_angle(index)
|
||||
sent_prx_message(backend_driver[instance].lua_driver_backend, distance, yaw, pitch, false)
|
||||
end
|
||||
if (backend_driver[instance].min_distance == 0 or distance < backend_driver[instance].min_distance) then
|
||||
-- store min data incase user wants to use it as a 1-D RangeFinder
|
||||
backend_driver[instance].min_distance = distance
|
||||
end
|
||||
end
|
||||
|
||||
if (update_rfnd) then
|
||||
send_rfnd_message(backend_driver[instance].lua_driver_backend, backend_driver[instance].min_distance)
|
||||
-- reset
|
||||
backend_driver[instance].min_distance = 0
|
||||
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()
|
||||
50
libraries/AP_Scripting/drivers/TOFSense-M/TOFSense-M_CAN.md
Normal file
50
libraries/AP_Scripting/drivers/TOFSense-M/TOFSense-M_CAN.md
Normal file
@ -0,0 +1,50 @@
|
||||
# TOFSense-M CAN Driver
|
||||
|
||||
TOFSense-M CAN Driver lua script. Only Copter/Rover/Plane 4.5 and above
|
||||
|
||||
## How to use - Pre-Configuration
|
||||
|
||||
- Connect sensors to autopilot's CAN1 port or CAN2 port
|
||||
- If connected to CAN1, set CAN_D1_PROTOCOL = 10 (Scripting), CAN_P1_DRIVER = 1 (First driver)
|
||||
- If connected to CAN2, set CAN_D2_PROTOCOL = 10 (Scripting), CAN_P2_DRIVER = 2 (Second driver)
|
||||
- Set SCR_ENABLE = 1 to enable scripting
|
||||
- Set SCR_HEAP_SIZE = 150000 (or higher)
|
||||
- If using the sensor as a 1-D Rangefinder (typically for terrain following); set RNGFND1_TYPE = 36 (Scripting) to enable the rangefinder scripting driver
|
||||
- If using the sensor as a 3-D Proximity sensor (typically for obstacle); set PRX1_TYPE = 15 (Scripting) to enable the proximity scripting driver
|
||||
- Reboot the autopilot
|
||||
- Copy the TOFSense-M_CAN.lua script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot
|
||||
|
||||
|
||||
## How to use - Script Parameter Configuration
|
||||
|
||||
If everything above is done correctly, new "TOFSENSE_" parameters should be visible (only after script loads. Please refresh parameters if not visible). Script parameters to adjust are listed below
|
||||
|
||||
### TOFSENSE_PRX
|
||||
If you have set RNGFND1_TYPE = 36, then set this as 0
|
||||
If you have set PRX1_TYPE = 15, then set this as 1
|
||||
Any change in this parameter will require a reboot (or scripting restart), ignore any errors on change before reboot. Make sure RNGFND/PRX is configured before setting this.
|
||||
|
||||
|
||||
### TOFSENSE_NO
|
||||
Total number of TOFSense-M CAN sensors connected on the bus. Change will require a reboot
|
||||
|
||||
|
||||
### TOFSENSE_MODE
|
||||
TOFSENSE-M mode to be used.
|
||||
- 0 for 8x8 mode.
|
||||
- 1 for 4x4 mode.
|
||||
All sensors must be in same mode. You can change the mode of sensor from NAssistant Software
|
||||
|
||||
### TOFSENSE_ID1
|
||||
First TOFSENSE-M sensor ID. Leave this at 0 to accept all IDs and if only one sensor is present. You can change ID of sensor from NAssistant Software.
|
||||
|
||||
|
||||
### TOFSENSE_INST1
|
||||
First TOFSENSE-M sensors RNGFND_/PRX_ Instance
|
||||
Setting this to 1 will pick the first backend from PRX_ or RNG_ Parameters.
|
||||
So for example if RNGFND1_TYPE = 36, RNGFND2_TYPE = 36, then you can set this parameter to 2, to pick RNGFND2_ parameters to configure this sensor
|
||||
|
||||
|
||||
### Configuring more than one sensor on same CAN bus
|
||||
|
||||
As described above, TOFSENSE_INST2, TOFSENSE_ID2 and TOFSENSE_INST3, TOFSENSE_ID3 can be adjusted so to support multiple sensors
|
||||
252
libraries/AP_Scripting/drivers/TOFSense-M/TOFSense-M_Serial.lua
Normal file
252
libraries/AP_Scripting/drivers/TOFSense-M/TOFSense-M_Serial.lua
Normal file
@ -0,0 +1,252 @@
|
||||
--[[
|
||||
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()
|
||||
@ -0,0 +1,37 @@
|
||||
# TOFSense-M Serial Driver
|
||||
|
||||
TOFSense-M Serial Driver lua script. Only Copter/Rover/Plane 4.5 and above
|
||||
|
||||
## How to use - Pre-Configuration
|
||||
|
||||
- Connect sensors to one of autopilot's Serial port
|
||||
- Set SERIALx_PROTOCOL 28
|
||||
- Set SCR_ENABLE = 1 to enable scripting
|
||||
- Set SCR_HEAP_SIZE = 150000 (or higher)
|
||||
- If using the sensor as a 1-D Rangefinder (typically for terrain following); set RNGFND1_TYPE = 36 (Scripting) to enable the rangefinder scripting driver
|
||||
- If using the sensor as a 3-D Proximity sensor (typically for obstacle); set PRX1_TYPE = 15 (Scripting) to enable the proximity scripting driver
|
||||
- Reboot the autopilot
|
||||
- Copy the TOFSense-M_Serial.lua script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot
|
||||
|
||||
## How to use - Script Parameter Configuration
|
||||
|
||||
If everything above is done correctly, new "TOFSENSE_S_" parameters should be visible (only after script loads. Please refresh parameters if not visible). Script parameters to adjust are listed below. Any changes would require reboot
|
||||
|
||||
### TOFSENSE_S1_PRX
|
||||
If you have set RNGFND1_TYPE = 36, then set this as 0
|
||||
If you have set PRX1_TYPE = 15, then set this as 1
|
||||
Any change in this parameter will require a reboot (or scripting restart), ignore any errors on change before reboot. Make sure RNGFND/PRX is configured before setting this.
|
||||
|
||||
### TOFSENSE_S1_SP
|
||||
UART instance sensor is connected to. Set 1 if sensor is connected to the port with fist SERIALx_PROTOCOL = 28. For example, if SERIAL1_PROTOCOL = 28, and SERIAL2_PROTOCOL = 28, then setting this parameter 2 would assume the SERIAL2 port is attached to the sensor
|
||||
|
||||
### TOFSENSE_S1_BR
|
||||
Serial Port baud rate. Sensor baud rate of the sensor can be changed from Nassistant software
|
||||
|
||||
|
||||
## Advanced: Configuring more than one sensor on different Serial ports
|
||||
|
||||
It is not recommended to use this driver for more than one sensor. However, it is possible if you wish to do it.
|
||||
Simply make a copy of the lua script, open it and change the first line "local sensor_no = 1" to "local sensor_no = 2" and upload that script along side the first script.
|
||||
Once a reboot is done, new TOFSENSE_S2_ parameters should be visible which can be configured as described above.
|
||||
|
||||
Loading…
Reference in New Issue
Block a user