function

Control.Multirotor.Allocation.quadrotorWrenchToThrust

Derive the square quadrotor allocation inverse from physical geometry

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`positionBodyFlu_m`	`Real`	`[4, 3]`
input	`yawMomentPerThrust_m`	`Real`	`[4]`
output	`wrenchToRotorThrust`	`Real`	`[4, 4]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function quadrotorWrenchToThrust
    "Derive the square quadrotor allocation inverse from physical geometry"
    input Real positionBodyFlu_m[4, 3];
    input Real yawMomentPerThrust_m[4];
    output Real wrenchToRotorThrust[4, 4];
  protected
    Real effectiveness[4, 4];
    Boolean accepted;
  algorithm
    effectiveness := rotorEffectiveness(
      positionBodyFlu_m, yawMomentPerThrust_m);
    (wrenchToRotorThrust, accepted) := LinearAlgebra.solve(
      effectiveness, identity(4));
    assert(accepted,
      "Quadrotor geometry must provide independent thrust, roll, pitch, and yaw authority");
  end quadrotorWrenchToThrust;