Except the nickname!!! On September 11, the Publishing House of United Electronics Industry will deliver 15 copies of the 98-yuan authoritative teacher's book "Design and Control of Multi-Rotor Aircraft" to all those who are concerned about the Publishing House. Please look forward to your attention!!!

Introduction

In the last lecture, the open source flight control PX4 attitude control code analysis, we have a detailed analysis of the PX4 attitude control code, while you are a little familiar with the PX4 code, we will also take out the position controller code part to interpret it. The code of the position controller is mainly composed of the following two functions:

1. void Position Control:: _position Controller() is located in Firmware src modules mc_pos_control PositionControl. CPP file;

2. Void Position Control:: _velocity Controller (const float & dt) is also located in Firmware src modules mc_pos_control Position Control. CPP file.

Let's first post the source code of two functions:

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 in NE-direction is limited by the thrust excess after
  //    consideration of the desired thrust in D-direction. In addition, the thrust in
  //    NE-direction is also limited by the maximum tilt.

  const Vector3f vel_err = _vel_sp - _vel;

  // Consider thrust in D-direction.
  float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) +  _param_mpc_z_vel_d.get() * _vel_dot(2) + _thr_int(
           2) - _param_mpc_thr_hover.get();

  // The Thrust limits are negated and swapped due to NED-frame.
  float uMax = -_param_mpc_thr_min.get();
  float uMin = -_param_mpc_thr_max.get();

  // make sure there's always enough thrust vector length to infer the attitude
  uMax = math::min(uMax, -10e-4f);

  // Apply Anti-Windup in D-direction.
  bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) ||
             (thrust_desired_D <= uMin && vel_err(2) <= 0.0f);

  if (!stop_integral_D) {
    _thr_int(2) += vel_err(2) * _param_mpc_z_vel_i.get() * dt;

    // limit thrust integral
    _thr_int(2) = math::min(fabsf(_thr_int(2)), _param_mpc_thr_max.get()) * math::sign(_thr_int(2));
  }

  // Saturate thrust setpoint in D-direction.
  _thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax);

  if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) {
    // Thrust set-point in NE-direction is already provided. Only
    // scaling by the maximum tilt is required.
    float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
    _thr_sp(0) *= thr_xy_max;
    _thr_sp(1) *= thr_xy_max;

  } else {
    // PID-velocity controller for NE-direction.
    Vector2f thrust_desired_NE;
    thrust_desired_NE(0) = _param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _thr_int(0);
    thrust_desired_NE(1) = _param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _thr_int(1);

    // Get maximum allowed thrust in NE based on tilt and excess thrust.
    float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
    float thrust_max_NE = sqrtf(_param_mpc_thr_max.get() * _param_mpc_thr_max.get() - _thr_sp(2) * _thr_sp(2));
    thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);

    // Saturate thrust in NE-direction.
    _thr_sp(0) = thrust_desired_NE(0);
    _thr_sp(1) = thrust_desired_NE(1);

    if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) {
      float mag = thrust_desired_NE.length();
      _thr_sp(0) = thrust_desired_NE(0) / mag * thrust_max_NE;
      _thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE;
    }

    // Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output
    // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
    float arw_gain = 2.f / _param_mpc_xy_vel_p.get();

    Vector2f vel_err_lim;
    vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain;
    vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;

    // Update integral
    _thr_int(0) += _param_mpc_xy_vel_i.get() * vel_err_lim(0) * dt;
    _thr_int(1) += _param_mpc_xy_vel_i.get() * vel_err_lim(1) * dt;
  }
}

Code interpretation

Interpretation of Position Control Function in Outer Loop of Position Controller:

void PositionControl::_positionController()
{
  // P-position controller

// Calculating velocity expectation based on position error and P parameter of position loop (NED system)

  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()));

// Velocity expectation and velocity expectation generated by superposition position position error feed forward as total velocity expectation

  _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.

// According to the maximum horizontal velocity set, the expected horizontal velocity is limited, and the desired velocity caused by position error is guaranteed first.

  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.

// Limit D-direction velocity expectation according to the maximum and minimum D-direction velocity

  _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
}

Interpretation of Speed Control Function in Inner Loop of Position Controller:

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 in NE-direction is limited by the thrust excess after
  //    consideration of the desired thrust in D-direction. In addition, the thrust in
  //    NE-direction is also limited by the maximum tilt.

// Computational velocity error (NED series)

  const Vector3f vel_err = _vel_sp - _vel;

  // Consider thrust in D-direction.

