I’m implementing a ROS2 offboard control package that publishes trajectory setpoints of the form
position = [x_target, y_target, z_target]
velocity = [NAN, NAN, NAN]
acceleration = [NAN, NAN, NAN]
yaw = NAN
yawspeed = NAN
until I get to a target position, then I switch to a custom guidance law that generates acceleration setpoints. At which point I change the trajectory_setpoint message to the form:
position = [NAN, NAN, NAN]
velocity = [NAN, NAN, NAN]
acceleration = [ax_des, ay_des, az_des]
yaw = NAN
yawspeed = NAN
However, when I start sending the acceleration setpoints, the vehicle looks like it enters a failsafe and starts a land sequence. Looking in the log, the vehicle_local_position_setpoint.vz jumps to +1m/s and az jumps to ~9 m/s^2. After the jump, vz stays at 1m/s until landed and az goes to zero. Other than that, there are no indications in the log of anything that would have triggered a failsafe. No failure_detector flags, no failsafe_flags, no issues with the estimator status.
Is this a bug? Am I using acceleration setpoints wrong? How would I even go about debugging this? I’ve dug through all the relevant source code and can’t find an reasonable explanation.
I haven’t worked with offboard mode yet, but I have had similar behavior in a different situation.
I was creating very small missions in QCG, with waypoints closer than 1m and the quad did a takeoff and initiated the landig immediately after, without even starting the mission. The reason for this was that, because the waypoint were so close to each other PX4 was thinking it had already completed each WP and thus the entire mission and was ready to land.
I am not sure if this applies to your situation, maybe there is something that is telling PX4 that it has reached its goals and is ready to land?
With offboard mode, you directly supply the trajectory, attitude or rate setpoints to PX4’s control loop, as opposed to waypoint missions which I’m assuming get handled by the navigator module which in turn handles the publishing of trajectory setpoints to the control loop. There’s no check for arriving at a target position in offboard mode unless you employ it yourself.
My situation looks like some failsafe is being employed because when I switch from only position setpoints to only acceleration setpoints the NAN vz setpoint I send gets reset to +1m/s as seen in the vehicle_local_position_setpoint topic (which is the output of the position control loop). My problem is I can’t tell how or why a failsafe would have been invoked, because there are no failsafe or failure detector flags that would indicate an issue and the autopilot was operating as expected while I was publishing position setpoints.
So I guess the question is what condition would be required when sending acceleration setpoints only that wouldn’t be required when sending position only setpoints.
The only things I’ve noticed different from a previously successful flight in which I was using acceleration setpoints the entire flight duration is that for the unsuccessful flight I did not have a valid hover thrust estimate (in which case it looks like the mc controller should be using the MPC_THR_HOVER param and still work) and that the CS_YAW_ALIGN flag is set true for the successful flight and false for the unsuccessful flight. I can’t find any where in the source code where this would result in a failsafe trajectory setpoint being generated though
