Scanning rangefinder

[geofrancis ([𝙂𝙚𝙤])]

I had an idea for avoidance on ardupilot. Its so far working too easily, so I assume I’m missing something.

Would anything bad happen if I used lua to set a servo with a range finder attached to it to scan 45 degree sectors with a second delay at each of the 8 points, then use the same lua script to change the rangefinder orientation parameter each time it changes position?

I have tested this by manually changing the orientation of the rangefinder in parameters and it moves position so does anyone see a problem with automating this using lua?

How does avoidance treat intermittent returns? Does it remember something is there or will it immediately forget? Will it twitch?

my current idea is something like this
set RNGFND1_ORIENT to 0 to set rangefinder as forward position
set servo to 900us move servo to forward position
wait 1 second to get readings
change RNGFND1_MAX_CM to 3 to avoid false targets while rotating the sensor
change RNGFND1_ORIENT from 0 to 1 to change to +45 degree position
set servo to 1150us to move servo to 45 degree position
change RNGFND1_MAX_CM to 30000 to re-enable rangefinder
wait 1 second for readings
change RNGFND1_MAX_CM to 3 to avoid false targets while rotating the sensor
change RNGFND1_ORIENT from 1 to 2 to change to +90 degree position
set servo to 1300us to move servo to 90 degree position
change RNGFND1_MAX_CM to 30000
wait 1 second for readings.
repeat for the rest and go back to 0

[rishabsingh3003 (Rishabh Singh)]

Hi @geofrancis

“How does avoidance treat intermittent returns? Does it remember something is there or will it immediately forget? Will it twitch?”

Depends on what you are using: Simple Avoidance or Path Planning in autonomous mission?
For simple avoidance, it has no memory, so it only takes into consideration what the proximity sensors see at the present.

For autonomous modes path planning (BendyRuler), we use Obstacle Database, which stores obstacles for a few seconds and does have memory.

This is not the first time I am hearing about a rangefinder on top of a servo. For most people (especially on Copters), I have advised to either install multiple 1-D rangefinders or use one of those 360-degree lidars. But this is expensive and I know your end goal is to keep the project economical (from our previous conversations). So the servo idea might work (really unsure of the reliability and speed) but the only bottleneck I see is the constant change of parameters. I don’t know what the limits are on the rate of change of so many parameters and if doing so would cause any issues.

IMO, the more elegant approach would be to write a Lua script AP_Proximity backend driver that you can directly feed data to, using lua script. I am doing a similar thing for AP_RangeFinder at the moment. It will require modifications to the firmware though.

Hope this helps

[geofrancis ([𝙂𝙚𝙤])]

My biggest worry at the moment is trashing the eprom with writes to the parameters, using lua and parameters is more of a hacked proof of concept, if it can make it work like this then its a viable option for proper integration into ardupilot.

[rishabsingh3003 (Rishabh Singh)]

I think there is a way that just temporarily loads the parameter changes with Lua, does not write to eeprom. You can make those calls much faster than saving to eeprom. Ofcourse on reboot, it’ll go back to defaults

[geofrancis ([𝙂𝙚𝙤])]

@rishabsingh3003

temporarily loads the parameter changes with Lua

I had a check there and I see what you mean, there are different commands for settings parameters and saving them so my idea should work ok and not write anything to rom.

Parameters (param:)

• get(parameter_name) - Returns parameter value if available, or nil if not found.
• set(parameter_name) - Sets a parameter by name if available. Returns true if the parameter is found.
• set_and_save(parameter_name) Sets and saves a parameter by name if available. Returns true if the parameter is found.

@rishabsingh3003

he servo idea might work (really unsure of the reliability and speed)

The problem of reliability and speed are connected, most if the time when I read about a setup like this failing It’s because they have the servo running so fast it just overheats.

As for the scanning speed, its usefulness is determined by the speed of the vehicle and the range of the rangefinder. if you can see far enough and the vehicle is slow enough then it doesnt need to be that fast at scanning.

[rishabsingh3003 (Rishabh Singh)]

@geofrancis atleast it’ll be a fun project worth a try IMO! I can provide some support

[geofrancis ([𝙂𝙚𝙤])]

my ideal setup would be to have 8 little lidars around the boat for close in, then rotate a long range rangefinder like a radar or a powerful lidar for long range early warning of large objects like boats and bridges.

so i suppose ideally I would want simple avoidance on the short range sensors but path planning on the long range sensor.

it could do different search patterns, when travelling forwards you only need to look at -90 to +90, halving the scan time, or even just -45 to +45 would halve it again, it could change scan radius based on speed so the faster you go the more ahead it looks.

[geofrancis ([𝙂𝙚𝙤])]

what if i used the parameter hackery to set the long range range finder to temporarily replace the short range rangefinder when its at that orientation. that way i still get 360 coverage while its rotating.

i just found this

Advanced Configuration

If using a lidar or proximity sensor the following “obstacle database” parameters are available:

• OA_DB_SIZE : the maximum number of obstacles that can be tracked
• OA_DB_EXPIRE : the number of seconds after an obstacle disappears from view that it is removed from the database
• OA_DB_QUEUE_SIZE : the buffer size between the lidar and obstacle database. Normally this can be left at the default value
• OA_DB_OUTPUT : controls whether tracked objects are visible on the GCS as small airplanes

So if im understanding this correctly i can tune the database to compensate for the slow scanning? like if i set the DB_expire to match the scan time of the rangefinder it should hold objects until the rangefinder comes around again.

