Issue with SITL Fixed-Wing Takeoff Command: "Already higher than takeoff altitude (not descending)"

PX4 Autopilot
1

Hi everyone,

I’m experiencing an issue with fixed-wing takeoffs in SITL when publishing a takeoff command via ROS2. When I send a VehicleCommand.msg to the /fmu/in/vehicle_command topic with a ROS2 node, the aircraft barely leaves the ground and I see the warning:

“Already higher than takeoff altitude (not descending).”

I am documenting my troubleshooting process in hopes of getting some insights or suggestions on what might be causing this issue.

Environment and Setup

  • OS: Ubuntu 22.04
  • PX4 Autopilot Version: branch release/1.15
  • QGroundControl Version: v4.4.4
  • ROS2 Node: Custom Python node that publishes VehicleCommand messages to /fmu/in/vehicle_command
  • Relevant QoS Settings:
    where px4_qos_pub is gathered from here and defined as:
    px4_qos_pub = QoSProfile(
    reliability=QoSReliabilityPolicy.BEST_EFFORT,
    durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
    history=QoSHistoryPolicy.KEEP_LAST,
    depth=0
)

Steps to Reproduce

  1. Launch QGroundControl.

  2. Launch SITL:

    make px4_sitl gz_rc_cessna

  3. Arm the fixed-wing.
    (this can be done using the GUI in QGroundControl or via a published VehicleCommand message)

  4. Publish a VehicleCommand message for takeoff.
    (basic example below gives the general idea):

    altitude = 30 # changing this value does not change the behavior
min_pitch_in_radians = np.deg2rad(8)

msg = VehicleCommand()
msg.command = 22    # VEHICLE_CMD_NAV_TAKEOFF
msg.param1 = min_pitch_in_radians
# msg.param4 = yaw_angle
# msg.param5 = latitude
# msg.param6 = longitude
msg.param7 = altitude

try:
    self.vehicle_command_pub.publish(msg)
except Exception as e:
    print(f"Failed to publish takeoff command: {e}")

Result

  1. Fixed-wing transitions from NAV state 2 (Position Mode) into NAV state 17 (Takeoff Mode).
  2. Error Message: “Already higher than takeoff altitude (not descending)”
  3. Aircraft takes off and immediately reaches clearance altitude: “Reached clearance altitude”
  4. Aircraft transitions from NAV state 17 (Takeoff Mode) to NAV state 4 (Hold Mode).
  5. Aircraft loiters at Hold Mode altitude set by NAV_MIN_LTR_ALT (defaults to -1.0m).

Log Example

I’ve logged a basic example of this behavior. I let the aircraft stay in hover mode for a minute before sending a mission command to perform a landing (VEHICLE_CMD_DO_LAND_START = 189). The flight log can be found here: https://review.px4.io/plot_app?log=6190b665-9b99-4714-8567-3cd948eaa2d5.

ROS2 Node

I’ve setup a fairly simple ROS2 node in python with a publisher:

self.vehicle_command_pub = self.create_publisher(
            VehicleCommand,
            self.ns + "/fmu/in/vehicle_command",
            px4_qos_pub
        )

The node contains a variety of services (arm, disarm, takeoff, start mission, return-to-launch, land, etc.) which, when called, simply use the above publisher to post the appropriate VehicleCommand message from px4_msgs to the /fmu/in/vehicle_command topic. The code I posted above is the main, non-boilerplate code for the takeoff service (i.e., ignoring the setup of the service and the callback function) and is typical of all my service calls and works without an issue. I have had no issues publishing any VehicleCommand messages via this method and all changes in arming or flight modes are accurately reflected in QGroundControl. If it is helpful, I can post the entire file, but it really isn’t anything fancy.

Workarounds

Ultimately, I’m not here looking for a workaround, though in the meantime, I’ve found these two approaches work.

1. Hold Mode

Because the aircraft goes straight into the Hold Mode after takeoff, you can set the NAV_MIN_LTR_ALT parameter to something like 30.0 meters which will help the aircraft climb to altitude.

2. Mission Mode

If you plan a mission in QGroundControl, you can publish a VehicleCommand to enter Mission Mode (VEHICLE_CMD_MISSION_START = 300, NAV state 3) instead of Takeoff Mode (VEHICLE_CMD_NAV_TAKEOFF = 22, NAV state 17). The aircraft has no problem taking off.

Digging into this, it seems that a mission mode takeoff is slightly different from a takeoff using VEHICLE_CMD_NAV_TAKEOFF. Indeed, the documentation quoted below appears to insinuate that a takeoff via Mission Mode is done with a MAVLink backend as opposed to a uORB message interface.

“The takeoff behavior is defined in a Takeoff mission item, which corresponds to the MAV_CMD_NAV_TAKEOFF MAVLink command. During mission execution the vehicle will takeoff towards this waypoint, and climb until the specified altitude is reached. The mission item is then accepted, and the mission will start executing the next item.”

This could explain the differences in behavior with a Mission Mode takeoff item and Takeoff Mode.

Source Code

Tracking down the error, I’ve found the code responsible in this file. It seems there is some logic within the set_takeoff_position() function responsible for verifying if the aircraft is already at an altitude above the suggested takeoff altitude (e.g., if the aircraft were already in flight).

However, there also seems to be some logic that I’m not following. It seems like the code sets the takeoff altitude to be the current altitude

if (rep->current.valid && PX4_ISFINITE(rep->current.alt)) {
    takeoff_altitude_amsl = rep->current.alt;
} else {
...
}

only to then later check if the takeoff altitude is less than the current altitude?

if (takeoff_altitude_amsl < _navigator->get_global_position()->alt) {
    // If the suggestion is lower than our current alt, let's not go down.
    takeoff_altitude_amsl = _navigator->get_global_position()->alt;
    mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude\t");
    events::send(events::ID("navigator_takeoff_already_higher"), {events::Log::Error, events::LogInternal::Info},
        "Already higher than takeoff altitude (not descending)");
	}

I wonder if the way this is setup is somehow causing this issue to appear.

Anyway, I would love some with a bit more experience with the code base to take a look at this. Not sure where I might be going wrong.