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.