Control.Multirotor.LogLinear.outerLoop

Parameters, inputs, outputs, defaults and equations from the source file.

function outerLoop
    "Construct collective thrust and attitude from SE_2(3) feedback"
    input Real stateError[9]
      "SE_2(3) tangent ordered {position,velocity,rotation}";
    input Real accelerationReferenceWorld[3](each unit = "m/s2")
      "Feed-forward world acceleration";
    input Real quaternionWorldBody[4](each unit = "1")
      "Actual body-to-world quaternion used to rotate invariant errors";
    input Real headingQuaternionWorldBody[4](each unit = "1")
      "Yaw reference used to construct the commanded body frame";
    input Real positionIntegralWorld[3]
      "World-frame position-error integral state";
    input Real thrustTrim(unit = "N") "Nominal collective thrust";
    input Real mass(unit = "kg") = 1.0 "Vehicle mass";
    input Real gravity(unit = "m/s2") = 9.80665
      "Positive gravitational acceleration";
    input Real positionGain_s2[3] = {1.5, 1.5, 2.0}
      "Position-error to acceleration gain";
    input Real velocityGain_s[3] = {2.2, 2.2, 2.5}
      "Velocity-error to acceleration gain";
    input Real integralGain[3] = {0.05, 0.05, 0.05};
    input Real translationalCorrectionFraction = 0.3
      "Maximum feedback force divided by vehicle weight";
    output Control.Multirotor.LogLinear.OuterLoopResult result;
  protected
    constant Real worldX[3] = {1.0, 0.0, 0.0};
    constant Real worldY[3] = {0.0, 1.0, 0.0};
    constant Real worldZ[3] = {0.0, 0.0, 1.0};
    Real feedbackAccelerationBody[3](each unit = "m/s2");
    Real feedbackAccelerationWorld[3](each unit = "m/s2");
    Real feedbackAccelerationNorm(unit = "m/s2");
    Real feedbackAccelerationLimit(unit = "m/s2");
    Real thrustVectorWorld[3](each unit = "N");
    Real thrustDirectionWorld[3];
    Real headingEuler[3](each unit = "rad");
    Real headingDirectionWorld[3];
    Real headingBasisWorld[3];
    Real bodyYWorld[3];
    Real bodyYNorm;
    Real bodyXWorld[3];
    Real bodyToWorld[3, 3];
  algorithm
    assert(mass > 0.0, "Vehicle mass must be positive");
    assert(gravity > 0.0, "Gravity must be positive");
    assert(thrustTrim >= 0.0, "Trim thrust must be nonnegative");
    assert(translationalCorrectionFraction >= 0.0,
      "The translational correction fraction must be nonnegative");
    for axis in 1:3 loop
      assert(positionGain_s2[axis] >= 0.0,
        "Every position gain must be nonnegative");
      assert(velocityGain_s[axis] >= 0.0,
        "Every velocity gain must be nonnegative");
    end for;

    // The SE_2(3) logarithm expresses position and velocity error in the
    // current body frame. Map those errors to an acceleration with explicit
    // physical gains, then rotate once into world coordinates. The previous
    // implementation added two unit-incompatible LQR output blocks directly
    // to a force while leaving a coupled angular-rate block unscaled by mass;
    // at the RDD2's 2 kg mass that changes the horizontal feedback sign and
    // creates a +0.0418 1/s pole.
    feedbackAccelerationBody := positionGain_s2 .* stateError[1:3]
      + velocityGain_s .* stateError[4:6];
    feedbackAccelerationWorld := LieGroups.SO3.Quat.rotate(
      quaternionWorldBody, feedbackAccelerationBody);
    feedbackAccelerationNorm :=
      MathUtilities.norm3(feedbackAccelerationWorld);
    feedbackAccelerationLimit := translationalCorrectionFraction * gravity;
    if feedbackAccelerationNorm > feedbackAccelerationLimit then
      feedbackAccelerationWorld := feedbackAccelerationLimit
        * feedbackAccelerationWorld / feedbackAccelerationNorm;
    end if;

    // Feed-forward is deliberately outside the feedback limiter: clipping a
    // feasible trajectory acceleration must not consume the error-feedback
    // authority. All acceleration terms become force through the same mass.
    thrustVectorWorld := mass * (accelerationReferenceWorld
        + feedbackAccelerationWorld
        + integralGain .* positionIntegralWorld)
      + thrustTrim * worldZ;
    // The attitude setpoint already represents the horizontal position
    // correction. Adding the old LQR rotation rows as a second body-rate
    // command double-counted that correction and caused the unstable pole.
    result.angularVelocityCorrection := zeros(3);
    result.thrust := MathUtilities.norm3(thrustVectorWorld);
    thrustDirectionWorld := if result.thrust > 1.0e-3 then
        thrustVectorWorld / result.thrust
      else
        worldZ;

    headingEuler := LieGroups.SO3.EulerB321.from_Quat(
      headingQuaternionWorldBody);
    headingDirectionWorld := {
      cos(headingEuler[1]),
      sin(headingEuler[1]),
      0.0};
    headingBasisWorld := headingDirectionWorld;
    bodyYWorld := cross(thrustDirectionWorld, headingBasisWorld);
    bodyYNorm := MathUtilities.norm3(bodyYWorld);
    if bodyYNorm <= 1.0e-3 then
      headingBasisWorld := if abs(thrustDirectionWorld[1])
          <= abs(thrustDirectionWorld[2])
          and abs(thrustDirectionWorld[1])
          <= abs(thrustDirectionWorld[3]) then
          worldX
        else if abs(thrustDirectionWorld[2])
          <= abs(thrustDirectionWorld[3]) then
          worldY
        else
          worldZ;
      bodyYWorld := cross(thrustDirectionWorld, headingBasisWorld);
      bodyYNorm := MathUtilities.norm3(bodyYWorld);
    end if;
    assert(bodyYNorm > 1.0e-6,
      "Unable to construct a body frame from the thrust direction");
    bodyYWorld := bodyYWorld / bodyYNorm;
    bodyXWorld := cross(bodyYWorld, thrustDirectionWorld);
    bodyToWorld[:, 1] := bodyXWorld;
    bodyToWorld[:, 2] := bodyYWorld;
    bodyToWorld[:, 3] := thrustDirectionWorld;
    result.attitudeSetpoint := LieGroups.SO3.Quat.from_DCM(bodyToWorld);
    annotation(Documentation(info="<html>
      <p>Maps the body-local SE_2(3) position and velocity errors into a
      bounded world-frame acceleration, adds un-clipped trajectory
      feed-forward, world-frame integral acceleration, and trim, and converts
      the resulting force into collective thrust and a body-to-world attitude
      setpoint.</p>
      <p>If the thrust vector is nearly zero, world z is used
      as the thrust direction; a least-aligned Cartesian axis resolves the
      heading singularity.</p>
    </html>"));
  end outerLoop;