Obstacle Avoidance for 3DR Solo Rev. 2

Introduction

Here it comes: an affordable, front facing collision detection solution for your 3DR Solo Drone!

This hack describes everything you need to establish Lidar-based obstacle detection with direct feedback in the Solex App.

We are using the Lightware SF20 lidar sensor which is the most appropriate sensor for this use case due to its small form factor, (relative) low cost, precision and low weight. In addition, it comes with a built-in servo driver for SLAM maps within user-definable view angle and distance. The Lidar is being controlled by an Arduino, which manages the Lidar configuration, scanning and obstacle detection.

The Arduino is connected to the IMX companion computer in Solo via USB. On the IMX we add the required controls and actions, LED handling, speech output and collision prevention routines in shotmanager.

A word of caution before you proceed: All in the following described modifications require a great portion of care since we are modifying some of the core modules of the 3DR Solo software. It is strongly recommended to backup all the original files on the IMX before making any changes. This project will void your 3DR warranty.

Prerequisites

• Know how to SSH into Solo and to make modifications using PuTTY
• Familiar with the Arduino IDE
• Knowledge on how to install Python packages
• Basic Arduino and Python programming skills

Functionality

Current features include:

• 3 sector scanning for obstacles (left, center, right) within a 6 meter range ahead of Solo.
• Scan is enabled by log-button press on the controller paddle. A short button press toggles the GoPro on/off as before.
• Scanning on/off is indicated by a text and voice prompt in Solex.
• The Solo LEDs indicate the detection of an obstacle by flashing in purple color: Left LED: obstacle to the left; right LED: obstacle to the right; both front LEDs: obstacle in center (LED control requires Arducopter 3.5 or higher installed)
• If a front collision is detected, Solo goes into BRAKE mode in all flight modes except LAND or RTL. After Solo stopped, it automatically switches back into Loiter.
• In all smartshots, you will get visual feedback by the LEDs and voice feedback in Solex.
• Obstacle avoidance keeps track of the forward pitch angle along with Solo altitude. As soon as the pitch angle for the current altitude would cause the lidar beam to hit ground, obstace avoidance is temporarily disabled. Both front LEDs flash in yellow to indicate that status.
• Text and speech output in Solex, for obstacles in the center of the flight path also the approximate distance to the obstacle is reported

Setup of the SF20 Lidar

Print the Lidar housing, Servo mount and the Solo mount from the STL files linked to this blog in the attachment section.

Mount the Lidar on the Micro Servo. It is recommended to use a high quality digital servo such as the referenced Savox for low EMI emission and durability.

Mount the Servo into the printed Solo mount and attach the mount onto the Solo shell with double-sided adhesive tape as shown in the following pictures.

Connect the Arduino Board

First, attach the 3DR Solo Breakout board to the accessory bay. Instead of using a breakout board, you may also solder a micro USB jack to the corresponding pins of the accessory bay port. All pin attachments and wiring can be found in the 3DR Solo Dev Guide.

Next, bridge the 3DRID pin on the breakout board to GND to set the Solo IMX as USB host and then connect the Arduino to the USB connector on the breakout board.

The Arduino Mega board then can be mounted on top of the breakout board as shown on the images below. You may ask why I am using the bulky Mega and not a Nano or Mini board? Well, the Mega contains 4 UARTs for serial communication while the other Arduino boards only have one UART. We need two, one to communicate between the IMX and the Arduino, and another one to communicate with the Lidar.

As an alternative to the Arduino Mega you may also use an Arduino Micro Pro which has a much better form factor and can fit directly into the lidar housing. The github referenced here contains now also a sketch for the Micro Pro board.

Wire up the SF20 and Servo

Make sure you use the SF20 serial cable supplied with the Lidar and wire up the servo and SF20 according to the following diagram.

Software Installation

A. Arduino:

Download the Arduino sketch from my Git (see Appendix) and install the Lightware SF20 libray in the Arduino IDE (link to the Lightware Git is in the appendix). Deploy the code on the Arduino. The code should compile and deploy w/o errors or warnings.

By using a Firmata client installed on your PC you can test the lidar functionality before starting to do the modifications on the Solo side. For Windows, MAC or Linux machines, you find a link to download a test client here.

In the Firmata test client, define pin 2, 3, 4 and 7, 8, 9, 10 as input. Pin 2 in state High indicates an obstacle on the left, pin 3 in center, and pin 4 in state High indicates an obstacle on the right. Pin 7 -10 are used to display the distance to the obstacle detected in binary format. Define pin 6 as output.

By setting pin 6 to High, the scanning should be enabled and the servo will sweep in a +/- 20 degree angle from its center position. You can modify the lw20 scan characteristics by modifying the attributes in the Setup section of the Arduino sketch. When you set pin 6 to Low, scanning should stop.

B. Pymata Installation

We are using Firmata for the communication between the IMX / Python world and the Arduino. Therefore we need to install the Python implementation of Firmata, "Pymata" on the IMX. There is a perfect description available by Daniel Nugent here on Hackster on how to setup and install Pymata on Solo so there is no need to replicate that in this blog.

It is very important that you go ahead with a version no later than 2.1 of Pymata. The newer versions require a different Python install method which is not supported by the Solo Python environment. You find version 2.1 in the Pymata Git referenced in the appendix.

C. Solo Shotmanager Modifications

All necessary changes for the lidar-based obstacle avoidance are included in 2 Solo files, shotmanger.py, and buttonmanager.py. Both files are located in the /usr/bin directory of the IMX.

Backup both files before you copy the modified files from the git (see link in the appendix) to the IMX into the /usr/bin directory so that you can revert back in case something goes wrong.

After you have installed those 2 files, reboot Solo and try it out!

Long button press of the Solo Controller paddle will turn on the scanning. If there is an obstacle in range you will get a text and voice prompt in Solex. Another long button press will turn the scanning off. A short click will release the Gopro shutter as before.

Note that the scanning is temporarily disabled at altitudes below one meter and in case the lidar beam would hit the ground when the drone leans forward at altitudes below 3 meters. This is indicated by yellow-flashing front LEDs.

Note that the LED support requires Arducopter 3.5 or higher installed.

If you are running OpenSolo, you need to manually adopt the buttonmanager changes. OpenSolo provides an enhanced handling of button events which is not reflected in the shotmanager and buttonmanager files.

In all flight modes except RTL or LAND, and altitudes > 1 meter, an obstacle detection in a +/- 30 degree angle in front of Solo will result in entering BRAKE and Solo will instantly stop in GPS assisted modes. Solo will switch back to Loiter automatically.

Lightware advised that using a printed plastic housing for the SF20 may result in overheating of the Lidar sensor. Better choose the LW20 which comes with a aluminum housing. However I did not experience any issues with the SF20 in the 3D printed housing I created so far.

Known Limitations

• Works reliably only under clear weather conditions. In rain, snow and fog, the SF20 reports randomly false readings.
• There is a lag until Solo goes into BRAKE mode. Also, Solo tends to drift a couple of meters depending on cruise speed after entering BRAKE. So be careful that you don't crash.

Obstacle Avoidance In Action

Try it out - and have fun - but always remember be careful!