Copter: object avoidance should support cheaper lidar

[rmackay9]

The SF40C is about $1k USD which is expensive for many users. We should add support for cheaper sensors like these:

• RPLidar http://www.robotshop.com/en/rplidar-360-laser-scanner.html
• Sweep http://scanse.io/

[khancyr]

@rmackay9 , I got RPLidar 1 and I don't think it is suitable for flying vehicle, but version 2 seems ok (http://www.robotshop.com/en/rplidar-a2-360-laser-scanner.html)

[patrickpoirier51]

Prototype of a Multi-Laser -STM VL53LOX - Time Of Flight Range: 2 Meter , Powered by Arduino or Tennsy

The POC : Proximity Obstacle Collision avoidance system

More info here: https://gitter.im/ArduPilot/ardupilot/VisionProjects/archives/2017/02/26

[geofrancis]

those i2c VL53L0X lidar sensors look ideal, if they could be supported then they would be great for short range avoidance and a fraction of the weight of something like a sr-04 sonar. https://www.pololu.com/product/2490

they would make great altimeters too if your just wanting it for landing for plane and copter.

[geofrancis]

https://github.com/ArduPilot/ardupilot/issues/2717

[patrickpoirier51]

Here is my first mavlink test with the POC

The arduino base system , sequentially sends these messages: command_heartbeat(); command_distance_1(); command_distance_2(); command_distance_3(); command_distance_4(); command_distance_5(); command_distance_6();

Each distance message is //MAVLINK DISTANCE MESSAGE int sysid = 1;
int compid = 158;
uint32_t time_boot_ms = 0; /< Time since system boot/ uint16_t min_distance = 1; /< Minimum distance the sensor can measure in centimeters/ uint16_t max_distance = 160; /< Maximum distance the sensor can measure in centimeters/ uint16_t current_distance = dist1; /< Current distance reading/ uint8_t type = 0; uint8_t id = 1; uint8_t orientation = 4; uint8_t covariance = 0;

and for each message we associate distance , id and orientation of the associated sensor. The actual setup is dist 1 - id 1 - orient 4 == looking at 6 oclock dist 2 - id 2 - orient 6 == looking at 9 oclock dist 3 - id 3 - orient 7 == looking at 11 oclock dist 4 - id 4 - orient 0 == looking at 12 oclock
dist 5 - id 5 - orient 1 == looking at 1 oclock
dist 6 - id 6 - orient 2 == looking at 3 oclock

The message refresh rate is a slow 4,5 Hz and it seems that its not reading correctly all the time after few experiments, reduced the 4 sensors = front - back - left- right and refresh is at 8 HZ and the reading is consistent

[rmackay9]

@patrickpoirier51, looks good! If the distances are appearing on the proximity viewer then it's working and it should do object avoidance.

[patrickpoirier51]

Doing test with Object Avoidance transmitting Mavlink Message on a Pixracer running 3.5.2 (master)

Changed the Mavlink message structure like this: id 1 - orient 4 == looking at 6 oclock id 2 - orient 24 == looking upward id 3 - orient 6 == looking at 9 oclock id 4 - orient 0 == looking at 12 oclock id 5 - orient 1 == looking at 3 oclock

Parameters are as follow: PRX_TYPE = 2 (MAVLINK) AVOID_TYPE = 2 (USE PROXIMITY SENSOR) AVOID_ENABLE = 1 AVOID_DIST_MAX= 1 METER Connected to SERIAL 2 = 112 (Kbauds) , using 1 (MAVLINK)

With all sensor enabled I cannot takeoff in AltHold = Motors are spinning but not full speed If I disable id 2 - orient 24 == looking upward , I can takeoff and get avoidance on 360 degs. If I disable all sensors but id 2 - orient 24 == looking upward = I got BAD LIDAR HEALTH

[patrickpoirier51]

The POC has a little brother: http://www.kaidengtoys.com/goodshow/k90-k90-remote-control-edition-6-axis-gyro-quadcop.html Look at 2:42 https://youtu.be/o0QdqPtIn5A?t=163

