Main Content

PX4 Autopilot and NVIDIA Jetson in Hardware-in-the-Loop (HITL) Simulation with UAV Dynamics Modeled in Simulink

This example shows how to use the UAV Toolbox Support Package for PX4® Autopilots to verify an autonomous algorithm deployed on NVIDIA® Jetson™ as Onboard Computer along with Pixhawk® hardware board, in HITL mode with UAV Dynamics contained in Simulink®.

For Autonomous algorithms which are computationally intensive, you can use an Onboard Computer on the drone along with the Autopilot. NVIDIA Jetson is a commonly used Onboard Computer for drones. Using Simulink, you can design a complex Autonomous algorithm and deploy the same on NVIDIA Jetson.

This example also showcases 3D scenario simulation during the flight and streaming the image to the onboard computer using the simulated camera sensor. Unreal Engine® simulation environment is used for the 3D scenario Simulation and visualization. This image can be received in NVIDIA Jetson and can be used for scene aware intelligent path planning.

In summary, in this example you,

  • Enable onboard computer workflows with PX4 HITL.

  • Implement a PX4 path planning interface in Simulink and deploy on NVIDIA Jetson.

  • Enable flight visualization with PX4 HITL and stream simulated camera image to NVIDIA Jetson.

  • Run and complete a UAV mission with onboard computer.

Limitation: The Unreal Engine simulation environment is supported only on Microsoft® Windows® system. If you are using a Linux® system, skip adding the 3D scenario simulation step and still be able to complete this example.

Prerequisites

It is recommended to understand the PX4 Autopilot in Hardware-in-the-Loop (HITL) Simulation with UAV Dynamics in Simulink example.

Required Third-Party Software This example requires this third-party software:

Required Hardware To run this example, you will need the following hardware:

  • Pixhawk Series flight controller

  • Micro USB type-B cable

  • Micro-SD card

  • Pixhawk serial port connectors

  • NVIDIA Jetson & power adaptor

  • Host PC Configured with MATLAB supported GPU. It is recommend to use GPU with compute capability of more than 5.

Workflow to Run Model on NVIDIA Jetson Along with Pixhawk in HITL Mode

The above diagram illustrates the PX4 and NVIDIA Jetson HITL setup and the physical communication between various modules.

This example uses four different Simulink models.

  • Simulink model for Flight Controller to be deployed on PX4 Autopilot.

  • Simulink model for UAV Dynamics and sensor simulation.

  • Simulink model for Autonomous algorithm to be deployed on NVIDIA Jetson.

  • Simulink model for flight visualization with Unreal Engine Simulation for Unmanned Aerial Vehicles.

To avoid performance degradation in MATLAB® due to three different Simulink models running at the same time, launch three separate sessions of same MATLAB.

  • In the first session of MATLAB, the Flight Controller is deployed on Autopilot and the UAV Dynamics model will run on host computer communicating with Autopilot.

  • In the second session of MATLAB, the Simulink model for flight visualization with Unreal Engine Simulation will be running. This can be skipped if you opt to not add the flight visualization.

  • In the third session of MATLAB, the Simulink model for NVIDIA Jetson communicates with MATLAB on host computer using Monitor & Tune Simulation.

Step 1: Make Hardware Connections and setup the Pixhawk in HITL mode

1. Connect your Pixhawk board to the host computer using the USB cable.

2. Ensure that you have configured the Pixhawk board in HITL mode as documented in Setting Up PX4 Autopilot in Hardware-in-the-Loop (HITL) Mode from QGroundControl.

3. Ensure that you have setup the PX4 Firmware as mentioned in Set Up PX4 Firmware for Hardware-in-the-loop (HITL) Simulation.

4. Setup and configure your NVIDIA Jetson on network using MATLAB Coder Support Package for NVIDIA Jetson and NVIDIA DRIVE Platforms.

Step 2: Launch First Session of MATLAB and the MATLAB Project

The support package includes an example MATLAB project having the PX4 flight controller and the UAV to follow the mission set in the QGroundControl (QGC).

1. Open MATLAB.

2. Open the px4demo_HardwareInLoopWithSimulinkPlant MATLAB project.

3. Once the Simulink project is open, click the Project Shortcuts tab on the MATLAB window and click Open Autopilot Controller to launch PX4 Controller named Quadcopter_ControllerWithNavigation.

