位置控制器PX4代码解析（文中有福利！！！）

号外号外！！！本公众号将联合电子工业出版社于9月11号送出15本价值98元的全权老师著作《多旋翼飞行器设计与控制》，关注本公众号的朋友均可参加，敬请期待~~还没关注的朋友赶紧关注吧！！！

引言

上一讲开源飞控PX4姿态控制代码解析我们对PX4姿态控制的代码进行了详细解析，趁大家对PX4代码还有点熟悉的时候我们把位置控制器的代码部分也拿出来解读一下吧。位置控制器的代码主要是以下两个函数：

1、void PositionControl::_positionController()位于Firmware\src\modules\mc_pos_control\PositionControl.cpp文件中；

2、void PositionControl::_velocityController(const float &dt)也是位于Firmware\src\modules\mc_pos_control\PositionControl.cpp文件中；

我们先贴出两个函数的源码：

void PositionControl::_positionController()
{
  // P-position controller
  const Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(),
           _param_mpc_z_p.get()));
  _vel_sp = vel_sp_position + _vel_sp;

  // Constrain horizontal velocity by prioritizing the velocity component along the
  // the desired position setpoint over the feed-forward term.
  const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position),
           Vector2f(_vel_sp - vel_sp_position), _param_mpc_xy_vel_max.get());
  _vel_sp(0) = vel_sp_xy(0);
  _vel_sp(1) = vel_sp_xy(1);
  // Constrain velocity in z-direction.
  _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
}

void PositionControl::_velocityController(const float &dt)
{
  // Generate desired thrust setpoint.
  // PID
  // u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
  // Umin <= u_des <= Umax
  //
  // Anti-Windup:
  // u_des = _thr_sp; r = _vel_sp; y = _vel
  // u_des >= Umax and r - y >= 0 => Saturation = true
  // u_des >= Umax and r - y <= 0 => Saturation = false
  // u_des <= Umin and r - y <= 0 => Saturation = true
  // u_des <= Umin and r - y >= 0 => Saturation = false
  //
  //   Notes:
  // - PID implementation is in NED-frame
  // - control output in D-direction has priority over NE-direction
  // - the equilibrium point for the PID is at hover-thrust
  // - the maximum tilt cannot exceed 90 degrees. This means that it is
  //    not possible to have a desired thrust direction pointing in the positive
  //    D-direction (= downward)
  // - the desired thrust in D-direction is limited by the thrust limits
  // - the desired thrust