[patrickpoirier51]

Update: with new code release https://github.com/ArduPilot/ardupilot/issues/6080 the AltHold is working OK. Thanks to Randy for this. Will perform more test and report on discuss.ardupilot.org. Next step will be implementing VL53L1 ... when available http://www.st.com/content/ccc/resource/technical/document/data_brief/group1/05/c4/52/c9/2f/fb/41/98/DM00364684/files/DM00364684.pdf/jcr:content/translations/en.DM00364684.pdf

[rmackay9]

Great! The multi-zone range finders will be great to start playing with. I've actually got a LeddarVu which has 8 zones which is a start. It's a bit pricey but hopefully prices will come down.

[geofrancis]

@patrickpoirier51 when will you be releasing your code for the vl53l1?

[patrickpoirier51]

@geofrancis My code is a mess... but here are links to codes I used

MAVLINK on ARDUINO http://forum.arduino.cc/index.php?topic=382592.0

VL53L0X – maximum sensors on I2C arduino bus https://forum.pololu.com/t/vl53l0x-maximum-sensors-on-i2c-arduino-bus/10845/7

[SteveJos]

Hello, I started the integration of the latest RPLIDAR-A2 with 16m range in the AP_PROXIMITY library. The serial communication is working and I already get measurements from the LIDAR in the proximity window, but further testing is needed . Please have a look at the PR #6552:

https://github.com/ArduPilot/ardupilot/pull/6552

[dionator]

Does anyone have more detail on the VL53L1? I can't find any info on this except the "data brief" document on ST's website. Any ideas about availability? Incidentally, could this be one of the sensors that going to be in the upcoming iPhone 8?

[patrickpoirier51]

Here is the answer from MOUSER when I requested for a quote on the VL53L1 "No bid, not available for mass market"

[patrickpoirier51]

Well... the POC has been commercialised: http://www.teraranger.com/products/teraranger-multiflex/

[avncalst]

Short remark: I used dronekit on a RPI access point to address and readout 3 VL53L0x sensors. I wrote a brief python program to send the mavlink distance messages and readout the sensors. The readout python code is based on the excellent work of johnbryanmoore on github (johnbryanmoore/VL53L0X_rasp_python). A sample of my code looks like:

def range_sensor(self,dist,orient):

    self.msg = self.vehicle.message_factory.distance_sensor_encode(
        0,      # time system boot, not used
        1,      # min disance cm
        160,    # max dist cm
        dist,   # current dist, int cm
        0,      # type sensor
        1,      # on board id, not used
        orient, # orientation: 0...7
        0,      # covariance, not used
        )

    self.vehicle.send_mavlink(self.msg)

def measure(self):
    distance1 = self.tof1.get_distance()/10
    self.range_sensor(distance1, 0)
    distance2 = self.tof2.get_distance()/10
    self.range_sensor(distance2, 1)
    distance3 = self.tof3.get_distance()/10
    self.range_sensor(distance3, 7)

    time.sleep(self.timing/1000000.00) 

RNG = RNGFIND() while True: """ measurement loop """ RNG.measure() time.sleep(0.1)

[patrickpoirier51]

Cool, so you are using the TCA9548A I2C Multiplexer ?

[avncalst]

No I am not using the I2C multiplexer. At boot the 3 sensors get a different address, see johnbryanmoore/VL53L0X_rasp_python/python/VL53L0X_multi_example.py

[patrickpoirier51]

Yeah, like I did with the Arduino https://github.com/patrickpoirier51/POC/blob/master/VL53_MULTI_PROMINI.ino

[poliant]

I'm trying to send DISTANCE_SENSOR message to FC (pixhawk 2.4.8 clone) through RPi3 using python dronekit following template from @avncalst but it seems pixhawk does not accept it reporting "Bad LiDAR Health". I'm using a Lidar Lite v1 connected via PWM to RPi collecting measurements and forwarding (20Hz frequency) to FC

