Issue with Attitude Target: Thrust and Quaternions are conflicting

I have a quadrotor with Pixhawk Hex Cube Orange. I have installed ROS Humble, PX4, and MicroXRCEAgent on the onboard computer running Ubuntu 22 to implement a simple PID controller and provide attitude setpoints to the drone. I have tested my code with PX4-SITL in the Gazebo simulation with an X500 drone. So, the implementation is such that I switch to offboard, arm the drone, and send attitude setpoints from a PID controller to track the desired position.

Now, when I run the same script on the real drone hardware, it is going to Offboard and getting armed. But when I send attitude targets through the MicroXRCEAgent using the topic /fmu/in/vehicle_attitude_setpoint, the drone does not respond, and it disarms after a timeout.

When I checked in the QGroundControl with the MAVLink Inspector tool, I noticed that whatever I provide as the thrust_body part of the VehicleAttitudeSetpoint ROS message appears as the fourth element of the quaternion (q) instead of the thrust in the ATTITUDE_TARGET message inside the MAVLink Inspector.

I tried the MAVLink Inspector with the PX4 SITL running the same PID script. This time, the thrust_body[2] of the VehicleAttitudeSetpoint ROS message appears in the thrust element of the ATTITUDE_TARGET with an opposite sign in the MAVLink Inspector. To test further, I populated q with an array of four test values and thrust_body with an array of three test values and published the VehicleAttitudeSetpoint ROS message. In the MAVLink Inspector’s ATTITUDE_TARGET, the quaternion appeared in the (w,x,y,z) format, and thrust_body[0] and thrust_body[1] were ignored, and thrust_body[2] appeared with an opposite sign under thrust.

Then, I ran the same test values on the actual drone and checked the MAVLink Inspector’s ATTITUDE_TARGET. I noticed that whatever I provided in q[0], q[1], and q[2] of the ROS message was getting ignored. q[3], which is supposed to be the z component of the quaternion, appeared in the first element of the ATTITUDE_TARGET in the MAVLink Inspector. Whatever I provided as the three elements of the thrust_body appeared as the remaining three values of q in ATTITUDE_TARGET in the MAVLink Inspector.

The following is the python script of the PID controller.

#!/usr/bin/env python

__author__ = "Viswa Narayanan Sankaranarayanan"
__contact__ = "vissan@ltu.se"

import rclpy
import numpy as np
from tf_transformations import *
import time
from rclpy.node import Node
from rclpy.clock import Clock
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy

from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleStatus, VehicleOdometry, VehicleAttitudeSetpoint, VehicleControlMode, VehicleLocalPosition

