函数声明

quadrotor_msgs::Px4ctrlDebug
LinearControl::calculateControl(
    const Desired_State_t &des,
    const Odom_Data_t &odom,
    const Imu_Data_t &imu,
    Controller_Output_t &u)

输入参数分析

1. 期望状态 (Desired_State_t)

struct Desired_State_t {
    Eigen::Vector3d p;    // 期望位置 [x, y, z]
    Eigen::Vector3d v;    // 期望速度 [vx, vy, vz]
    Eigen::Vector3d a;    // 期望加速度 [ax, ay, az]
    double yaw;           // 期望偏航角
    Eigen::Quaterniond q; // 期望姿态四元数
};

2. 里程计数据 (Odom_Data_t)

struct Odom_Data_t {
    Eigen::Vector3d p;    // 当前位置 [x, y, z]
    Eigen::Vector3d v;    // 当前速度 [vx, vy, vz]
    Eigen::Quaterniond q; // 当前姿态四元数
};

3. IMU 数据 (Imu_Data_t)

struct Imu_Data_t {
    Eigen::Quaterniond q; // IMU 姿态四元数
    Eigen::Vector3d acc;  // 加速度 [ax, ay, az]
    Eigen::Vector3d gyro; // 角速度 [wx, wy, wz]
};

4. 控制器输出 (Controller_Output_t)

struct Controller_Output_t {
    double thrust;        // 推力大小
    Eigen::Quaterniond q; // 期望姿态四元数
};

返回值分析

返回类型：quadrotor_msgs::Px4ctrlDebug

这是一个 ROS 消息类型，包含以下字段：

// 期望速度
float64 des_v_x;
float64 des_v_y;
float64 des_v_z;

// 期望加速度
float64 des_a_x;
float64 des_a_y;
float64 des_a_z;

// 期望姿态四元数
float64 des_q_x;
float64 des_q_y;
float64 des_q_z;
float64 des_q_w;

// 期望推力
float64 des_thr;

数据格式转换

• 输入：使用 Eigen 库数据类型（Vector3d, Quaterniond）。
• 输出：使用 ROS 标准消息类型（float64）。
• 姿态表示：采用四元数避免万向节锁问题。
• 位置、速度、加速度：使用三维向量表示。

函数处理流程

// 1. 计算期望加速度
Eigen::Vector3d des_acc = des.a
    + Kv.asDiagonal() * (des.v - odom.v)
    + Kp.asDiagonal() * (des.p - odom.p);
des_acc += Eigen::Vector3d(0, 0, param_.gra);

// 2. 计算推力
u.thrust = computeDesiredCollectiveThrustSignal(des_acc);

// 3. 计算姿态
double roll = (des_acc(0) * sin - des_acc(1) * cos) / param_.gra;
double pitch = (des_acc(0) * cos + des_acc(1) * sin) / param_.gra;

// 4. 计算期望姿态四元数
Eigen::Quaterniond q = Eigen::AngleAxisd(des.yaw, Eigen::Vector3d::UnitZ())
    * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
    * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());

// 5. 更新控制器输出
u.q = imu.q * odom.q.inverse() * q;

// 6. 填充调试信息
debug_msg_.des_v_x = des.v(0);
// ... 其他调试信息 ...

关键特点

✅ 高效计算：使用引用传递参数，避免不必要的拷贝。
✅ 数据结构优化：输入参数使用 Eigen 库的数据类型，便于数学计算。
✅ 稳定性：姿态使用四元数表示，避免万向节锁问题。
✅ 调试支持：包含完整的调试信息，便于问题诊断。

这个函数是 无人机控制器的核心，它接收当前状态和期望状态，计算控制输入，并返回调试信息。函数的设计兼顾 计算效率 和 调试需求，并充分利用现代 C++ 以及 ROS 标准数据类型。