开源飞控PX4姿态控制代码解析

本篇文章首发于公众号：无人机系统技术。更多无人机技术相关文章请关注此公众号，有问题也可在公众号回复“加群”进入技术交流群进行交流。

本公众号将于9月11号联合电子工业出版社送出15本价值98元的《多旋翼飞行器设计与控制》书籍，敬请期待。关注：无人机系统技术，了解详情！！！

引言

上一讲PX4姿态控制算法详解我们对PX4姿态控制的算法进行了详细解析，还没看过的朋友先打开第三条内容仔细阅读后再来继续浏览吧。本讲内容主要是摘取PX4中关于姿态控制律实现的代码进行逐行分析，让各位朋友在代码实践中感受控制算法的魅力。姿态控制器的代码在文件Firmware\src\modules\mc_att_control\AttitudeControl\AttitudeControl.cpp的update函数中，我们先把代码贴上来，然后再进行逐行解读：

matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward)
{
  // ensure input quaternions are exactly normalized because acosf(1.00001) == NaN
  q.normalize();
  qd.normalize();

  // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
  const Vector3f e_z = q.dcm_z();
  const Vector3f e_z_d = qd.dcm_z();
  Quatf qd_red(e_z, e_z_d);

  if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
    // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
    // full attitude control anyways generates no yaw input and directly takes the combination of
    // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
    qd_red = qd;

  } else {
    // transform rotation from current to desired thrust vector into a world frame reduced desired attitude
    qd_red *= q;
  }

  // mix full and reduced desired attitude
  Quatf q_mix = qd_red.inversed() * qd;
  q_mix *= math::signNoZero(q_mix(0));
  // catch numerical problems with the domain of acosf