class OffboardControl(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        qos_profile = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=1
        )

        self.status_sub = self.create_subscription(
            VehicleStatus,
            '/fmu/out/vehicle_status',
            self.vehicle_status_callback,
            qos_profile)

        # Subscribers
        self.global_position = self.create_subscription(VehicleLocalPosition, '/fmu/out/vehicle_local_position', self.vehicle_position_callback, qos_profile)
        self.odometry = self.create_subscription(VehicleOdometry, '/fmu/out/vehicle_odometry', self.vehicle_odometry_callback, qos_profile)
        self.setpoint_position = self.create_subscription(VehicleLocalPosition, '/new_pose', self.sp_position_callback, qos_profile)
        self.mode_sub = self.create_subscription(VehicleControlMode, '/fmu/out/vehicle_control_mode', self.vehicle_control_mode_callback, qos_profile)

        #Publishers
        self.publisher_offboard_mode = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
        self.publisher_trajectory = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile)
        self.publisher_attitude = self.create_publisher(VehicleAttitudeSetpoint, '/fmu/in/vehicle_attitude_setpoint', qos_profile)
        self.publisher_command = self.create_publisher(VehicleCommand, '/fmu/in/vehicle_command', qos_profile)

        # Constants and cutoff values
        self.gravity = np.array([0, 0, -9.81])
        self.max_acc = 5
        self.max_throttle = 0.7
        timer_period = 0.02  # seconds
        self.timer = self.create_timer(timer_period, self.cmdloop_callback)


        # initial values for setpoints
        self.cur_pose = np.array([-0.0,-0.0,-0.0])
        self.cur_vel = np.array([-0.0,-0.0,-0.0])
        self.yaw = 0.0
        self.start_yaw = 1.0

        # Setpoints
        self.pos_sp = np.array([-0.0,-0.0,-2.5])
        self.vel_sp = np.array([0.0,0.0,0.0])
        self.yaw_sp = 1.57
        self.home_pos = np.array([0,0,-0.05])

        # Storage Variables
        self.desVel = np.zeros((3,))
        self.errInt = np.zeros((3,))
        self.errVel = np.zeros((3,))
        self.pre_time = Clock().now().nanoseconds/1E9
        self.offboard_time = Clock().now().nanoseconds/1E9
        self.nav_state = VehicleStatus.NAVIGATION_STATE_MAX
        self.dt = timer_period

        # Gains
        self.Kpos = np.array([-0.8, -0.8, -1.6])
        self.Kvel = np.array([-0.4, -0.4, -1.0])
        self.Kder = np.array([-0.0, -0.0, -0.0])
        self.Kint = np.array([-0.5, -0.5, -1.2])
        self.norm_thrust_const = 0.17

        # Msg Variables
        self.att_cmd = VehicleAttitudeSetpoint()
        # self.data_out = PlotDataMsg()


        # Flags
        self.armed = 1
        self.offboard_mode = False
        self.arm_flag = False
        self.mission_complete = False
        self.home = False

        print("Sleeping")

        time.sleep(2)
        print("Awake")
        self.mode()

    def vehicle_control_mode_callback(self, msg):
        self.offboard_mode = msg.flag_control_offboard_enabled
 
    def vehicle_status_callback(self, msg):
        self.nav_state = msg.nav_state
        self.armed = msg.arming_state

    def vehicle_position_callback(self, msg):
        self.cur_pose = np.array([msg.x,msg.y,msg.z])
        self.cur_vel = np.array([msg.vx,msg.vy,msg.vz])

    def vehicle_odometry_callback(self, msg):
        self.yaw = euler_from_quaternion([msg.q[1], msg.q[2], msg.q[3], msg.q[0]])[2]

    def sp_position_callback(self, msg):
        self.pos_sp = np.array([msg.x,msg.y,msg.z])

    def arm_uav(self):
        # Set to arm
        command_msg_arm = VehicleCommand()
        command_msg_arm.timestamp = int(Clock().now().nanoseconds / 1000)
        command_msg_arm.param1 = 1.0
        command_msg_arm.param2 = 0.0
        command_msg_arm.command = VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM
        command_msg_arm.target_component = 1
        command_msg_arm.target_system = 1
        command_msg_arm.source_component = 1
        command_msg_arm.source_system = 1
        command_msg_arm.from_external = True

        # Set to offboard mode
        command_msg_mode = VehicleCommand()
        command_msg_mode.timestamp = int(Clock().now().nanoseconds / 1000)
        command_msg_mode.param1 = 1.0
        command_msg_mode.param2 = 6.0
        command_msg_mode.command = VehicleCommand.VEHICLE_CMD_DO_SET_MODE
        command_msg_mode.target_component = 1
        command_msg_mode.target_system = 1
        command_msg_mode.source_component = 1
        command_msg_mode.source_system = 1
        command_msg_mode.from_external = True
        # print("Setting Arm")
        self.publisher_command.publish(command_msg_mode)
        self.publisher_command.publish(command_msg_arm) 

    def set_offboard(self):
        pass

    def disarm(self):
        # Set to arm
        command_msg_arm = VehicleCommand()
        command_msg_arm.timestamp = int(Clock().now().nanoseconds / 1000)
        command_msg_arm.param1 = 0.0
        command_msg_arm.param2 = 0.0
        command_msg_arm.command = VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM
        command_msg_arm.target_component = 1
        command_msg_arm.target_system = 1
        command_msg_arm.source_component = 1
        command_msg_arm.source_system = 1
        command_msg_arm.from_external = True

    def mode(self):
        # Set offboard mode to position control
        offboard_msg = OffboardControlMode()
        offboard_msg.timestamp = int(Clock().now().nanoseconds / 1000)
        offboard_msg.position=False
        offboard_msg.velocity=False
        offboard_msg.acceleration=False
        offboard_msg.attitude=True
        offboard_msg.body_rate=False
        # offboard_msg.actuator=False
        self.publisher_offboard_mode.publish(offboard_msg)


    def a_des(self):

        R = np.array([[np.cos(self.yaw), np.sin(self.yaw), 0], [-np.sin(self.yaw), np.cos(self.yaw), 0], [0, 0, 1]])
        cur_vel_W = self.cur_vel

        errPos = self.cur_pose - self.pos_sp
        print("Error: {:.2f}, {:.2f}, {:.2f}".format(errPos[0], errPos[1], errPos[2]))
        desVel = self.Kpos * errPos

        derVel = ((self.cur_vel - desVel) - self.errVel)/self.dt;
        self.errVel = self.cur_vel - desVel;
        self.errInt = self.errInt + self.errVel*self.dt

        max_int = np.array([2, 2, 6])
        self.errInt = np.maximum(-max_int, np.minimum(max_int, self.errInt))

        des_a = np.zeros((3,))
        des_a[0] = self.Kvel[0]*self.errVel[0] + self.Kder[0]*derVel[0] + self.Kint[0]*self.errInt[0]
        des_a[1] = self.Kvel[1]*self.errVel[1] + self.Kder[1]*derVel[1] + self.Kint[1]*self.errInt[1]
        des_a[2] = self.Kvel[2]*self.errVel[2] + self.Kder[2]*derVel[2] + self.Kint[2]*self.errInt[2]

        dA = np.zeros((3,))
        dA = R.dot(des_a)

        max_des = np.array([0.2, 0.2, 5])
        dA = np.maximum(-max_des,(np.minimum(max_des, dA)))

        if np.linalg.norm(dA) > self.max_acc:
            dA = (self.max_acc/np.linalg.norm(dA))*dA

        return (dA + self.gravity) 

    # Convert Acceleration vector to a rotation matrix
    def acc2rot(self,des_a, des_yaw):
        xb_des = np.array([1, 0, 0.0])
        if np.linalg.norm(des_a) == 0.0:
            zb_des = np.array([0,0,1])
        else:    
            zb_des = -des_a / np.linalg.norm(des_a)
        yb_des = np.cross(zb_des, xb_des) / np.linalg.norm(np.cross(zb_des, xb_des))
        proj_xb_des = np.cross(yb_des, zb_des) / np.linalg.norm(np.cross(yb_des, zb_des))
       
        rotmat = np.transpose(np.array([proj_xb_des, yb_des, zb_des]))
        return rotmat



    def cmdloop_callback(self):
        self.mode()
        if(self.armed == 2 and self.offboard_mode == True):
            self.arm_flag = True

            # Get the desired accelaration/forces
            des_a = self.a_des()

            # Compute the rotation matrix and the desired thrust
            r_des = self.acc2rot(des_a, 0.0)           
            zb = r_des[:,2]
            thrust = self.norm_thrust_const * des_a.dot(zb) + 1
            thrust = np.maximum(-0.8, np.minimum(thrust, 0.8))

            # Calculate the desired quaternion using small angle assumption
            quat_des = quaternion_from_euler(des_a[1], -des_a[0], 0.0)


            # Populate the attitude setpoint message
            now = int(Clock().now().nanoseconds/1E3)

            self.att_cmd.timestamp = now
            self.att_cmd.q_d[0] = quat_des[3]
            self.att_cmd.q_d[1] = quat_des[0]
            self.att_cmd.q_d[2] = quat_des[1]
            self.att_cmd.q_d[3] = quat_des[2]
            self.att_cmd.thrust_body[2] = thrust

            # Test values for the attitude setpoint message for debugging

            # self.att_cmd.q_d[0] = 0.6
            # self.att_cmd.q_d[1] = 0.5
            # self.att_cmd.q_d[2] = 0.4
            # self.att_cmd.q_d[3] = 1.0

            # self.att_cmd.thrust_body[0] = -0.3
            # self.att_cmd.thrust_body[1] = -0.2
            # self.att_cmd.thrust_body[2] = -0.1

            # print("Thrust: {:.2f}".format(thrust))
            # print([thrust, quat_des])

            self.publisher_attitude.publish(self.att_cmd)           

        elif self.arm_flag==False:
            self.arm_uav()
            print("Not armed yet")
            pass


