function

Tests.PositionLoopStabilityTests.commandedPitchRate

Body pitch rate the shipped control law commands for one hover state

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default
input	`positionErrorEast_m`	`Real`
input	`velocityErrorEast_m_s`	`Real`
input	`pitch_rad`	`Real`
input	`mass`	`Real`
input	`gravity`	`Real`
input	`thrustTrim`	`Real`
input	`attitudeGain`	`Real`	`[3]`
output	`pitchRate_rad_s`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function commandedPitchRate
    "Body pitch rate the shipped control law commands for one hover state"
    input Real positionErrorEast_m;
    input Real velocityErrorEast_m_s;
    input Real pitch_rad;
    input Real mass;
    input Real gravity;
    input Real thrustTrim;
    input Real attitudeGain[3];
    output Real pitchRate_rad_s;
  protected
    Real quaternionWorldBody[4];
    Real error[9];
    Control.Multirotor.LogLinear.OuterLoopResult result;
    Real angularVelocity[3];
  algorithm
    // Pitch about body y tilts world z toward world x, which is the axis the
    // east position loop closes through.
    quaternionWorldBody := {cos(0.5 * pitch_rad), 0.0, sin(0.5 * pitch_rad),
      0.0};
    error := Control.Multirotor.LogLinear.stateError(
      {positionErrorEast_m, 0.0, 0.0},
      {velocityErrorEast_m_s, 0.0, 0.0},
      quaternionWorldBody,
      {0.0, 0.0, 0.0},
      {0.0, 0.0, 0.0},
      {1.0, 0.0, 0.0, 0.0});
    result := Control.Multirotor.LogLinear.outerLoop(
      error,
      {0.0, 0.0, 0.0},
      quaternionWorldBody,
      {1.0, 0.0, 0.0, 0.0},
      {0.0, 0.0, 0.0},
      thrustTrim,
      mass,
      gravity);
    angularVelocity := Control.Multirotor.LogLinear.attitudeControl(
      attitudeGain, quaternionWorldBody, result.attitudeSetpoint)
      + result.angularVelocityCorrection;
    pitchRate_rad_s := angularVelocity[2];
  end commandedPitchRate;