Visual Tracking on Pixhawk

System Overview

The project consists of:

A quarotor equipped with a Pixhawk, the Drone
A linux computer connected to a
HD webcam, and
3DR telemetry radio.
Software running on the linux computer for tracking the drone, and sending the position over the telemetry link
Modified Pixhawk firmware with sensors added to the ekf2 module

The quadrotor has an Aruco marker http://docs.opencv.org/master/d9/d6d/tutorial_table_of_content_aruco.html#gsc.tab=0 on the underside. The location of the marker is detected by a Linux computer running C++ code with the openCV library and send the position to the Pixhawk over MAVLINK using the ATT_POS_MOCAP topic.

Installation Instructions

Default Pixhawk Firmware

For instructions on setting up the required tool chain and building the standard Pixhawk firmware, follow the tutorial at http://dev.px4.io/starting-installing.html

The Pixhawk team uses Git to synchronize their work. If you are not familiar with git, now would be a good time to familiarize yourself with it.

The Pixhawk firmware is a rather large project. The main part of the software is located at https://github.com/PX4/Firmware.git, but some part reside in so called git submodules.

To get a general idea about, how to make custom modules (programs) for the Pixhawk I suggest reading through the examples at http://dev.px4.io/

Updated Pixhawk Firmware

The important changes in the Pixhawk firmware are located at: <blockqute> src/lib/ecl - this is the submodule for the EKF source code (and some other modules). The reason why it's in a submodule, is because it's ment as a portable estimator and can be used in other projects as well </blockquote> <blockqute> src/modules/ekf2 - this is a wrapper for the EKF mentioned above. It passes data from uORB topics to the filter and back. </blockquote>

To fetch the code I delvelpped and assign the correct submodule for the ekf run the following commands:

mkdir Pixhawk
cd Pixhawk
git clone https://github.com/skrogh/Firmware.git
cd Firmware
git remote add upstream https://github.com/PX4/Firmware.git
git checkout mocap-ekf2
Tools/check_submodules.sh
cd src/lib/ecl
git remote rename origin upstream
git remote add origin https://github.com/skrogh/ecl.git
git fetch origin
git checkout mocap-ekf2
cd ../../..
make px4fmu-v2_default

This will do:

Make a new directory for the project
Enter it
Clone my fork of the repository. This includes updated wrappers for the filter and some changes to enable Mikrokopter drivers
Enter the repository
Add a remote to the original repository. This can later be used to update the software to include changes made by the PX4 team
All work on motion capture input has ben done on a separate branch mocap-ekf2 so we switch to that
Tools/check_submodules.sh is a script that ensures, that submodules are up to date. We run it to fetch all submodules
Go into the submodule for the estimator
Move the original "origin" remote to "upstream" (these are just names, but this is to keep consistency)
Add a new remote to the version of this repo I made the changes in
Fetch changes
Again work was done on the mocap-ekf2 branch, so switch to this one.
Go back to the toplevel
build the default configuration

Pixhawk Setup

General Advice

Not all settings for the Pixhawk are stored in the firmware. A lot of parameters that affects the Pixhawk can be set either from a ground control station, or via the Nuttx shell.

It is recommended to connect to the Nuttx shell through the serial port and not the USB port. This will also give you debut info during boot.

When the Pixhawk boots a script ROMFS/px4fmu-v2_common/init.d/rcS is launched. Based on some parameters and files located on the SD card, the Pixhawk starts the appropriate modules (flight controllers, estimators, sensor drivers etc.)

The boot script does something like this:

If the file etc/rc.txt exists on the SD card, this is run instead of the autostart routine (step 2-6).
if SYS_AUTOSTART parameter is set, a (automatically generated) script is run that sets parameters associated with the chosen robot. (ex. for SYS_AUTOSTART=4001 https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/4001_quad_x is run).
If the file etc/config.txt exists on the SD card this script will run, allowing you to overwrite parameters for the robot, without creating your own custom one.
Sensor drivers are startet along with output dirvers (PWM and/or Mikrokopter if chosen).
Most SYS_AUTOSTART configurations (if not all?) will set the VEHICLE_TYPE variable. This will chose which modules are startet for flight controller and estimator(s). (Note the difference between parameters and variables. Parameters are saved in flash and prevail during poweroff - they are also available in C/C++ code).
if the file /etc/extras.txt exists this will run as the last part of the autostart.
A Nuttx shell is opened over USB if no SD card is present. Otherwise MAVLINK wil be started on the USB port (on my fork a Nuttx will always be present on the USB, to start mavlink for ground control connection type mavlink start -r 800000 -d /dev/ttyACM0 -m config -x in a Nuttx shell)

