function

Control.Multirotor.RateLoop.bodyMoment

Body moment from a proportional body-rate error with gyroscopic feed-forward

Maps a body-rate error into a body moment for a rigid multirotor: M = I (k (omega_sp - omega)) + omega x (I omega). The gyroscopic term is a feed-forward that cancels the rigid-body coupling so the proportional term only has to close the tracking error. Inputs and the returned moment share the vehicle body frame.

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`angularVelocitySetpoint`	`Real`	`[3](each unit = "rad/s")`	Commanded body angular velocity
input	`angularVelocity`	`Real`	`[3](each unit = "rad/s")`	Measured body angular velocity
input	`inertia`	`Real`	`[3](each unit = "kg.m2")`	Diagonal body inertia {ixx, iyy, izz}
input	`rateGain`	`Real`	`[3](each unit = "1/s")`	Proportional body-rate bandwidth per axis
input	`gyroscopicFeedforward`	`Boolean`	`= true`	Add omega x (I omega) so the moment holds a spinning body on rate
output	`moment`	`Real`	`[3](each unit = "N.m")`	Commanded body moment

Modelica declarations and implementation

Show parameters, interfaces and source

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

function bodyMoment
    "Body moment from a proportional body-rate error with gyroscopic feed-forward"
    input Real angularVelocitySetpoint[3](each unit = "rad/s")
      "Commanded body angular velocity";
    input Real angularVelocity[3](each unit = "rad/s")
      "Measured body angular velocity";
    input Real inertia[3](each unit = "kg.m2")
      "Diagonal body inertia {ixx, iyy, izz}";
    input Real rateGain[3](each unit = "1/s")
      "Proportional body-rate bandwidth per axis";
    input Boolean gyroscopicFeedforward = true
      "Add omega x (I omega) so the moment holds a spinning body on rate";
    output Real moment[3](each unit = "N.m") "Commanded body moment";
  protected
    Real angularMomentum[3](each unit = "kg.m2/s");
  algorithm
    for axis in 1:3 loop
      moment[axis] := inertia[axis] * rateGain[axis]
        * (angularVelocitySetpoint[axis] - angularVelocity[axis]);
    end for;
    if gyroscopicFeedforward then
      angularMomentum := {
        inertia[1] * angularVelocity[1],
        inertia[2] * angularVelocity[2],
        inertia[3] * angularVelocity[3]};
      moment := moment + cross(angularVelocity, angularMomentum);
    end if;
    annotation(Documentation(info="<html>
      <p>Maps a body-rate error into a body moment for a rigid multirotor:
      <code>M = I (k (omega_sp - omega)) + omega x (I omega)</code>. The
      gyroscopic term is a feed-forward that cancels the rigid-body coupling so
      the proportional term only has to close the tracking error. Inputs and the
      returned moment share the vehicle body frame.</p>
    </html>"));
  end bodyMoment;