Main Content

Monitor and Tune PX4 Host Target Flight Controller with Simulink-Based Plant Model

This example shows how to use the UAV Toolbox Support Package for PX4 Autopilots to verify the controller design using PX4 Host Target versus the simulator designed in Simulink®.

The UAV Toolbox Support Package for PX4 Autopilots enables you to use Simulink to design a flight controller algorithm to stabilize the vehicle based on the current vehicle attitude, position, and velocity and also track the desired attitude using Simulink. The MAVlink blocks in the UAV Toolbox enable you to read and write the MAVLink HIL_* messages and design the plant dynamics.

This example shows how to validate a position controller design on a medium-sized quadrotor plant using a single Simulink model, and then take the same controller and plant model and simulate it with the PX4 source code in Software In The Loop (SITL) simulation.

Prerequisites

  • Perform the initial Setup and Configuration tasks of the support package using Hardware Setup screens. In the Select a PX4 Autopilot and Build Target screen, select PX4 Host Target as the PX4 Autopilot board from the drop-down list.

Model

To get started, launch the Simulink Px4DemoHostTargetWithSimulinkPlant project.

Once the project launches, it loads the required workspace variables and opens the top model.

Model Architecture and Conventions

This project consists of the following models:

  • The top model, QuadcopterSimulation, consists of multiple subsystems and model references.

  • The Plant and Visualization subsystem contains the model reference Quad_Plant_dynamics model reference, which has the Quadcopter plant and sensor dynamics.

  • The px4Demo_FlightController_top is the harness model that contains the FlightController model reference. The model is set up to run with the PX4 source code by creating a PX4 Host Target executable.

  • The Quad_Plant_top harness model contains the Quad_Plant_dynamics model reference. This harness model is set up to run in normal simulation pacing mode in lockstep with the px4Demo_FlightController_top harness model.

The Project shortcuts guide you through the three tasks as you progress through the example.

Task 1: Simulate the Drone

1. Open the example Px4DemoHostTargetWithSimulinkPlant project.

The QuadcopterSimulation model opens when the project starts. You can also open the model by clicking Run Quadcopter full simulation in the Projects Shortcut tab.

2. Navigate through the different subsystems to learn about the model hierarchy and quadcopter dynamics.

3. The Controller and the Plant and Visualization subsystems exchange the HIL_ACTUATOR_CONTROLS, HIL_SENSOR, HIL_GPS, and HIL_STATE_QUATERNION MAVlink messages as described in Integrate Simulator Plant Model Containing MAVLink Blocks with Flight Controller Running on PX4 Host Target.

4. The Commands subsystem provides the desired X, Y, Z coordinates and the yaw values for the quadcopter.

5. To simulate the model, go to the Simulation tab in the Simulink model window and click Run. The lower-left corner of the model window displays status while Simulink prepares to run the model on the host computer.

6. Observe that the quadcopter hovers at an altitude commanded by the Dashboard slider block.

7. Click the Circular Guidance toggle switch to observe the quadcopter following a circular trajectory.

8. You can also bring up the UAV Animation window by clicking on the Show animation button on the UAV Animation block. To locate the block, go to Plant and Visualization > Quad_Plant_Dynamics > Visualization Subsystem.

9. Note that the controller gains are optimized for close tracking of a circular trajectory. It may be possible that for other types of input commands such as step or ramp, the response is not ideal. In such a scenario, the controller gains must be adjusted to meet the specified requirements. For more details on the controller tuning, see Position Tracking for X-Configuration Quadcopter.

In this task, we designed the multicopter plant and controller, and ensured that its performance is satisfactory in simulation.

Task 2: Deploy Controller as Host Target and Run Plant Model in Simulink

In this task, we use the controller designed in Task 1 and run it with PX4 source code using the PX4 Host Target feature (for more information, see Integrate Simulator Plant Model Containing MAVLink Blocks with Flight Controller Running on PX4 Host Target). Use two separate instances of MATLAB to run controller and plant model.

Step 1: Launch Controller Model

1. Click the Monitor and Tune Host Target controller in the Project Shortcuts tab to open the px4Demo_FlightController_top model.

The model block references the FlightController model, which was used in the QuadcopterSimulation model.

The uORB Read blocks are used to subscribe to the vehicle_local_position_groundtruth and vehicle_attitude_groundtruth topics. These topics contain data sent by the simulator plant model using HIL_STATE_QUATERNION and HIL_GPS messages.

The Signal Conditioning subsystem inside the FlightController model reference extracts the current position, current velocity, and current attitude data and feeds it to the Controller subsystem. The Controller subsystem designs the Rate controller and the Position and Attitude controller as explained in Position Tracking for X-Configuration Quadcopter Using Rate Controller.

The FlightController model outputs the actuator values that are then fed to the PX4 PWM Output block.

2. Copy the MATLAB Project Path to clipboard.

Step 2: Launch Simulink Plant model in second MATLAB

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

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

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

4. Click Run quadcopter plant in Normal simulation in the Project Shortcuts tab to open the Quad_Plant_top model.

The model block references the Quad_Plant_dynamics model, which was used in QuadcopterSimulation.

Ensure that the Simulation Pacing option for this model is enabled, as described in Integrate Simulator Plant Model Containing MAVLink Blocks with Flight Controller Running on PX4 Host Target.

The TCP read from PX4 Host Target MATLAB System block is used to read the MAVLink data sent from the px4Demo_FlightController_top model.

The Enabled subsystem has the MAVLink Deserializer block that extracts the HIL_ACTUATOR_CONTROLS message.

The TCP write to PX4 Host Target MATLAB System block sends the HIL_SENSOR, HIL_GPS, and HIL_STATE_QUATERNION MAVLink messages to the px4Demo_FlightController_top model.

Step 3: Deploy controller model over Monitor & Tune and run Plant Simulation

1. In Configuration parameters > Hardware Implementation, set the Hardware board parameter to PX4 Host Target.

2. Under Target hardware resources > Build Options, set Simulator to Simulink.

3. In the Simulation tab, set the Simulation Stop time to inf.

4. On the Hardware tab, in the Mode section, select Run on board and then click Monitor & Tune to start signal monitoring and parameter tuning.

5. Wait for Simulink to complete the code generation. The following dialog box appears. Do not click OK yet.

In the plant model launched in second MATLAB, follow the below steps.

1. In the Simulation tab, set the Simulation Stop Time to inf.

2. Click Run on the Simulation Tab.

After the simulation starts in the plant model, click OK in the dialog box of the first model.