4. Navigate to Navigation subsystem. This is a Variant Subsystem with guidanceType as the variant control variable. Define guidanceType = 1 in the global workspace to choose the navigation subsystem for this example.

5. In the Project Shortcuts tab, click Open UAV Dynamics to launch the Simulink UAV Dynamics model named UAV_Dynamics_Autopilot_Communication.

6. Open the Simulink Plant model UAV_Dynamics_Autopilot_Communication and configure the serial port. Select the serial port of Pixhawk which is connected to the host computer. Add the following UDP connections of onboard computer in the MAVLink Bridge blocks. For more information, see MAVLink Connectivity for QGC, On-board Computer and Simulink Plant. Double-click the MAVLink Bridge blocks to open the block Parameters dialog box.

a. Add the IP address of onboard computer (NVIDIA Jetson) in the MAVLink Bridge Source and MAVLink Bridge Sink blocks. Ensure that you can ping the NVIDIA Jetson successfully from the host PC. Enter the port number as 14540.

b. Add localhost connection for the flight visualization in the MAVLink Bridge Source block. Enter the port as 25000. Skip this if you are opting to not add the flight visualization.

7. Copy the MATLAB Project Path to clipboard.

Step 3: Configure Simulink Controller Model for HITL Mode

1. Follow the instructions mentioned in Configure Simulink Model for Deployment in Hardware-in-the-Loop (HITL) Simulation.

Note: These steps are not required in the pre-configured model. Perform these steps if you have changed the hardware or not using the pre-configured model.

2. Click Build, Deploy & Start from Deploy section of Hardware tab in the Simulink Toolstrip for the Controller model Quadcopter_ControllerWithNavigation.

The code will be generated for the Controller model and the same will be automatically deployed to the Pixhawk board (Pixhawk 6x in this example).

After the deployment is complete, QGroundControl will be automatically launched.

Note: If you are using Ubuntu®, QGC might not launch automatically. To launch QGC, open Terminal and go to the location where QGC is downloaded and run the following command:

./QGroundControl.AppImage

Step 4: Launch Second Session of MATLAB and Open the Flight Visualization Model

Note: Skip this step if you do not opt for flight visualization with PX4 HITL.

1. Open the second instance of the same MATLAB version. In this MATLAB session, The Simulink model for scenario simulation and flight visualization using unreal environment runs.

Ensure that your Host PC is Configured with MATLAB supported GPU (GPU with compute capability of more than 5).

2. Open the px4demo_HardwareInLoopWithSimulinkPlant MATLAB project.

3. Once the Simulink project is open, click the Project Shortcuts tab on the MATLAB window and click Open 3D Visualization with Unreal Engine to launch the onboard model named Unreal_3DVisualization.

In this model, The MAVLink data from the PX4 Autopilot is received over UDP (port : 25000) and is used to decode the position and attitude data of the UAV. After coordinate conversion, it is then passed to the Simulation 3D UAV Vehicle block for flight visualization.

4. Enable the streaming of simulated depth sensor data in NVIDIA Jetson by updating the variable enableOnboardStreaming to 1.

5. The Simulation 3D Camera block provides the Camera image from the Unreal environment. In this example you stream the depth images from the camera block to NVIDIA Jetson using Video Send block. Double-click block to open the block Parameters dialog box. Add the IP address of onboard computer (NVIDIA Jetson) in the dialog box and click OK.

6. On the Simulation tab, click Run to simulate the model. Once the model start running, you will see the Unreal simulation environment getting launched. A sample screen is shown below.

Step 5: Launch Third Session of MATLAB and the Onboard Model

1. Open the third instance of the same MATLAB version.

2. Navigate to the project path previously copied in Step 2 in current MATLAB.

3. Click on the .prj file to launch the same Project in current MATLAB.

4. In the Project Shortcuts tab, click Open Onboard Algorithm to launch the onboard model named Onboard_Autopilot_Communication.

This model implements the PX4 path planning interface using MAVLink Serializer and MAVLink Deserializer blocks. For more information, see PX4 Path Planning Interface. The MAVLink messages that are exchanged as part of this interface are shown in the following diagram.

5. This example provides a basic onboard logic, which is sending back the received waypoint as the updated waypoint. You can consider designing your own onboard logic after completing this basic example.

