PX4代码解析：姿态期望生成

本篇文章首发于公众号：无人机系统技术。更多无人机技术相关文章请关注此公众号，有问题也可在公众号回复“加群”进入技术交流群进行交流。另外，想要获取本文算法所在论文的朋友也可以在公众号底部加我微信获取。

引言

关于PX4源码中控制器的设计部分我们前几讲已经对位置控制、姿态控制等核心函数进行了详细分析，逐行解释。今天我们就将剩下的最后一部分内容：位置控制器输出到姿态控制器输入的转换部分做一个解读。

转换部分的函数就一个：

vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp)在Firmware\src\modules\mc_pos_control\Utility\ControlMath.cpp文件中。

我们先贴出此函数的源码：

vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp)
{
  vehicle_attitude_setpoint_s att_sp = {};
  att_sp.yaw_body = yaw_sp;

  // desired body_z axis = -normalize(thrust_vector)
  Vector3f body_x, body_y, body_z;

  if (thr_sp.length() > 0.00001f) {
    body_z = -thr_sp.normalized();

  } else {
    // no thrust, set Z axis to safe value
    body_z = Vector3f(0.f, 0.f, 1.f);
  }

  // vector of desired yaw dir