Velocity and Position Control in Offboard Mode

Hello all,
I am using Ubuntu 22.04, PX4 v1.14.2 and communicating through MicroDDS.
I am trying to offboard control the standard vtol in SITL using Gazebo such that I can send both position coordinates in the local frame and velocity setpoints, and have it transition at a certain velocity. To offboard control the drone I am trying to use the TrajectorySetpoint message, and position + velcoity control is working, however the vtol will not speed up faster than 7.7 m/s. The minimum blending speed for the VTOL is 8 m/s and the transition speed is 10 m/s (as found in the documentation) so I am not sure how I am supposed to transtion into FW from MC if I can’t get past 7.7 m/s. I have tried changing parameters in PX4, sending higher velocities, and sending crazy high velocites so that it just “maxes out” and still cannot get higher than 7.7 m/s. I know there is probably an easy fix out there just unsure of where. I have included my code down below.

#!/usr/bin/env python3
import rclpy 
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleLocalPosition, VehicleStatus, VehicleOdometry, VehicleCommandAck
import numpy as np
class OffboardControl(Node):
        """ Node for controlling a vehicle in offboard mode. """

        def __init__(self) -> None:

                # Configure QoS profile for publishing and subscribing
                qos_profile = QoSProfile(reliability = ReliabilityPolicy.BEST_EFFORT, 
                                         durability = DurabilityPolicy.TRANSIENT_LOCAL,
                                         history = HistoryPolicy.KEEP_LAST, 
                                         depth = 1)
                # Create publishers
                self.offboard_control_mode_publisher = self.create_publisher(
                        OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
                self.trajectory_setpoint_publisher = self.create_publisher(
                        TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile)
                self.vehicle_command_publisher = self.create_publisher(VehicleCommand, '/fmu/in/vehicle_command', qos_profile)

                # Create subscribers
                self.vehicle_local_position_subscriber = self.create_subscription(
                        VehicleLocalPosition, '/fmu/out/vehicle_local_position', self.vehicle_local_position_callback, qos_profile)
                self.vehicle_status_subscriber = self.create_subscription(
                        VehicleStatus, '/fmu/out/vehicle_status', self.vehicle_status_callback, qos_profile)
                self.vehicle_odometry_subscriber = self.create_subscription(
                        VehicleOdometry, '/fmu/out/vehicle_odometry', self.vehicle_odometry_callback, qos_profile)
                self.vehicle_command_ack_subscriber = self.create_subscription(
                        VehicleCommandAck, '/fmu/out/vehicle_command_ack', self.vehicle_command_ack_callback, qos_profile)
                # Initialize variables
                self.offboard_setpoint_counter = 0
                self.vehicle_local_position = VehicleLocalPosition()
                self.vehicle_status = VehicleStatus()
                self.vehicle_odometry = VehicleOdometry()
                self.vehicle_command_ack = VehicleCommandAck()

                self.vehicle_vel_x = self.vehicle_odometry.velocity

                self.takeoff_height = -30.0

                #Create a timer to publish control commands
                self.timer = self.create_timer(0.1, self.timer_callback)

                self.taken_off = None
                self.first_setpoint = None
                self.second_setpoint = None
                self.landed = None

        def vehicle_local_position_callback(self, vehicle_local_position):
                """Callback function for vehicle_local_position topic subscriber"""
                self.vehicle_local_position = vehicle_local_position
        def vehicle_status_callback(self, vehicle_status):
                """Callback function for vehicle_status topic subscriber."""
                self.vehicle_status = vehicle_status

        def vehicle_odometry_callback(self, vehicle_odometry):
                """Callback function for vehicle_status topic subscriber."""
                self.vehicle_odometry = vehicle_odometry

        def vehicle_command_ack_callback(self, vehicle_command_ack):
                """Callback function for vehicle_status topic subscriber."""
                self.vehicle_command_ack= vehicle_command_ack

        def arm(self):
                """Send an arm command to the vehicle."""
                        VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=1.0)
                self.get_logger().info('Arm command sent')
        def disarm(self):
                """Send a disarm command to the vehicle."""
                        VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=0.0)
                self.get_logger().info('Disarm command sent')

        def engage_offboard_mode(self):
                """Switch to offboard mode."""
                        VehicleCommand.VEHICLE_CMD_DO_SET_MODE, param1=1.0, param2=6.0)
                self.get_logger().info("Switching to offboard mode")

        def land(self):
                """Switch to land mode."""
                self.get_logger().info("Switching to land mode")

        def publish_offboard_control_heartbeat_signal(self):
                """Publish the offboard control mode."""
                msg = OffboardControlMode()
                msg.position = True 
                msg.velocity = True
                msg.acceleration = False
                msg.attitude = False
                msg.body_rate = False
                msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)

        def publish_position_setpoint(self, x: float, y: float, z: float):
                """Publish the trajectory setpoint."""
                msg = TrajectorySetpoint()
                msg.position = [x, y, z]
                msg.yaw = 1.57079  # (90 degree)
                msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
                self.get_logger().info(f"Publishing position setpoints {[x, y, z]}")

        def publish_vehicle_command(self, command, **params) -> None:
                """Publish a vehicle command."""
                msg = VehicleCommand()
                msg.command = command
                msg.param1 = params.get("param1", 0.0)
                msg.param2 = params.get("param2", 0.0)
                msg.param3 = params.get("param3", 0.0)
                msg.param4 = params.get("param4", 0.0)
                msg.param5 = params.get("param5", 0.0)
                msg.param6 = params.get("param6", 0.0)
                msg.param7 = params.get("param7", 0.0)
                msg.target_system = 1
                msg.target_component = 1
                msg.source_system = 1
                msg.source_component = 1
                msg.from_external = True
                msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)

        def publish_velocity_setpoint(self, vx, vy, vz):
                """Publish a velocity setpoint."""
                msg = TrajectorySetpoint()
                msg.position = [np.nan, np.nan, np.nan]
                msg.velocity = [vx, vy, vz]
                msg.acceleration = [np.nan, np.nan, np.nan]
                msg.jerk = [np.nan, np.nan, np.nan]
                msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
                self.get_logger().info(f"Publishing velocity setpoint: [{vx}, {vy}, {vz}]")

        def publish_vehicle_command_vtol_transition(self, transition_type) -> None:

                self.publish_vehicle_command(command = VTOL_TRANSITION_COMMAND,
                                             param1 = float(transition_type),
                                             target_system = 1, 
                                             target_component = 1,
                                             source_system = 1,
                                             source_component = 1,
                                             from_external = True)
                self.get_logger().info(f"VTOL transition command sent. Transition Type: {transition_type}")

        def timer_callback(self) -> None:
                """Callback function for the timer."""


                if self.offboard_setpoint_counter == 10:

                if self.vehicle_local_position.z > self.takeoff_height and self.vehicle_status.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD and self.taken_off is None:
                    print("publishing takeoff")
                    self.publish_position_setpoint(0.0, 0.0, self.takeoff_height)
                    self.publish_velocity_setpoint(0.0, 0.0, -2.50)
                    if self.vehicle_local_position.z < self.takeoff_height + 0.5:
                        self.taken_off = True
                        print("taken off")

                if self.vehicle_local_position.z < self.takeoff_height + 0.2 and self.first_setpoint is None:
                    self.publish_position_setpoint(400.0, 0.0, self.takeoff_height)
                    self.publish_velocity_setpoint(10.0, 0.0, 0.0)
                    if self.vehicle_local_position.x >= 39.5: 
                           self.first_setpoint = True 
                           print("first setpoint")

                if self.offboard_setpoint_counter < 11:
                       self.offboard_setpoint_counter += 1
def main(args=None) -> None:
        print('Starting offboard control node...')
        offboard_control = OffboardControl()

if __name__ == '__main__':
        except Exception as e:

Hi @gabedavid , there is no need to send velocity and position setpoints in two different trajectory messages.
You can send just one mesage with both fields filled with the desired velocity.
The position controller will use the postion value as main reference and the velocity one as feedforward value.

Moreover, at each iteration you are sending a trajectory message with a 90deg yaw rotation followed by another message with 0deg yaw rotation as you are not disbling with nan the yaw control in your velocity setpoint message.
This could cause inner (rates) controller to saturate as your attitude setpoints keep changing.

Initially, you could send just the “velocity” setpoint and check if that allows your model to reach the desired speed

This worked amazingly, thank you vert much. One small issue was when sending one large trajectory message the velocity was not controlled, and just used the defualt values in the parameters. However, sending two commands is not that big of a deal to me.

1 Like