6. Navigate to Target hardware resource > Board Parameters, enter the IP address of the NVIDIA Jetson and your login credentials.

7. Double-click the UDP Send block to open the block Parameters dialog box. Enter the IP address of the host PC on which you are running UAV_Dynamics_Autopilot_Communication as Remote IP address. Enter the value for Port as 14580. Ensure that the host computer and the onboard computer are connected to same network.

8. Configure the Network Video Receive block to receive the depth data from Unreal environment. Note that the port number and compression parameters in the Network Video Receive block are same as those of the corresponding Video Send block streaming camera image from Unreal Engine.

Note: If you do not opt for flight visualization with PX4 HITL, comment the Network Video Receive block.

9. Click Monitor & Tune from Run on Hardware section of Hardware tab in the Simulink Toolstrip.

The code will be generated for the Controller model and the same will be automatically deployed to NVIDIA Jetson. NVIDIA Jetson should start communicating with host over Monitor & Tune Simulation.

Note: Ensure that there are no other deployed models from Simulink are running in NVIDIA Jetson. If you are unable to verify, reboot the Pixhawk hardware board before starting the deployment.

The algorithm in NVIDIA Jetson also communicates with the Plant model UAV_Dynamics_Autopilot_Communication over UDP.

Step 6: Run the UAV Dynamics model, Upload Mission from QGroundControl and Fly the UAV

1. In the Simulink toolstrip of the Plant model (UAV_Dynamics_Autopilot_Communication), on the Simulation tab, click Run to simulate the model. Once the plant model starts running, in QGroundControl connection will be established.

2. Set the PX4 parameter COM_OBS_AVOID enabling the PX4 path planning interface. Navigate to Parameters from the main menu and set the COM_OBS_AVOID parameter value to 1.

3. In the Parameters tab, set the COM_DISARM_PRFLT parameter value to -1.

4. Start MAVlink Stream for TRAJECTORY_REPRESENTATION_WAYPOINTS. This MAVLink message stream is required for onboard connectivity and is not started by default on USB (/dev/ttyACM0). To start the stream, run the following Command in the QGC MAVLink shell.

mavlink stream -r 25 -s TRAJECTORY_REPRESENTATION_WAYPOINTS -d /dev/ttyACM0

5. In the QGC, navigate to the Plan View.

6. Create a mission. For information on creating a mission, see Plan View.

After you create a mission, it is visible in QGC.

7. Click Upload button in the QGC interface to upload the mission from QGroundControl.

8. Navigate to Fly View to view the uploaded mission.

9. Start the Mission in QGC. The UAV should follow the mission path.

10. Observe the flight visualization in the Unreal Engine.

11. Go to the Onboard_Autopilot_Communication model and validate the depth image streamed to Jetson during the flight in the Video Viewer (from Computer Vision Toolbox™).

Troubleshooting

  • While Simulating the visualization model in Step 4, you might get any STD exception errors such as, "some module could not be found".

Change the compiler to Microsoft Visual C++ 2019 using mex -setup c++ command to fix the issue.

  • "Avoidance system not available" warning when starting a mission. This warning occurs because the communication between NVIDIA Jetson and PX4 HITL is not established.

Ensure that host PC and NVIDIA Jetson are connected to same network. Try pinging NVIDIA Jetson from the host PC and vice versa. Also, double-check the NVIDIA Jetson IP address entered in the MAVLink Bridge blocks as mentioned in 6th point of Step 2 and Host PC IP address entered in the UDP send block as mentioned in 7th point of Step 5.

  • When you start the Mission the vehicle is not taking off.

Firstly, verify if you are receiving the 'TRAJECTORY_REPRESENTATION_WAYPOINTS' message in the Onboard model. If the 'isNew' output for 'TRAJECTORY_REPRESENTATION_WAYPOINTS' in the MAVLink Deserializer Block related to is always false, it indicates that the message is not being received.

In this case, start MAVlink Stream for TRAJECTORY_REPRESENTATION_WAYPOINTS. This MAVLink message stream is required for onboard connectivity and is not started by default on USB (/dev/ttyACM0). To start the stream, run the following Command in the QGC MAVLink shell.

mavlink stream -r 25 -s TRAJECTORY_REPRESENTATION_WAYPOINTS -d /dev/ttyACM0