大疆OSDK ROS环境下坐标系与节点问题

坐标系

       坐标系对控制来说是至关重要的，不像算法，算法错了就错了，但是控制出问题了就可能面临炸机。

        先上图：

        如果用速度控制的话，控制层面的水平面坐标系应该以下图为准：

        做控制肯定会用到 /dji_osdk_ros/attitude 和 /dji_osdk_ros/velocity 。这两个分别是大疆无人机的姿态四元组数据（x , y , z , w）和飞机的移动速度。这两项都遵循ROS规范，也就是说x轴为正东方向，y轴指向正北。即下图：

        将飞机的姿态四元组数据转成 欧拉角（roll , pitch ,yaw)后，横滚角和俯仰角可以不做变换，偏航角yaw在正东时为0度，即下图所示情况：

如果以北为0°，需要手动减去90°，同时会改变值域。

        在实际使用中一般会结合算法使用，需要注意算法输出的控制信息是否具有方向性。

节点报错

        如果已经开启OSDK通信串口，并且串口连接正确，仍然出现以下报错，就需要将大疆节点终端通过Ctrl+C中断，虽然无法立即停止，但是大约循环到第六次就退出了，别用Ctrl+Z。停止后重启节点就行。

补充

        姿态四元组数据转欧拉角

struct EulerAnglesDeg {
    double roll;   // 横滚角 (deg)  φ
    double pitch;  // 俯仰角 (deg)  θ
    double yaw;    // 偏航角 (deg)  ψ
};
/**
 * @brief  四元数转欧拉角（ZYX顺序 → 偏航-俯仰-横滚）
 *         航空/飞控/机器人领域标准顺序
 * @param  q        输入四元数（q0=w）
 * @param  euler    输出欧拉角（度）
 * @return 0=成功  -1=四元数模长为0
 */
static inline void Quat_To_EulerZYX_Deg(const DJI::OSDK::Telemetry::Quaternion& q, EulerAnglesDeg& euler)
{
    float w = q.q0;
    float x = q.q1;
    float y = q.q2;
    float z = q.q3;

    // 快速归一化
    float norm = sqrtf(w*w + x*x + y*y + z*z);
    if (norm > 1e-8f) {
        float inv = 1.0f / norm;
        w *= inv; x *= inv; y *= inv; z *= inv;
    } else {
        euler.roll = euler.pitch = euler.yaw = 0.0f;
        return;
    }

    const float DEG_PER_RAD = 57.29577951308232f;

    // Roll
    float sinr_cosp = 2.0f * (w*x + y*z);
    float cosr_cosp = 1.0f - 2.0f*(x*x + y*y);
    euler.roll = atan2f(sinr_cosp, cosr_cosp) * DEG_PER_RAD;

    // Pitch
    float sinp = 2.0f * (w*y - x*z);
    if (fabsf(sinp) >= 1.0f)
        euler.pitch = copysignf(90.0f, sinp);
    else
        euler.pitch = asinf(sinp) * DEG_PER_RAD;

    // Yaw
    float siny_cosp = 2.0f * (w*z + x*y);
    float cosy_cosp = 1.0f - 2.0f*(y*y + z*z);
    euler.yaw = atan2f(siny_cosp, cosy_cosp) * DEG_PER_RAD;
}