Control.Multirotor.LogLinear.Controller

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

block Controller
    "Sampled integral state around the continuous log-linear control maps"
    parameter Real samplePeriod(unit = "s") = 0.01;
    parameter Real mass(unit = "kg") = 1.0;
    parameter Real gravity(unit = "m/s2") = 9.80665;
    parameter Real thrustTrim(unit = "N") = mass * gravity;
    parameter Real attitudeGain[3](each unit = "1/s") = {2.0, 2.0, 1.0};
    parameter Real positionGain_s2[3] = {1.5, 1.5, 2.0};
    parameter Real velocityGain_s[3] = {2.2, 2.2, 2.5};
    parameter Real integralGain[3] = {0.05, 0.05, 0.05};
    parameter Real integralFractionLimit[3] = {0.25, 0.25, 0.25};
    parameter Real translationalCorrectionFraction = 0.3;

    input Real positionWorld[3](each unit = "m", each start = 0.0);
    input Real velocityWorld[3](each unit = "m/s", each start = 0.0);
    input Real quaternionWorldBody[4](
      each unit = "1",
      start = {1.0, 0.0, 0.0, 0.0});
    input Real positionReferenceWorld[3](each unit = "m", each start = 0.0);
    input Real velocityReferenceWorld[3](each unit = "m/s", each start = 0.0);
    input Real accelerationReferenceWorld[3](
      each unit = "m/s2",
      each start = 0.0);
    input Real headingQuaternionReference[4](
      each unit = "1",
      start = {1.0, 0.0, 0.0, 0.0});
    input Boolean resetIntegral(start = false);

    output Real stateError[9]
      "SE_2(3) tangent ordered {position,velocity,rotation}";
    output Real thrust(unit = "N") "Collective thrust force";
    output Real attitudeSetpoint[4](each unit = "1")
      "Body-to-world quaternion {w,x,y,z}";
    output Real angularVelocitySetpoint[3](each unit = "rad/s")
      "Commanded body angular velocity";
    output Real angularVelocityCorrection[3](each unit = "rad/s")
      "Rotational part of the SE_2(3) correction";
    discrete output Real positionIntegralWorld[3](
      each start = 0.0,
      each fixed = true);

  protected
    Control.Multirotor.LogLinear.OuterLoopResult outerLoopResult;
  equation
    assert(samplePeriod > 0.0, "Controller sample period must be positive");
    stateError = Control.Multirotor.LogLinear.stateError(
      positionWorld,
      velocityWorld,
      quaternionWorldBody,
      positionReferenceWorld,
      velocityReferenceWorld,
      headingQuaternionReference);
    outerLoopResult = Control.Multirotor.LogLinear.outerLoop(
      stateError,
      accelerationReferenceWorld,
      quaternionWorldBody,
      headingQuaternionReference,
      positionIntegralWorld,
      thrustTrim,
      mass,
      gravity,
      positionGain_s2,
      velocityGain_s,
      integralGain,
      translationalCorrectionFraction);
    thrust = outerLoopResult.thrust;
    attitudeSetpoint = outerLoopResult.attitudeSetpoint;
    angularVelocityCorrection = outerLoopResult.angularVelocityCorrection;
    angularVelocitySetpoint = Control.Multirotor.LogLinear.attitudeControl(
      attitudeGain,
      quaternionWorldBody,
      attitudeSetpoint);

  algorithm
    when sample(0.0, samplePeriod) then
      if resetIntegral then
        positionIntegralWorld := zeros(3);
      else
        positionIntegralWorld :=
          Control.Multirotor.LogLinear.integratePositionError(
          positionReferenceWorld - positionWorld,
          pre(positionIntegralWorld),
          samplePeriod,
          gravity,
          integralGain,
          integralFractionLimit);
      end if;
    end when;

    annotation(Documentation(info="<html>
      <p>Combines the pure control functions with a sampled, bounded position
      integral. The geometric proportional feedback remains algebraic and is
      therefore available continuously between integral updates.</p>
      <p>Vehicle packages normally extend this model to provide mass, gravity,
      trim thrust, and gains. Connect <code>angularVelocitySetpoint</code> to a
      body-rate loop and <code>thrust</code> to the vehicle's control allocator.</p>
    </html>"));
  end Controller;