Control application development

1. uav_control code framework

The program execution flowchart is shown below.

2. uav_control topics

uav_control contains two functional modules: uav_controller (UAV control) and uav_estimator (UAV state). These modules contain all the topics used by uav_control. The use of each topic is described in detail in the code comments, making its purpose easy to understand at a glance. See the code for details.

3. uav_control parameters

Be sure to check which YAML file is loaded by the running uav_control program. Edit the corresponding YAML file to change the parameters.

If you use the ground control station to change parameters, note that the ground control station has its own set of default parameters. These parameters must be changed through the ground control station parameter file, which is specifically located in param under the hidden .ros folder.

4. uav_control messages

uav_control uses topics, and each topic has a corresponding message. Half of the messages are under common. The message files also contain detailed comments explaining their purpose; refer to them when using the messages.

The remaining messages use px4_msgs, which is in the specified folder. These messages are mainly related to PX4 topics. They also contain detailed comments; refer to them when using the messages.

5. uav_control application development demos

These control demos show users how to call the control interface. The demos are located in tutorial_demo.

As shown above, this example implements inertial-frame control. A detailed explanation follows.

Detailed analysis of enu_xyz_pos_control.cpp

1. Example overview

---

2. Overall architecture

This example is a ROS 2 node. It uses a state machine + timer polling to drive the mission flow without blocking the main thread.

┌─────────────────────────────────────────────────────────────┐
│              EnuXyzPosControlDemo (rclcpp::Node)             │
├─────────────────────────────────────────────────────────────┤
│  Subscribe                     │  Publish                      │
│  /uav{N}/prometheus/state      │  /uav{N}/prometheus/command  │
│  /uav{N}/prometheus/control_state                             │
├─────────────────────────────────────────────────────────────┤
│  100ms timer tick() → Phase state machine → publish UAVCommand│
└─────────────────────────────────────────────────────────────┘
                              │
                              ▼
                    uav_control controller node
                    (receives command, executes MOVE/LAND)

---

3. Dependencies and message types

3.1 Header files

3.2 Key message fields used in this example

UAVCommand (published):

agent_cmd: MOVE / LAND
move_mode: XYZ_POS          # Inertial-frame position control
position_ref[3]: [x, y, z]  # Target position [m]
yaw_ref: Target yaw angle [rad]
command_id: Increments to prevent duplicate/out-of-order commands
header.frame_id: "ENU"

UAVControlState (subscribed):

control_state:
  INIT = 0
  RC_POS_CONTROL = 1
  COMMAND_CONTROL = 2      # ← This example sends commands only in this mode
  LAND_CONTROL = 3

UAVState (subscribed):

position[3]: Current ENU position [m]  # Used to determine the takeoff altitude and whether the target has been reached

---

4. The EnuXyzPosControlDemo class

4.1 Constructor flow

EnuXyzPosControlDemo(const rclcpp::NodeOptions &options)
    : Node("enu_xyz_pos_control", options)

During construction, the following steps are completed in order:

1. Initialize the shared parameter singleton
   SharedParamFile::instance() — Uses the parameter file shared with other project nodes.

2. Declare and read ROS parameters (with default values):

   Parameter	Type	Default value	Meaning
   uav_id	int	1	UAV ID
   target_x	double	1.0	Target ENU-X [m]
   target_y	double	0.0	Target ENU-Y [m]
   target_z	double	1.0	Target ENU-Z [m]
   target_yaw	double	0.0	Target yaw [rad]

3. Shared parameter overrides

   • uav_id can be overridden by GET_SHARED_PARAM("uav_id", ...)
   • takeoff_height_ is read from /uav_control_main_{id}/control/Takeoff_height (default: 1.0 m)

4. Build topic names
   uav_name_ = "/uav" + std::to_string(uav_id_)
   For example, uav_id=1 → /uav1/prometheus/command

5. Create publishers/subscribers (all are class member variables)

6. Print operating instructions (use remote controller switch SWA to unlock and switch SWB to COMMAND_CONTROL)

7. Start a 100 ms wall timer with tick() as its callback

4.2 ROS topic interfaces

---

5. The Phase state machine

The example uses the Phase enum to describe mission phases. tick() advances it every 100 ms.

stateDiagram-v2
    [*] --> WAIT_FOR_COMMAND_CONTROL
    WAIT_FOR_COMMAND_CONTROL --> WAIT_TAKEOFF_STABLE: Entered COMMAND_CONTROL
    WAIT_TAKEOFF_STABLE --> SENT_MOVE: |z - takeoff_height| < 0.3m
    SENT_MOVE --> WAIT_REACH_TARGET: Next tick
    WAIT_REACH_TARGET --> HOVERING_30S: Distance to target ≤ 0.3m
    HOVERING_30S --> DONE: Hovered for 30s; publish LAND
    DONE --> [*]: rclcpp::shutdown()

Safety logic: If the system leaves COMMAND_CONTROL while the mission is in progress (except during the initial wait or after completion), it prints a red error and calls shutdown to prevent commands from continuing to be sent in the wrong mode.

---

6. Analysis of core functions

6.1 distance3() — 3D Euclidean distance