simple avoidance will just stop if anything is detected, but i dont know what it will do when the target disappears, it will probably move forwards again until the rangefinder comes around again.

Printed and built a test rig il try and get something tested soon

[geofrancis ([𝙂𝙚𝙤])]

after some scripting by @Yuri_Rage the prototype works!

first test of scanning SR04 rangefinder on ardupilot

[geofrancis ([𝙂𝙚𝙤])]

im blown away by this. i never expected this to work

[Yuri_Rage]

I thought this might “sort of” work, and I expected to have to request some help from the dev team to get it to the point where it is in that video. It’s a really creative take on manipulating existing parameters and hardware support to achieve a broader goal.

@rishabsingh3003, I don’t know if you’d explore incorporating such an animal into the firmware codebase (vs this scripted parameter manipulation), but it seems promising enough to suggest!

[geofrancis ([𝙂𝙚𝙤])]

second test with the servo reversed and an additional 200ms delay.

second test of scanning SR04 rangefinder on ardupilot with 3 way scanning.

[geofrancis ([𝙂𝙚𝙤])]

This is just the minimum required to test the idea, there are many ways it could be improved, next step will be more orientations and timing tweaks.

local BOOT_DELAY    = 5000  -- delay 5s on initial boot
local SAMPLE_DELAY  = 1000
local MOVE_DELAY    =  200  -- adjust this for servo movement time (ms)
local SERVO_CHAN    =   94
local DISABLE_RANGE =    3
local ENABLE_RANGE  = 3000

local PWM         = 0
local ORIENTATION = 1
local POSITION = {
    [0] = {[PWM] = 1000, [ORIENTATION] = 1},
    [1] = {[PWM] = 1500, [ORIENTATION] = 0},
    [2] = {[PWM] = 2000, [ORIENTATION] = 7},
    [3] = {[PWM] = 1500, [ORIENTATION] = 0}
}

local position_index = 0

function sample()
    param:set('RNGFND1_MAX_CM', ENABLE_RANGE)
    position_index = (position_index + 1) % (#POSITION + 1)
    return set_position, SAMPLE_DELAY
end

function set_position()
    param:set('RNGFND1_MAX_CM', DISABLE_RANGE)
    SRV_Channels:set_output_pwm(SERVO_CHAN, POSITION[position_index][PWM])
    param:set('RNGFND1_ORIENT', POSITION[position_index][ORIENTATION])
    return sample, MOVE_DELAY
end

return set_position, BOOT_DELAY

[geofrancis ([𝙂𝙚𝙤])]

After some more testing, the sr04 rangefinder takes some time to stabilize, upping move delay to 500ms and reducing sample time to 500ms made it more accurate and faster since its now 1 second per sector instead of 1.2.

latest version with 5 sectors

Third test of scanning SR04 rangefinder with 5 way scanning.

[geofrancis ([𝙂𝙚𝙤])]

More testing now with 7 way scanning, I think what I am eventually going to do is have a dynamic scan radius based on vehicle speed,
when stationary it can scan all 8 sectors,
when moving slowly forward it can scan 7 sectors,
as it gets faster it will limit that down to 5 sectors then 3 then just fix it straight ahead once its fast enough that should give a good compromise of scan speed vs coverage.

In the high speed forward facing configuration it could pan the servo slightly as the vehicle turns so it’s looking where the vehicle is going.

If using something like a lidar with a very narrow beam the servo could be moved a little as its taking reading to get a more complete reading of its sector.

This should work with multiple different rangfinders mounted to the same rotating platform, as long as they are at least 45 degrees apart they will never occupy the same orientation.

Fourth test of SR04 with 7 way scanning.

[geofrancis ([𝙂𝙚𝙤])]

I have a Sailwinch servo ordered that can turn 1.5x so it should be ideal for testing this

[ppoirier]

Well, You finally made it ?

Copter: object avoidance should support cheaper lidar

[geofrancis ([𝙂𝙚𝙤])]

yes, i remember reading about something similar but it was while ago.

Your solution on constantly scanning and feeding the position back is a much faster way, but as you describe on github that comes with timing problem since there is a delay causing accuracy issues, its also limited to one rangefinder where this script will work with any rangefinder supported by arudpilot but now that I think about it, It wouldn’t be hard to implement your method in the same way but using the ADC to set the rangefinder orientation.

By stopping and waiting at each sector for the readings to stabilize before enabling the rangefinder gives accurate data but at the expense of speed. my plan is to eventually use this for a long range radar or LIDAR so combining enough range with a slow enough speed means that the scan speed required is reduced dramatically.

Every other method i had seen of doing this required extra hardware, usually an arduino and some mavlink or i2c that does the scanning then sends that information to the flight controller, this only uses the lua interface to access ardupilot and nothing else so no soldering is required.

The more I think about it, your method of fast scanning using the potentiometer feedback would be good as a fast search mode, where scan time is more important than directional accuracy, if something is detected it could slow down to the 8 sector mode for more accurate direction and ranging.

[geofrancis ([𝙂𝙚𝙤])]

this is the 7 way script with the 500ms timing
rangefinderscan.lua (1.3 KB)

[geofrancis ([𝙂𝙚𝙤])]

Just to test I added a second rangefinder, my idea of having a dedicated forward-looking rangefinder along with the scanning one looks like it should work, this way its always looking the most important way, plus having something like a LIDAR looking forward might catch things the radar misses like vegetation or tree branches.