【PX4二次开发】PX4 offboard 模式发布 推力+姿态

写在前面

由于项目需要，最近想发布推力+姿态指令给PX4控制器，然后由PX4跟踪给定的控制指令。我注意到，所需的推力是一个归一化的值，所以我想当然的将所需推力与最大推力的比值作为归一化推力发布出去。不过，在测试时候发现，发布的这个归一化推力根本无法让无人机起飞，于是我就这一问题展开了资料的查询和分析……

PX4 offboard 模式发布推力+姿态

我们先进行一个简单的小测试让无人机实现悬停，此时推力为无人机的重力，姿态指令为0。
参考官方mavros写的python源码如下：

px4_node_test.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import PoseStamped, TwistStamped, Twist, Pose, Quaternion, Point
from mavros_msgs.msg import State, AttitudeTarget
from sensor_msgs.msg import Imu
from std_msgs.msg import Bool
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest
import tf
import math
import numpy as np

PI = 3.14159
THRUST_FULL = 31

# sdf参数：
m = 1.5
thrust = m * 9.8
motor_constant = 5.84e-06
min_motor_vel = 0
max_motor_vel = 1100
THRUST_FULL = 4 * motor_constant * max_motor_vel * max_motor_vel

current_state = State()

# Callback functions
def state_cb(msg):
    global current_state
    current_state = msg

# Function to convert Euler angles to quaternion
def euler_to_quat(roll, pitch, yaw):
    q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
    quat = Quaternion()
    quat.x = q[0]
    quat.y = q[1]
    quat.z = q[2]
    quat.w = q[3]
    return quat

# Function to scale thrust
def thrust_scale(thrust):
    print("THRUST_FULL:",THRUST_FULL)
    if thrust >= THRUST_FULL:
        thrust = THRUST_FULL

    # 每个电机推力的归一化
    # THR_MDL_FAC = 1 时使用！
    thrust_normalized = thrust/THRUST_FULL
    print("thrust_normalized:",thrust_normalized)

    # 每个电机转速的归一化
    # THR_MDL_FAC = 0 时使用！
    # thrust_normalized = (np.sqrt(thrust /4 /motor_constant)-min_motor_vel)/(max_motor_vel-min_motor_vel)
    # print("thrust_normalized:",thrust_normalized)

    return thrust_normalized


def main():
    rospy.init_node("offb_node_py")

    state_sub = rospy.Subscriber("mavros/state", State, callback=state_cb)
    imu_sub = rospy.Subscriber("mavros/imu/data", Imu)
    pos_sub = rospy.Subscriber("mavros/local_position/pose", PoseStamped)
    vel_sub = rospy.Subscriber("mavros/global_position/raw/gps_vel", TwistStamped)
    local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
    setvel_pub = rospy.Publisher("mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=10)
    start_pub = rospy.Publisher("start_time", Bool, queue_size=10)
    refv_sub = rospy.Subscriber("reference", Twist)
    refp_sub = rospy.Subscriber("reference2", Pose)

    rospy.wait_for_service("/mavros/cmd/arming")
    arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
    rospy.wait_for_service("/mavros/set_mode")
    set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)

    raw_pub_att = rospy.Publisher('mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10)

    rate = rospy.Rate(20.0)
    while not rospy.is_shutdown() and not current_state.connected:
        rospy.loginfo("Waiting for FCU connection...")
        rate.sleep()

    offb_set_mode = SetModeRequest()
    offb_set_mode.custom_mode = "OFFBOARD"

    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    raw_att = AttitudeTarget()
    raw_att.type_mask = 7  # Ignore body rate
    raw_att.thrust = thrust_scale(thrust)  # Initial thrust
    raw_att.orientation = euler_to_quat(0, 0, 0)  # Level orientation

    last_request = rospy.Time.now()

    while not rospy.is_shutdown():
        if current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0)):
            set_mode_resp = set_mode_client(offb_set_mode)
            if set_mode_resp.mode_sent:
                rospy.loginfo("Offboard enabled")
            last_request = rospy.Time.now()
        else:
            if not current_state.armed and (rospy.Time.now() - last_request > rospy.Duration(5.0)):
                arm_resp = arming_client(arm_cmd)
                if arm_resp.success:
                    rospy.loginfo("Vehicle armed")
                last_request