// According to the speed error, we calculate the lift of D direction (i.e. the output of the controller and the input of the control, here we call it lift in the actual physical sense): thr_sp_d=P+I+D+thr_hover, note that direction D is downward, so when neutral throttle is added, it will be negative.

  float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) +  _param_mpc_z_vel_d.get() * _vel_dot(2) + _thr_int(
           2) - _param_mpc_thr_hover.get();

// Because D is downward, the upper and lower limits are minus signed.

  // The Thrust limits are negated and swapped due to NED-frame.
  float uMax = -_param_mpc_thr_min.get();
  float uMin = -_param_mpc_thr_max.get();
  
  // make sure there's always enough thrust vector length to infer the attitude
  uMax = math::min(uMax, -10e-4f);

  // Apply Anti-Windup in D-direction.

// The integral is anti-saturation. If D reaches the upper and lower limit of the expected lift and the error direction is the same, the integral is stopped. Otherwise, the normal integral and the upper and lower bounds of the integral are restricted.

  bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) ||
             (thrust_desired_D <= uMin && vel_err(2) <= 0.0f);

  if (!stop_integral_D) {
    _thr_int(2) += vel_err(2) * _param_mpc_z_vel_i.get() * dt;

    // limit thrust integral

// Limited D-Direction Lift Integral

    _thr_int(2) = math::min(fabsf(_thr_int(2)), _param_mpc_thr_max.get()) * math::sign(_thr_int(2));
  }

  // Saturate thrust setpoint in D-direction.

// Limit D-direction lift expectation

  _thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax);

// When the expectation of NE direction has been assigned, the maximum tilt angle can be used to limit the tilt direction. This is planned by the upper level of flight control. There is no need to consider it here. Normally, this code will not run.

  if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) {
    // Thrust set-point in NE-direction is already provided. Only
    // scaling by the maximum tilt is required.
    float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
    _thr_sp(0) *= thr_xy_max;
    _thr_sp(1) *= thr_xy_max;

// Normally, the process of calculating lift expectation in NE plane based on velocity error

  } else {
    // PID-velocity controller for NE-direction.

// PID calculation of velocity error in NE plane and lift expectation in NE plane

Vector2f thrust_desired_NE;
thrust_desired_NE(0) = _param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _thr_int(0);
thrust_desired_NE(1) = _param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _thr_int(1);

// According to the maximum allowable inclination angle, the current D-direction expected lift and the maximum allowable throttle, the maximum allowable lift in the NE plane can be calculated. It is well understood that the lift generated by the whole aircraft can be decomposed into the D-direction of the NED system and the components in the NE plane. The value they are assigned is directly related to the inclination angle of the aircraft. . At the same time, the sum of the squares of the two is equal to the square of the throttle of the aircraft, which is a dimensionless normalized relationship.

// Get maximum allowed thrust in NE based on tilt and excess thrust.
float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
float thrust_max_NE = sqrtf(_param_mpc_thr_max.get() * _param_mpc_thr_max.get() - _thr_sp(2) * _thr_sp(2));
thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);

// Saturate thrust in NE-direction.
_thr_sp(0) = thrust_desired_NE(0);
_thr_sp(1) = thrust_desired_NE(1);

// Lift is limited when the expectation of lift in NE plane exceeds the allowable maximum calculated above.

if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) {
  float mag = thrust_desired_NE.length();
  _thr_sp(0) = thrust_desired_NE(0) / mag * thrust_max_NE;
  _thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE;
}

// Integral anti-saturation processing, when the output of the control law does not reach the limit, _thr_sp and thrust_desired_NE are equal, so the normal integral, when the output reaches the limit, will subtract a value beyond the limit in the opposite direction at the same time of integration, the more it exceeds, the more it subtracts, which can play the role of anti-saturation and avoid the integral after the limit. It continues to grow.

    // Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output
    // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
    float arw_gain = 2.f / _param_mpc_xy_vel_p.get();

    Vector2f vel_err_lim;
    vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain;
    vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;

    // Update integral
    _thr_int(0) += _param_mpc_xy_vel_i.get() * vel_err_lim(0) * dt;
    _thr_int(1) += _param_mpc_xy_vel_i.get() * vel_err_lim(1) * dt;
  }
}

summary

In this paper, the code of position controller in PX4 is explained in detail. The code is analyzed line by line. As for the derivation of the algorithm formula, you can refer to the previous article: UAV controller design (2): position controller design. Now you should set up the whole controller of PX4 multi-rotor aircraft. There is a clear understanding of the design and implementation. In addition to the implementation of the outer loop controller and the inner loop attitude controller, there is also a function that the outer loop controller outputs to the inner loop instruction generation.