def main(args=None):
    rclpy.init(args=args)
    offboard_control = OffboardControl()
    rclpy.spin(offboard_control)
    offboard_control.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

I have the rosbags and PX4 param files. But I’m not sure how to share it here because the file formats are not supported.

I am attaching the screenshots from QGroundControl. If you need more information, please let me know.


I am only beginning to try to understand the code, so my comments are likely unhelpful.

  1. where is your zip file ? I am new to this forum and would like to know what you have done.
  2. PX4 control is basically
    thrust setpoint–> attitude setpoint–>esc-allocator
    so arn’t conflicts expected if you inject both thrust and attitude setpoints ?
  3. are there assumptions in mc_att_control that make the body z-coordinate very special, which could lead to conflicts?
  4. gazebo has FLU, whereas PX4 has FLD
    coordinates. Does this account for your opposite sign?
  1. I’m unable to add zip files. So, I edited the original post. I have added the Python script that I used.
  2. If we use MicroXRCEAgent, we publish a VehicleAttitudeSetpoint message on the topic, /fmu/in/vehicle_attitude_setpoint. It has both body_thrust and quaternion.
  3. I’m not sure what assumptions you’re talking about. Could you please elaborate?
  4. PX4 SITL with MicroXRCEAgent publishes /fmu/out/vehicle_odometry, which uses NED for pose and FRD for velocities. It is the same for the actual hardware also. Opposite sign is not an issue. I just wanted to mention how it appears on QGroundControl. The drone is flying in the simulator. The issue is that whatever I’m sending in thrust_body of the VehicleAttitudeSetpoint message is appearing in quaternion of ATTITUDE_TARGET in MAVLink Inspector.

