[offboard] acceleration setpoint is not filtered and affects too much on the thrust setpoint

[squizz617]

Description

When controlling the drone through trajectory setpoints in offboard mode, acceleration setpoints have excessively strong effects on the resulting thrust setpoints, as the position & velocity controller do not honor MPC_ACC_HOR_MAX parameter to limit the input acceleration setpoints. Even a short glitch/noise (e.g., one of the setpoint messages sets acceleration.y to 3000 m/s^2) is enough to disrupt the entire trajectory.

Reproduction

Consider the following offboard mission where each waypoint consists of repeated TrajectorySetpoint ROS messages:

1. Waypoint0 - takeoff (x100)

setpoint_msg = px4_msgs.msg.TrajectorySetpoint()
setpoint_msg.z = -5.0
setpoint_msg.yaw = 0.0

2. Waypoint1 (x50)

setpoint_msg = px4_msgs.msg.TrajectorySetpoint()
setpoint_msg.x = 20.0
setpoint_msg.y = 10.0
setpoint_msg.z = -5.0
setpoint_msg.yaw = 1.0

3. Waypoint2 (x50)

setpoint_msg = px4_msgs.msg.TrajectorySetpoint()
setpoint_msg.x = 35.0
setpoint_msg.y = 10.0
setpoint_msg.z = -5.0
setpoint_msg.yaw = -0.2

4. Waypoint3 (x50)

setpoint_msg = px4_msgs.msg.TrajectorySetpoint()
setpoint_msg.x = 25.0
setpoint_msg.y = -10.0
setpoint_msg.z = -5.0
setpoint_msg.yaw = -1.8

5. Waypoint4 - return (x50)

setpoint_msg = px4_msgs.msg.TrajectorySetpoint()
setpoint_msg.x = 0.0
setpoint_msg.y = 0.0
setpoint_msg.z = -5.0
setpoint_msg.yaw = 3.0

Executing the mission by publishing the sequence of messages at 10Hz, the drone draws a quadrilateral trajectory, as shown in flight log01.

However, when one of the waypoint1 messages (22nd message, to be precise) somehow has an error, additionally setting the target y acceleration to 3000 as shown below,

setpoint_msg1.x = 20.0
setpoint_msg1.y = 10.0
setpoint_msg1.z = -5.0
setpoint_msg1.yaw = 1.0
setpoint_msg1.acceleration[1] = 3000

the thrust setpoint over the y axis quickly dominates and blows the drone off, even though the normal messages following the anomalous one quickly resets target acceleration to zero, as shown in flight log02.

Analysis

The velocity controller does not clamp the acceleration setpoint value to the maximum (MPC_ACC_HOR_MAX), but directly use the input setpoint to compute the thrust setpoint. Like how _vel_sp and _thr_sp vectors are constrained to the bounds defined by relevant parameters (e.g., MPC_XY_VEL_MAX, MPC_Z_VEL_MAX_DN, ...), it seems reasonable that acceleration setpoints are also filtered to prevent such anomalous inputs from jeopardizing the mission.

Additional info

Tested on Ubuntu 20.04 running ROS2-foxy, px4_sitl_rtps at v1.12.0 with gazebo 11.5.1.

[Jaeyoung-Lim]

@squizz617 How are you sending the acceleration setpoints? Is this through mavlink? or are you publishing them directly through RTPS?

[squizz617]

@Jaeyoung-Lim I'm publishing directly to /VehicleCommand_PubSubTopic rtps topic from the offboard computer.

[bresch]

Even without saturation of the acceleration setpoint, the maximum acceleration is still somehow constrained by the maximum allowed tilt of the vehicle.

We could also add an acceleration saturation if really needed, but remember that when using this message, the controller will directly execute the setpoints without smoothing them; you get the complete control of the drone so it's also your responsibility to generate a nice (smooth) and feasible trajectory on your companion computer.

If you are not willing to generate a smooth trajectory on the companion computer, it is also possible to send mission waypoints through mavlink. Those can be executed in auto mode (not offboard) and the autopilot will take care of all the vel/accel/jerk constraints to generate a smooth trajectory.

[squizz617]

Hi @bresch, thank you for the reply!
I do understand how PX4 normally generates smoothed trajectory from the mission setpoints, and that offboard mode should be used with caution as the setpoints are directly executed without smoothing.

However, my concerns here are twofold:

1. Parameter configuration (MPC_ACC_HOR_MAX) is not being honored, even though the documentation clearly states that it's for setting the maximum horizontal acceleration for both auto and manual modes. As you mentioned, the maximum acceleration is "somehow constrained by the maximum allowed tilt", not by the specific parameter value that seems to exist specifically for this task.
2. Yes, the controller ought to "directly execute the setpoints" without smoothing them in offboard mode. However, the case I presented above seems to indicate otherwise. Let me simplify the case a little bit. Consider the following two types of setpoint messages. The only difference is the acceleration over the y-axis being 0.0 or 3000.0:
   Msg A. {x: 100.0, y: 0.0, z: -5.0, yaw: 0.0, yawspeed: 0.0, vx: 0.0, vy: 0.0, vz: 0.0, ax: 0.0, ay: 0.0, ax: 0.0}
   Msg B. {x: 100.0, y: 0.0, z: -5.0, yaw: 0.0, yawspeed: 0.0, vx: 0.0, vy: 0.0, vz: 0.0, ax: 0.0, ay: 3000.0, ax: 0.0}
   Say we send a total of 100 messages by repeatedly sending Msg A 50 times, then sending Msg B once, and then sending Msg A 49 times. If all setpoint messages are really directly executed, shouldn't the controller instantaneously set target y-acceleration to 3000.0 the moment it receives Msg B, and then quickly reset the target y-acceleration to 0 the next moment, so that the drone keeps moving towards (100.0, 0.0, -5.0)? In fact, the drone keeps getting pushed towards the y axis. I am really curious if this is how it's supposed to be.

[dagar]

For safety how about a final constraint of the acceleration setpoint in the position controller?

PX4-Autopilot/src/modules/mc_pos_control/PositionControl/PositionControl.cpp

Lines 179 to 180 in 8f03045

	Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
	ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);