def send_distance_message(dist, orient):
    msg = vehicle.message_factory.distance_sensor_encode(
             0,      # time system boot - not used
             25,      # min disance cm (defined 25cm as referement)
             40000,    # max dist cm (40mt for LidarLite)
             dist,   # current dist, int cm - MUST BE INT
             4,      # type= 0: laser; 4: unknown/analog
             0,      # on board id, not used
             orient, # must be set to MAV_SENSOR_ROTATION_PITCH_180 (12) for mavlink rangefinder, represents forward-facing
             0,      # covariance, not used
    )
vehicle.send_mavlink (msg)
vehicle.commands.upload()

I've checked RPi calculates correctly the distance (at least in the range 20cm - 3mt) but no message is passed by to FC.

FC settings:

PRX_TYPE: 2 
AVOID_ENABLE: 2
AVOID_DIST_MAX: 5
AVOID_MARGIN: 2

Radar does not activate.

Thanks for support

PS: how can I install mavlink inspector on windows (7 and/or 10)?

[avncalst]

Did you set the RNGFND tab?

[poliant]

Yes I did. But RNGFND is disable. And in mavlink inspector DISTANCE_SENSOR does not appear. What I really do not understand is why there is the message "bad lidar health"...

[poliant]

I've enable RNGFND as well, but problem still persist

[avncalst]

Here a simple test file. Works for me. FC settings: Copter version 3.5.3, RNGFND:10 (mavlink), PRX_TYPE:2 (mavlink)

import time from dronekit import connect, VehicleMode

vehicle = connect('udpin:127.0.0.1:15550', wait_ready=False) vehicle.initialize(8,30) vehicle.wait_ready('autopilot_version') print "\nGet all vehicle attribute values:" print " Autopilot Firmware version: %s" % vehicle.version print " Major version number: %s" % vehicle.version.major print " Minor version number: %s" % vehicle.version.minor print " Patch version number: %s" % vehicle.version.patch print " Release type: %s" % vehicle.version.release_type() print " Release version: %s" % vehicle.version.release_version() print " Stable release?: %s" % vehicle.version.is_stable() print " test mav_distance message" def range_sensor(dist,orient):

    msg = vehicle.message_factory.distance_sensor_encode(
        0,      # time system boot, not used
        1,      # min disance cm
        160,    # max dist cm
        dist,   # current dist, int
        0,      # type sensor
        1,      # on board id, not used
        orient, # orientation: 0...7
        0,      # covariance, not used
        )

    vehicle.send_mavlink(msg)

def measure():

    distance1=150
    range_sensor(distance1, 0)

    distance2=150
    range_sensor(distance2, 1)

    distance3=150
    range_sensor(distance3, 7)

while True:

measure()
time.sleep(0.1)        

[poliant]

not different from what I have... let's try

thanks

[poliant]

problem in my code was the orientation: I was passing a non consistent value (so the warning message "bad lidar health").... how to make a mountain out of a molehill

thanks for support

PS: it seems there is no need to set RNGFND to 10, it is enough PRX_TYPE to 2, but I've not tested on-air (quad still not assembled)

[SiyaLiu]

Hi, everyone, pls allow me to add a low cost LiDAR TFmini from Benewake. It's mini size with big smart, easy to integrate and use. Ranging 12m, 6g. Public price is US$39.9 only.

You are welcome to check below eBay link for the Benewake TFmini LiDAR to know more:

https://www.ebay.com.hk/itm/Benewake-LiDAR-Sensor-Range-Finder-Distance-Measurement-Module-TFmini-12m/222769896378

Thanks

[amilcarlucas]

Sure @SiyaLiu go ahead do a github pull request and we will review it.

[khancyr]

I think @patrickpoirier51 did some test with it?

[SiyaLiu]

yes, @patrickpoirier51 , is it ok that you share your test with TFmini LiDAR here too ?

[patrickpoirier51]

Yes, once we can get the TFMINI fully working, I will update here as well.

[patrickpoirier51]