static double distance3(double x1, double y1, double z1, double x2, double y2, double z2)
{
    const double dx = x1 - x2;
    const double dy = y1 - y2;
    const double dz = z1 - z2;
    return std::sqrt(dx * dx + dy * dy + dz * dz);
}

Used to determine the distance between the UAV's current position and (target_x_, target_y_, target_z_).

6.2 publish_move_xyz_pos() — Publish a position-control move command

void publish_move_xyz_pos()
{
    uav_command_.header.stamp = this->now();
    uav_command_.header.frame_id = "ENU";
    uav_command_.agent_cmd = prometheus_msgs::msg::UAVCommand::MOVE;
    uav_command_.move_mode = prometheus_msgs::msg::UAVCommand::XYZ_POS;
    uav_command_.position_ref[0] = static_cast<float>(target_x_);
    uav_command_.position_ref[1] = static_cast<float>(target_y_);
    uav_command_.position_ref[2] = static_cast<float>(target_z_);
    uav_command_.yaw_ref = static_cast<float>(target_yaw_);
    uav_command_.command_id += 1;
    uav_command_pub_->publish(uav_command_);
}

Key points:

• MOVE + XYZ_POS = specify an (x,y,z) position setpoint in the inertial frame
• frame_id = "ENU" identifies the reference frame
• Each publication increments command_id, consistent with the design of UAVCommand.msg, to prevent the controller from ignoring old or duplicate packets

6.3 publish_land_and_finish() — Land and terminate the node

void publish_land_and_finish()
{
    uav_command_.header.stamp = this->now();
    uav_command_.header.frame_id = "ENU";
    uav_command_.agent_cmd = prometheus_msgs::msg::UAVCommand::LAND;
    uav_command_.command_id += 1;
    uav_command_pub_->publish(uav_command_);

    phase_ = Phase::DONE;
    rclcpp::shutdown();
}

After LAND is published, the controller switches from COMMAND_CONTROL to LAND_CONTROL, which is part of the normal process.

6.4 tick() — Main-logic heartbeat

It runs every 100 ms with the following structure:

1. Check rclcpp::ok()
2. Determine whether the system is in COMMAND_CONTROL
   • No → wait or exit due to an error
   • Yes → use switch(phase_) to advance the state machine

Takeoff stability determination (WAIT_TAKEOFF_STABLE):

const double z = static_cast<double>(uav_state_.position[2]);
if (std::fabs(z - static_cast<double>(takeoff_height_)) >= 0.3)
    break;  // Altitude difference ≥ 0.3m; continue waiting

Implicit prerequisite: The user has already used the remote controller to take off. The MOVE command is sent only after the altitude approaches the configured Takeoff_height; the example does not send a takeoff command itself.

Arrival determination (WAIT_REACH_TARGET):

• Threshold: 0.3 m (the same as the takeoff stability threshold)
• After arrival, hover_deadline_ = now + 30s is set

---

7. Member variables

---

8. The main() entry point

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<EnuXyzPosControlDemo>(rclcpp::NodeOptions());
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

Standard ROS 2 node lifecycle: init → create the node → spin to process callbacks and timers → shutdown.

---

9. Runtime sequence (user perspective)

Timeline ─────────────────────────────────────────────────────────────►

[Start node] → Print a prompt (wait for RC unlock + COMMAND_CONTROL)
     │
     ▼
[User takes off via RC] → Altitude approaches takeoff_height (±0.3m)
     │
     ▼
[Automatic] Publish MOVE (XYZ_POS) → Fly to (target_x, target_y, target_z)
     │
     ▼
[Automatic] Distance ≤ 0.3m → Hover for 30 seconds
     │
     ▼
[Automatic] Publish LAND → Node shutdown

---

10. Launch configuration mapping

The default parameters in launch/enu_xyz_pos_control_launch.py match those in the code:

{"uav_id": 1},
{"target_x": 1.0},
{"target_y": 0.0},
{"target_z": 1.0},
{"target_yaw": 0.0},

This means the UAV flies from near the takeoff altitude of approximately (0,0,1) to the ENU point (1, 0, 1) with a yaw of 0 rad.

Launch example:

ros2 launch prometheus_demo enu_xyz_pos_control_launch.py

The simulation scripts are located in Scripts/simulation/tutorial_demo/enu_xyz_pos_control/ (for P230 / P450 / P600 and other models).

---

11. Relationship to UAVCommand control modes

This example uses the following combination of Prometheus high-level interfaces:

For velocity control, body-frame position control, trajectory tracking, and other functions, simply change move_mode and the corresponding *_ref fields. The state-machine framework can be reused.

Other move_mode enum values (from UAVCommand.msg):

---

12. Summary

enu_xyz_pos_control.cpp is the most basic ENU position-control example in Prometheus. It subscribes to state / control_state and publishes UAVCommand. After confirming that the system is in COMMAND_CONTROL and that the takeoff altitude is stable, it flies to the parameter-defined target in XYZ_POS mode, hovers for 30 seconds, then executes LAND and exits. Understanding this file provides a template for writing more complex missions, such as multi-waypoint missions, formations, and actual-aircraft adaptation.

---

Related files