Fixed Wing position control using v1.15 flight modes

PX4 Autopilot
1

Hi! I am using the new flight modes feature from release v.1.15 implemented in a C++ node which publishes position setpoints (TrajectorySetpoints).

I have successfully implemented this for a quadcopter in simulation, and all is well. The flight mode seems to be called correctly and the drone flies as expected.

However, when switching to a fixed wing aircraft the aircraft does not respond to the messages. It slowly turns to the left instead of going in the direction of the target.

I have a difficulty understanding why…

My setpoint has the following implementation:

void PositionSetpointType::update(const Eigen::Vector3f &position_ned_m)
  {
    onUpdate();

    px4_msgs::msg::TrajectorySetpoint sp{};
    sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;

    sp.position[0] = position_ned_m.x();
    sp.position[1] = position_ned_m.y();
    sp.position[2] = position_ned_m.z();
    sp.velocity[0] = sp.velocity[1] = sp.velocity[2] = NAN;
    sp.acceleration[0] = sp.acceleration[1] = sp.acceleration[2] = NAN;
    sp.jerk[0] = sp.jerk[1] = sp.jerk[2] = NAN;
    sp.yaw = NAN;
    sp.yawspeed = NAN;

    _trajectory_setpoint_pub->publish(sp);
  }

  SetpointBase::Configuration PositionSetpointType::getConfiguration()
  {
    Configuration config{};

    config.rates_enabled = true;
    config.attitude_enabled = true;
    config.acceleration_enabled = true;
    config.velocity_enabled = true;
    config.position_enabled = true;
    config.altitude_enabled = true;
    config.climb_rate_enabled = true;

    return config;
  }

I have read this: https://docs.px4.io/main/en/flight_modes/offboard.html and noted that trajectory set point is not mentioned for fixed wing: https://docs.px4.io/main/en/flight_modes/offboard.html#fixed-wing

However some people have mentioned that this should be supported: https://github.com/PX4/PX4-Autopilot/pull/12532

How do I achieve this? Do I need to send SET_POSITION_TARGET_LOCAL_NED? How to achieve that from the new format of flight modes in v1.15 (which I generally like)?

Or should I start using offboard mode instead?

2

@Ejemyr You have enabled acceleration and velocity setpoints, but are sending NANs

3

Did you find a solution to this issue?

4

@ahmad.zain I already pointed out what the problem was :slight_smile:

(You should not send Nans for an enabled setpoint)

5

Hello @Jaeyoung-Lim

I saw your reply, but I was not passing nan, I was passing zeros for all setpoints besides the position setpoint in a trajectory_setpoint message. And in the offboard_control_mode message, I also had all setpoints set as False except for position, with the same issue as the original question of the plane either not responding or just ignoring the message with no effect on flight trajectory (or anything for simple observation). I managed to figure it out though, it was quite interesting.

In src/modules/fw_pos_control/FixedwingPositionControl.cpp line 2565, it checks if the passed velocity vector is finite Vector3f(trajectory_setpoint.velocity).isAllFinite(), and if this returns true, it adds a velocity setpoint to the current portion in the position_setpoint_triplet message, and for some reason this seems to conflict or override the set position target (I have no idea why).

For me, the solution was to actually PASS nan in the trajectory_setpoint message for velocity, and pass whatever value I wanted for position. This gets the finite check to fail, which keeps the current setpoint from being modified at the line mentioned above. This got the plane to respond in the gazebo-classic_plane simulation. I had some issues with takeoff, but when that worked, I managed to pass as many positions as I wanted, and the plane managed to go to all of them and then loiter as expected.

6

In src/modules/fw_pos_control/FixedwingPositionControl.cpp line 2565, it checks if the passed velocity vector is finite Vector3f(trajectory_setpoint.velocity).isAllFinite() , and if this returns true, it adds a velocity setpoint to the current portion in the position_setpoint_triplet message, and for some reason this seems to conflict or override the set position target (I have no idea why).

@ahmad.zain Because the interface is intended to follow a path, if you send position and velocity together. The guidance controller will follow the path if you send the closest point and tangent of the path.

7

@Jaeyoung-Lim You have mentioned that the guidance controller will follow a path if position and velocity are sent in the trajectory_setpoint message. But from the documentation it seems for px4 1.15 that velocity setpoints are not accepted at all for Fixedwing. So which case is true?

I am trying to control a standard_vtol in fixedwing mode using position control. The position setpoints are met decently but I would like to know if adding velocity setpoints helps or are they completely neglected?

8

@vish The velocity setpoints are not used to specify airspeed, but to specify the tangent of the path the vehicle is following.

Pure velocity-based offboard control needs to be added. Please consider contributing if you are interested.

9

@Jaeyoung-Lim thank you, I would love to contribute to pire velocity control in offboard mode in the near future!

For now, I am trying to control the way the vehicle reaches a particular setpoint rather than it just reaching the setpoint- which is possible with velocity setpoints in MC mode. What would be the best way to achieve this in fixed wing?

Would attitude control be the best approach or position control with position + velocity setpoints?
Or would it be better to define smooth trajectories based on fixedwing dynamics?

If you also know some work already done , that could be a good starting point.
Any help would be much appreciated