{SOLVED}
Hello, what is the recommended method to publish body rate setpoints over the PX4-ROS2 bridge?
offboard_control_mode.msg implies that body_rate = True is an acceptable mode.
However, trajectory_setpoint.msg does not support body rates, so I turned to vehicle_rates_setpoint.msg and implemented a publish_vehicle_rates_setpoint() function in offboard_control.cpp.
When testing offboard control through jmavsim, I can publish trajectory setpoints without a problem. However, when publishing the vehicle rate setpoints, a failsafe is triggered and the quad lands to the ground hard. The px4 console outputs:
WARN [mc_pos_control] invalid setpoints
WARN [mc_pos_control] Failsafe: stop and wait
similarly to Issue 17715. I am not sure if timing is the issue as the VehicleRateSetpoints are being published at a rate of 10 Hz.
In subsequent runs I am getting the following output from the px4 console:
WARN [ekf2] primary EKF changed 0 (filter fault) -> 1
WARN [ekf2] primary EKF changed 1 (filter fault) -> 3
WARN [ekf2] primary EKF changed 3 (filter fault) -> 5
WARN [ekf2] primary EKF changed 5 (filter fault) -> 4
WARN [ekf2] primary EKF changed 4 (unhealthy) -> 5
INFO [commander] Landing detected
WARN [ekf2] primary EKF changed 5 (filter fault) -> 0
INFO [commander] Disarmed by landing
INFO [tone_alarm] notify neutral
I am not sure what to make of the EKF health. My question is, what is the underlying problem and, is there a better way to publish body rate setpoints to the px4 via offboard control via ROS2?
Thanks for your help!
–
SOLVED: I was setting roll, pitch, yaw setpoints but not thrust. Hence, the drone fell. Upon setting msg.thrust_body = {0.0, 0.0, -5.0} offboard control performed as desired. Still not sure what triggered the EKF warnings.