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

最新推荐文章于 2023-12-21 23:00:51 发布

weixin_44616080

最新推荐文章于 2023-12-21 23:00:51 发布

阅读量1.2k

点赞数

分类专栏：实战文章标签：无人机 PX4 开源飞控姿态期望生成多旋翼

本文链接：https://blog.csdn.net/weixin_44616080/article/details/101001633

版权

本文深入解析PX4飞控系统中位置控制器到姿态控制器的转换函数，主要讲解`thrustToAttitude`函数，涉及期望姿态的计算，包括期望偏航角、机体轴系单位向量的确定，以及如何转换为期望旋转矩阵和四元数。文章还提到了特殊情况的处理，如期望升力为0的情况。

摘要由CSDN通过智能技术生成

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

引言

关于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