Hello.
I’m implementing a ROS2 offboard control package to fly a dev drone running PX4 (v1.14). I have it set up so that I publish offboard_control_mode messages with the position flag set true and publish trajectory_setpoint messages with just position setpoints (all other fields set to NAN) until the quad reaches a desired position. I then switch to publishing offboard_control_mode messages with the acceleration flag set true (all other fields false) and trajectory_setpoints with just acceleration setpoints (all other fields NAN) to implement a custom guidance law.
The issue I’m having is as soon as I transition to offboard acceleration setpoints, the vehicle just initiates a land sequence. Looking back at the log I can’t find any indication of a failsafe condition being implemented. I’ve read through the commander code and mc_pos_controller code and haven’t found anything that stands out as to what might have went wrong.
When I switch to acceleration, the vehicle_control_mode updates accordingly, flag_control_position_enabled and flag_control_velocity_enabled go from true to false and flag_control_acceleration_enabled goes from false to true. flag_multicopter_position_control_enabled and flag_control_offboard_enabled stay true the entire time.
The trajectory_setpoint messages look correct, but the vehicle_local_position_setpoints aren’t tracking the change. The vehicle_local_position.vx/vy stop when I switch to acceleration setpoints, but the vehicle_local_position.vz setpoint goes to +1.0m/s and the vehicle_local_position.az setpoint jumps to roughly +9m/s^2
Does anyone know why this might be happening? Should I be sending the same offboard control message the entire time (position, velocity and acceleration true) and just go from NAN accleration setpoints to NAN position setpoints when I want to switch?
Am I right to assume that vehicle_local_position_setpoint.vz should always be NAN if trajectory_setpoint.velocity[2] is NAN? Would the only way to see the sudden jump to vehicle_local_position_setpoint.vz = 1m/s if the position control is generating a failsafe trajectoryb setpoint? What failsafe would I have invoked by just switching to acceleration setpoints? Are there any additional requirements for flying with only acceleration setpoints?
Any advice on troubleshooting would be greatly appreciated.
I should add that I ran the offboard control package with px4_sitl in gazebo and it performs perfectly. I’m using a modalai sentinel drone w/ sdk1.1.2. The PX4 fork I’m using is here:
This is still a problem on updated firmware. Someone please address this.
Using the same firmware to run px4_sitl this issue is non-existent when running in gazebo, but on the real quad something is generating a z velocity setpoint >= 1.0 that is getting clamped to 1.0m/s that az setpoints have to compensate for so you can’t perform acceleration control directly. I can upload the SITL vs real flight logs if needed.
@jmltorch I dont think pure acceleration setpoints via trajectory_setpoints are supported.
I’ve read through the multicopter position controller source code and can’t find any reason it wouldn’t work. It should just skip the position and velocity control loops. It shouldn’t fail the valid input check. I’ve rediscovered this issue while trying to implement an external adaptive controller and the controller still works it just has to have a large biased az setpoint to overcome the vz setpoint being set somewhere in the px4 control stack.
The same external controller publishing to trajectory_setpoint topic via ros2 with acceleration only setpoints works perfectly in gazebo running px4_sitl built from the same firmware. The +1m/s (I’m assuming it’s clamped to my max vz down param value which is 1m/s) vz setpoint only appears on real hardware. Running px4 sitl handles incoming net acceleration setpoints perfectly.
I should note this happens when I switch from position to acceleration setpoints midflight AND when I switch to offboard mode from the ground and am only ever sending acceleration setpoints. For both cases the vehicle_local_position_setpoint.vz=1.0 shows up in real but not SITL
Verified, acceleration setpoint only based control is supported. I found out what’s causing the issue, but I don’t know why it’s happening and it appears platform specific, but I’m sure it could affect other platforms depending on the actual root cause.
In the multicopter position controller code (PositionControl.cpp) at the end of the position control loop the xy and z velocity setpoints are constrained. The xy setpoints are constrained as a 2d vector and the z setpoint is constrained individually by a call to math::constrain(). For some reason, if every setpoint (px,py,pz,vx,vy,vz) is NaN at this point, the 2d constrain function maintains the NaN values of the xy setpoints, but the constrain function acting on the vz setpoint incorrectly clamps the NaN vz setpoint to the downward velocity limit instead of maintaining the NaN value. The NaN vz setpoint now becomes a nonzero downward feedforward velocity for the velocity control loop which is now engaged instead of skipping to acceleration control. Incoming az setpoints now have to overcome this constant downward velocity setpoint.
Note, this issues only occurs on the px4 firmware running on my quad (arm platform), not when running in SITL on my laptop. The issue is resolved when I rebuild px4 with a is_finite check wrapping the constrain function on the vz setpoint, so my NaN vz setpoint is retained, position and velocity control loops are skipped and acceleration control works as expected.
I’m using modalai’s fork of PX4 which started from release 1.14, but this vz setpoint constraint is still applied the same way and the constrain function is still defined the same way as in px4 1.17 release. I have no way of knowing if this is specific to voxl2/qrb5165 or if it applies to all ARM platforms, because I don’t know which part of the build process is causing this
Yes, I have verified that the Hexagon compiler generates NaN comparison code that can give an incorrect result. A short term fix for this issue in the math library has been submitted for inclusion into the 1.18 release. The 1.18 release is intended to be the replacement for the ModalAI fork. fix(mathlib): avoid Hexagon NaN comparison issues by katzfey · Pull Request #27931 · PX4/PX4-Autopilot · GitHub . An update to a newer compiler version is being researched as, possibly, a better long term fix.