Thanks a lot for your file! I’m slow, so will look at it carefully later.

  1. [VehicleAttitudeSetpoint] "For multicopters thrust_body[0] and thrust[1] are usually 0 " Have you tried that? Does mc_att_control resolve the fundamental conflict when attitude is specified by assuming Thrust[0] and Thrust[1] should be zero and simply swap other parameters as you have observed? Or is QGroundControl misrepresenting the situation?

  2. As noted by someone else, ive only so far noticed the convoluted logic in matrix::Vector3f AttitudeControl::update(const Quatf &q) const The z-coordinate is indeed very special: to set yaw differently from roll and pitch. And is it a lot more special than that? Im only beginning to look at this stuff, and I need first to find out about much else.

  1. When I publish the actual values, I don’t set any values to thrust_body[0] and thrust_body[1]. When I echo the message, I can see that those two elements are set to zero by default. I’m not sure how I can check what’s happening inside the mc_att_control inside the Pixhawk hardware. Could you please tell me how? I do not think that QGroundControl is misrepresenting, because when I use the same QGroundControl with the simulation, it is representing the values correctly.

  2. I’m not sure how quaternion’s z component is messing with the thrust. I don’t find any reference to thrust inside the AttitudeControl::update function.

  1. I am a novice with little experience with debugging and none across processors. You could add code to publish debugging messages, but I assume there are better tools available. Please let me know!

You already calculate a rotation matrix without a small angle assumption, so why not get quat_des from that ?

  1. I’m also unsure of the debugging tools. That’s why I used QGroundControl. If you know any, please let me know.

The small angle assumption is just for testing. I’ll change it later.

I’m trying to point out to you that des_a[1] and -des_a[0] aren’t angles.
To get an angle approximation you need a ratio of two lengths, as computed in acc2rot.

What you have looks to me like 2 arbitrarily large or small numbers. In that case you most likely will compute either a rotation close to zero or a rotation close to 90 degrees.

So your “approximation” is probably very very very wrong, isn’t it ?

Yes. I understand that. And as you can notice here, I clip the des_a (dA) to be within +/- 0.2. So it doesn’t produce arbitrarily large value. Also, the quaternions I get are very close to 1,0,0,0 (w,x,y,z). I understand that you’re pointing a valid issue about how quaternions are calculated. But I don’t think that this is related to the original issue.