function

Control.Allocation.rotorWrenchEffectiveness

Map rotor thrust magnitudes into a full body-frame force and moment wrench

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`positionBody_m`	`Real`	`[:, 3]`	Rotor centers relative to the center of mass
input	`thrustDirectionBody`	`Real`	`[size(positionBody_m, 1), 3]`	Thrust direction for each rotor; normalized internally
input	`reactionTorquePerThrust_m`	`Real`	`[size(positionBody_m, 1)]`	Signed reaction torque along each rotor thrust axis
output	`effectiveness`	`Real`	`[6, size(positionBody_m, 1)]`	Maps rotor thrust into {forceBody, momentBody}

Modelica declarations and implementation

Show parameters, interfaces and source

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

function rotorWrenchEffectiveness
    "Map rotor thrust magnitudes into a full body-frame force and moment wrench"
    input Real positionBody_m[:, 3]
      "Rotor centers relative to the center of mass";
    input Real thrustDirectionBody[size(positionBody_m, 1), 3]
      "Thrust direction for each rotor; normalized internally";
    input Real reactionTorquePerThrust_m[size(positionBody_m, 1)]
      "Signed reaction torque along each rotor thrust axis";
    output Real effectiveness[6, size(positionBody_m, 1)]
      "Maps rotor thrust into {forceBody, momentBody}";
  protected
    Real directionNorm;
    Real direction[3];
  algorithm
    for rotor in 1:size(positionBody_m, 1) loop
      directionNorm := sqrt(sum(
        thrustDirectionBody[rotor, axis] ^ 2 for axis in 1:3));
      assert(directionNorm > 1.0e-12,
        "Every rotor must have a nonzero thrust direction");
      direction := thrustDirectionBody[rotor, :] / directionNorm;
      effectiveness[1:3, rotor] := direction;
      effectiveness[4:6, rotor] :=
        cross(positionBody_m[rotor, :], direction)
          + reactionTorquePerThrust_m[rotor] * direction;
    end for;
  end rotorWrenchEffectiveness;