Vision Tracking Specific Settings

Instead of making custom startup scrips, we reuse the ones present and set the correct parameters.

On the SD card create the file etc/config.txt:

# EKF2 Setup
echo "Disabeling INAV"
param set INAV_ENABLED 0
echo "Disabeling LPE"
param set LPE_ENABLED 0
echo "This makes the quad use ekf2"
# Mikrokopter setup
echo "Setting MikroKopter BL-ctl settings"
set OUTPUT_MODE mkblctrl
set MKBLCTRL_MODE x

This will tell the Pixhawk to disable the split attitude position estimators and use the combined ekf2 estimator instead. Furthermore the Mikrokopter motor driver will be enabled. Set SYS_AUTOSTART=4001 for a standard x-quadrotor configuration.

In the ekf2 module some parameters have been added:

EKF2_USE_MOCAP (default: 0) - use motion capture data? (if 0 will use GPS)
EKF2_USE_PREDICT (default: 1) - use IMU measurements to propagate the delayed state estimate to hide delay (set to 0 for debug) 
EKF2_MO_x_NOISE - variance of x position measurement
EKF2_MO_y_NOISE - variance of y position measurement
EKF2_MO_z_NOISE - variance of z position measurement
EKF2_MO_r_NOISE - variance of roll measurement (set high, otherwise you have to really carefully align pixhawk with marker)
EKF2_MO_p_NOISE - variance of pitch measurement (set high, otherwise you have to really carefully align pixhawk with marker)
EKF2_MO_h_NOISE - variance of headding measurement

Ground Station

QGroundControl is the recomended tool for visualizing onbard data in real time and setting parameters. It is available from http://qgroundcontrol.org/

AMP Mission Planner is another common ground station. I had some issues with QGroundControl not showing all parameters, but Mission Planner did. Mission Planner is, however, made with a different flight stack in mind (flight controller and estimators).

3DR Radio Notes

The radios used for telemetry are configured with the default settings. While the air-rate in this mode is rather slow, this also gives much better noise immunity.

The radio settings can be set from AMP Mission Planner: http://planner.ardupilot.com/wiki/common-3dr-radio-version-2/

Installing Tracker software

Build and install OpenCV 3.1 according to: http://docs.opencv.org/3.1.0/d7/d9f/tutorial_linux_install.html

Remember to build with contributed modules.

Clone the repository https://github.com/skrogh/droneTracker.git where you want the tracker and build:

mkdir tracker
cd tracker
git clone https://github.com/skrogh/droneTracker.git
cd droneTracker
./getDependencies.sh
cmake .
make

getDependencies.sh might take a while as it has to download and unpack the Eigen Library.

User Instructions

Coordinate systems

The tracking program can take a setting file giving the position and rotation of the camera in the world NED coordinate system. If this file is not supplied, the default is that the camera is positioned at the origin of the world coordinate system, rotated so that:

Axis Alignment
World Frame	Camera Frame
North	-y
East	-x
Down	-z

Aruco Markers

http://docs.opencv.org/master/d9/d6d/tutorial_table_of_content_aruco.html#gsc.tab=0

Starting Application

Set the correct settings on the pixhawk according to Visual_Tracking_on_Pixhawk#Vision Tracking Specific Settings

Software Overview

Original ekf2 Module

Motion Capture

Important notes

Changes in files not concerning the EKF

The following files have been changed, to enable Mikrokopter BL-CTRL motor drivers and ultrasonic rangefinder: cmake/configs/nuttx_px4fmu-v2_default.cmake in the process the ekf2 module was added to the build list along with the useful tool listener. To free up some space, some unused modules were commented out of the build list.

To Do

Add file input for Aruco board

At the moment the layout of the Aruco board is fixed to a single marker in the center of the drone. The position and size is hardcoded. This should be changed to take input from a yml file.

Revet to tracking red balls?

The choice to use Aruco markers was made because they are a relatively well tested and robust. Detection is fast and reliably, as long as lighting conditions are good. In strong back lighting conditions, tracking consistency is pretty bad due to decreased contrast and glare.

The original tracking algorithm used red plastic balls at the edges of the drone. The advantage of this is that the baseline is significantly increased.

It might be beneficial to replace the Arcuo tracker with tracking of balls.