PX4 Software-In-The-Loop(SITL) Simulation on Gazebo

Jaeyoung Lim

Introduction

PX4 is a great platform to implement a UAV system based on a opensource autopilot. One of the great features of PX4 is that you can run a SITL simulation(Software in the loop simulation) to simulate your flight on simulation. This is useful as you can check new mission or control algorithms before actually flying the quadrotor and possibly damaging it. This article shows will show you how to run a SITL for PX4 and how to interact with it through gazebo and qgroundcontrol. The article has mainly the same contents as the tutorial but is reorganized and more specific for the purpose of offboard control.

A brief introduction to Software in the loop simulation

Software in the Loop is a simulation of a system which is modeled and run under software without any hardware. To develop a control software, usually the system will have to pass three simulation verification before the software is implemented into a real system.
The first is sometimes called MITL(Model-In-The-Loop) which only includes a mathematical model of the controller. The next is to verify the controller in a SITl(Software-In-The-Loop). Software in the loop simulations verify the actual software integration of the controller to see if there is any unforeseen problems within the system. The third process is the PITL(Processor-In-The-Loop) which verifies there are no problems within the processor calculations. The last procedure is to have a HITL(Hardware-In-The-Loop) which includes the major hardware components in the control system to verify the controller is working properly.

PX4 SITL

PX4 is a flight control platform and can be run on SITL mode. The diagram shows a simple example of a px4 in SITL mode.

Rererence: https://github.com/PX4/Firmware/tree/master/posix-configs/SITL
The PX4 SITL can be interfaced using MAVLink (Similar to the real flight controller) through a UDP port.

Running the simulation

Installation

ROS

The current article is implemented based on ROS indigo. Other versions of ROS that has been after ROS indigo should work, but have not been verified. For installation instructions of ROS, try this tutorial.

Gazebo

Gazebo6 should be used for the PX4 gazebo plugin. You can check which specific version to use with ROS here.

PX4

PX4 and jMAVSim should be installed and compliled to run PX4 in SITL mode.

Clone the PX4 source.

mkdir -p ~/src
cd ~/src
git clone https://github.com/PX4/Firmware.git
cd Firmware
git submodule update --init --recursive
cd ..

Running the simulation

Different airframes can be selected to run the simulation. Supported airframes include Multirotors(with/without optical flow sensors), Planes, VTOL(tail sitters, quadplanes).

Quadrotor

The default quadrotor model is IRIS from 3DRobotics.

cd ~/src/Firmware
make posix_sitl_default gazebo

Screenshot from 2016-01-15 14:00:10

Quadrotor with Optical Flow

cd ~/src/Firmware
make posix gazebo_iris_opt_flow

Typhoon H 480

cd ~/src/Firmware
make posix gazebo_typhoon_h480

Standard VTOL

cd ~/src/Firmware
make posix_sitl_default gazebo_standard_vtol

Tailsitter VTOL

cd ~/src/Firmware
make posix_sitl_default gazebo_tailsitter

When the SITL is running properly, the log screen will appear as below.

Interfacing SITL with qgroundcontrol

The default UDP address for the mavlink is as follows
– to connect to a specific IP: “udp://:14540@192.168.1.36:14557”
– to connect to a local host: “udp://:14540@127.0.0.1:14557”

If qgroundcontrol is installed properly, it will automatically connect to the UDP port while the simulation is running.그림3

그림2

Mission with Quadrotor

Mission with Standard VTOL

You can also change parameter files and do automatic missions just as if you are interacting with the real flight controller.

Interfacing SITL with ROS

The biggest advantage of interfacing to SITL through ROS has the same interface with interfacing with a real flight controller. You only need to change the fcl_url on MAVROS.

roslaunch mavros px4.launch fcu_url:=”udp://:14540@127.0.0.1:14557″

It is good to have arguments on fcu_url in the launch file to be able to change between SITL and the real board to speed up the process.

Examples

Sending position setpoints

Trajectory control using position setpoints

14 thoughts on “PX4 Software-In-The-Loop(SITL) Simulation on Gazebo

  1. Okay. I see, thanks. Sorry about asking some naive questions since I am new to the Mavros_SITL. I was wondering if you can show me the .py code or terminal about “sending position setpoints” setting. I know how to do the “arm” and “takeoff”, but have no idea of doing “sending points” Python. (If you implement all of these by c++, I think I can read your next article “PX4 Offboard Control Using MAVROS on ROS”). Thanks again!

    Like

  2. I am facing this probelm to run firmware.
    — Detecting CXX compile features
    — Detecting CXX compile features – done
    — Check for working C compiler using: Ninja
    — Check for working C compiler using: Ninja — works
    — Detecting C compiler ABI info
    — Detecting C compiler ABI info – done
    — Detecting C compile features
    — Detecting C compile features – done
    — ccache enabled (export CCACHE_DISABLE=1 to disable)
    — Found PythonInterp: /usr/bin/python (found version “2.7.12”)
    — Found PY_jinja2: /usr/local/lib/python2.7/dist-packages/jinja2
    — PX4 ECL: Very lightweight Estimation & Control Library v0.9.0-608-gacde4eb
    — Configuring done
    — Generating done
    — Build files have been written to: /home/sarvesh/src/Firmware/build/px4_sitl_default
    ninja: Entering directory `/home/sarvesh/src/Firmware/build/px4_sitl_default’
    [21/696] git submodule src/drivers/gps/devices
    [24/696] git submodule src/lib/DriverFramework
    [25/696] git submodule src/lib/ecl
    [26/696] git submodule mavlink/include/mavlink/v2.0
    [696/696] Linking CXX shared library s…s/dyn_hello/examples__dyn_hello.px4mod
    make[1]: Leaving directory ‘/home/sarvesh/src/Firmware’

    Like

  3. Hi everyone,

    I am using this simulation. And I want to add second drone on simulation. But when I add second drone on gazebo. Gazebo stops the running. What can be problem ? How can I add second drone on gazebo ?

    Best Regards.

    Like

  4. Hi, currently I am running typhoon and it has a blue cone towards him. Did you know if there is a way to remove this? Thanks in advance

    Like

  5. Hi, currently I am running typhoon and it has a blue cone towards him. Did you know if there is a way to remove this? Thanks in advance

    Like

Leave a comment