Well, there is a new cheap sensor with a range of 6-12 meters on the bench. Its a POC using Benewake TFMINI Still a lot do do but the range makes it a great contender for an avoidance system on budget BOM on the picture is approx 150$

[karu2003]

I want to try to integrate my RangeFinder. Hardware should work right away. I do not know the programm structure of ardupilot - it can take a long time. https://github.com/ipmgroup/RangeFinder_9xVL53L0X_hardware

[khancyr]

@karu2003 already done : https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h

[geofrancis]

I see that VL53L0X has been added to ardupilot but when i tried it connected to i2c I dont get anything, is there something im missing?

[karu2003]

@khancyr only one was added. PCA9554 no support. I want 5 - 9 directions.

[patrickpoirier51]

Please ask question on FORUM, this is a developper's thread, this is used to add/enhance system and code

[patrickpoirier51]

@karu2003 interesting design, you can use an Arduino or a Teensy to collect I2C devices and transform into Serial Mavlink avoidance message , like I did with the POC

[jason099912]

Hi Patrick, I am very interested in your findings using the TF Minis as I will be attempting something similar. Which Board are you using to connect the multiple Lidars Arduino Mega? I see for individual sensors you are using Arduino Nano and I2C connection on Pixhawk.

[patrickpoirier51]

@jason099912 I am still workin on this prototype and I wil write a blog pretty soon. Yes it is converting the TFMINI Serial into I2C using Attiny85 and each sensor has his own I2C address. The Arduino ProMini do collect the I2C and transmit over Serial using Mavlink Protocol , just like the POC.

I tried collecting TFMINI directly from serial interface using a Tennsy3.5 but I could not get stable operation because of too many buffer overruns. The code can certainly be optimised but without real handshake it will allways be symptomatic. and Pricewise its less expensive to use multiple Attiny (1.50$) + ProMini (2,50$) than a single Teensy3.5 (30$).

[jason099912]

Hi Patrick, Thank you for the details it looks very interesting and could potentially be a very cost effective solution. Keep up the good work.

[geofrancis]

I have been on the ardupilot.org and people have asked the same question and got no response. There is no information on the ardupilot website either other than a link to the manufacturer. all I can find is the its rangefinder type 16, when I tried this it didnt work, I have lidar lites and a few different ultrasonics that I have used without issue. all I wanted was to know if there was anything specifically different about the VL53L0X or should it just work when plugged into the i2c and the type is set like any other rangefinder.

I plan on building a POC but i would like to get one working first for testing.

[patrickpoirier51]

@geofrancis there is no particular requirement to make the VL53L0X work. Once configured using the wizard (optionnal hardware-rangefinder) you reboot the FC and should be able to read sonar distance

[geofrancis]

thank you, then I probably have a faulty one.

[avncalst]

I mounted a Lidar-Lite v3 on a servo and adapted my python dronekit code. In this way I am able to measure distances in a 180 degree yaw range. First tests show promising results.

[patrickpoirier51]

Thats Cool :-)

What is the maximum scan speed you can achieve ? Can you easily correlate the controlled-vs-real position ?

I builded one with TFMINI + Arduino + Cheap Servo, I could get 2 cylcles per second but trying to go faster with an open-loop servo system was inducing errors == giving wrong vectors

[avncalst]

The scan speed is an issue. I am still testing and optimizing the code. Instead of scanning I direct the servo to the 5 predefined angles. I tested this morning the new code on my drone in loiter mode (still at low cycle times (1cycle/sec)). Works OK. I will continue to evaluate and try to speed up the cycle time, but it will be always inferior to 5 fixed lidars. However it is a cheaper solution.

[patrickpoirier51]

This is the prototype I talked you about == Its a micro servo and controlers inserted in a TicTac Box You can see the Digistump (Attiny85) clone that is used to control the servo. The Arduino Pro Mini sends 3 bits vector codes (3 digital pins) corresponding to the pwm code for azimuth. My next step is to add 3 opto sensors -or any type of encoder- to close the scanning loop so I dont have to ''tune'' the timing ;-)

[avncalst]

Nice work