Tests.RotorVibrationHoverStudy.HoverCase

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

model HoverCase
    "One vibration point flown in hover with its IMU stream dead reckoned"
    constant Real pi = 2.0 * asin(1.0);
    parameter Boolean vibrationEnabled = true
      "False flies the same hover with no rotor content, which is the floor
       that every vibration case has to be read against";
    parameter Real vibrationAngularAmplitude_rad_s(unit = "rad/s") = 0.05
      "Per-rotor angular-rate vibration amplitude at hover trim";
    parameter Real vibrationSpecificForceAmplitude_m_s2(unit = "m/s2") = 1.0
      "Per-rotor lateral specific-force vibration amplitude at hover trim";
    parameter Real vehicleMass_kg(unit = "kg") = 2.0;
    parameter Real gravity_m_s2(unit = "m/s2") = 9.80665;
    parameter Real propellerThrustCoefficient_N_s2(unit = "N.s2") = 8.54858e-6
      "RDD2 propeller thrust per squared rotor speed";
    parameter Real hoverFundamentalTarget_Hz(unit = "Hz") = 0.0
      "Zero flies the RDD2 propeller. A positive value sizes a propeller that
       trims at that tone instead, which moves the vibration frequency
       without moving its amplitude, because the amplitude law is referenced
       to each case own hover speed";
    parameter Real thrustToWeightRatio(min = 1.2) = 2.0
      "Full-command thrust as a multiple of weight, held equal across the
       propeller cases so they share one control authority";
    parameter Boolean antiAliasFilter = true
      "Run the sensor low-pass that precedes sampling in the real part";
    parameter Real imuSamplePeriod_s(unit = "s") = 0.001
      "Raw IMU sampling interval, matching the vehicle composition rate";
    parameter Real initialGroundClearance_m(unit = "m") = 1.0
      "Start the case in the air so the study measures hover and not a
       landing-gear contact transient";
    parameter Real hoverAltitude_m(unit = "m") = 1.1
      "Hover hold altitude, set to the initial trim height";
    parameter Real altitudeGain_1_s2(unit = "1/s2") = 2.0;
    parameter Real climbRateGain_1_s(unit = "1/s") = 3.0;
    parameter Real attitudeMomentGain_N_m(unit = "N.m") = 0.65;
    parameter Real rateMomentGain_N_m_s(unit = "N.m.s") = 0.17;
    parameter Vehicles.Rdd2.RotorGeometry rotorGeometry =
      Vehicles.Rdd2.RotorGeometry();

    final parameter Real thrustCoefficient_N_s2(unit = "N.s2") =
      if hoverFundamentalTarget_Hz > 0.0 then
        vehicleMass_kg * gravity_m_s2
          / (4.0 * (2.0 * pi * hoverFundamentalTarget_Hz) ^ 2)
      else propellerThrustCoefficient_N_s2
      "Propeller sized either from the airframe or from the target tone";
    final parameter Real trimRotorSpeed_rad_s(unit = "rad/s") =
      sqrt(vehicleMass_kg * gravity_m_s2 / (4.0 * thrustCoefficient_N_s2))
      "Rotor speed that trims four rotors against weight";
    final parameter Real maximumRotorSpeed_rad_s(unit = "rad/s") =
      trimRotorSpeed_rad_s * sqrt(thrustToWeightRatio)
      "Full-command rotor speed that realises the commanded thrust margin";

    Vehicles.Rdd2.Plant plant(
      rotorGeometry = rotorGeometry,
      vehicleMass_kg = vehicleMass_kg,
      gravity_m_s2 = gravity_m_s2,
      omegaMax = maximumRotorSpeed_rad_s,
      initialGroundClearance_m = initialGroundClearance_m,
      thrustCoefficient_N_s2 = thrustCoefficient_N_s2,
      enableRotorVibration = vibrationEnabled,
      rotorVibrationAngular_rad_s = vibrationAngularAmplitude_rad_s,
      rotorVibrationSpecificForce_m_s2 = vibrationSpecificForceAmplitude_m_s2,
      enableImuAntiAliasFilter = antiAliasFilter);

    final parameter Real wrenchToRotorThrust[4, 4] =
      Control.Multirotor.Allocation.quadrotorWrenchToThrust(
        rotorGeometry.positionBodyFlu_m,
        rotorGeometry.yawMomentPerThrust_m);
    final parameter Real hoverFundamental_Hz(unit = "Hz") =
      plant.hoverRotorFrequency_Hz
      "Fundamental rotor tone derived from this case's propeller";
    final parameter Real hoverBladePass_Hz(unit = "Hz") =
      plant.hoverBladePassFrequency_Hz "Blade-pass tone at hover trim";

    output Real zohAttitudeError_rad(unit = "rad", start = 0.0, fixed = true)
      "Zero-order-hold composition error against the vibration-free truth";
    output Real fohAttitudeError_rad(unit = "rad", start = 0.0, fixed = true)
      "First-order-hold composition error against the vibration-free truth";
    output Real zohVelocityError_m_s(unit = "m/s", start = 0.0,
      fixed = true)
      "Zero-order-hold delta-velocity error against the vibration-free truth";
    output Real fohVelocityError_m_s(unit = "m/s", start = 0.0,
      fixed = true)
      "First-order-hold delta-velocity error against the vibration-free truth";
    output Real sensedAngularVibration_rad_s(unit = "rad/s")
      "Magnitude of the angular disturbance injected ahead of the sensor
       bandwidth limit";
    output Real sensedSpecificForceVibration_m_s2(unit = "m/s2")
      "Magnitude of the specific-force disturbance injected ahead of the
       sensor bandwidth limit";
    output Real altitudeError_m(unit = "m")
      "Hover hold error, a witness that the case really flew";

  protected
    Real thrustCommand_N(unit = "N");
    Real momentCommand_N_m[3](each unit = "N.m");
    Real referenceVelocity_m_s[3](each unit = "m/s", each start = 0.0,
      each fixed = true)
      "Specific force integrated on the vibration-free truth attitude";
    discrete Real zohDeltaPosition_m[3](each start = 0.0, each fixed = true);
    discrete Real zohDeltaVelocity_m_s[3](each start = 0.0,
      each fixed = true);
    discrete Real zohDeltaQuaternion[4](start = {1.0, 0.0, 0.0, 0.0},
      each fixed = true);
    discrete Real fohDeltaPosition_m[3](each start = 0.0, each fixed = true);
    discrete Real fohDeltaVelocity_m_s[3](each start = 0.0,
      each fixed = true);
    discrete Real fohDeltaQuaternion[4](start = {1.0, 0.0, 0.0, 0.0},
      each fixed = true);
    discrete Real previousAngularVelocitySample_rad_s[3](each start = 0.0,
      each fixed = true);
    discrete Real previousSpecificForceSample_m_s2[3](each start = 0.0,
      each fixed = true);
    discrete Real scratchRotationJacobian[3, 3](each start = 0.0,
      each fixed = true);
    discrete Real scratchVelocityGyroscopeJacobian[3, 3](each start = 0.0,
      each fixed = true);
    discrete Real scratchVelocityAccelerometerJacobian[3, 3](
      each start = 0.0, each fixed = true);
    discrete Real scratchPositionGyroscopeJacobian[3, 3](each start = 0.0,
      each fixed = true);
    discrete Real scratchPositionAccelerometerJacobian[3, 3](
      each start = 0.0, each fixed = true);

  equation
    // Truth-feedback hover hold. The controller reads the vibration-free
    // truth on purpose, so the study measures the inertial mechanization
    // cost of vibration without also folding in a control response to it.
    thrustCommand_N = max(0.0, plant.vehicleMass_kg * (plant.gravity_m_s2
      + altitudeGain_1_s2
        * (hoverAltitude_m - plant.truth.positionWorldEnu_m[3])
      - climbRateGain_1_s * plant.truth.velocityWorldEnu_m_s[3]));
    momentCommand_N_m = -attitudeMomentGain_N_m * plant.truth.eulerRpy_rad
      - rateMomentGain_N_m_s * plant.truth.angularVelocityBodyFlu_rad_s;
    plant.commands.motor = Control.Multirotor.Allocation.rotorCommands(
      4,
      thrustCommand_N,
      momentCommand_N_m,
      wrenchToRotorThrust,
      fill(thrustCoefficient_N_s2, 4),
      fill(maximumRotorSpeed_rad_s, 4));

    der(referenceVelocity_m_s) = plant.truth.accelerationWorldEnu_m_s2
      + {0.0, 0.0, plant.gravity_m_s2};
    altitudeError_m = plant.truth.positionWorldEnu_m[3] - hoverAltitude_m;
    sensedAngularVibration_rad_s = MathUtilities.norm3(
      plant.rotorVibrationAngularVelocityBodyFlu_rad_s);
    sensedSpecificForceVibration_m_s2 = MathUtilities.norm3(
      plant.rotorVibrationSpecificForceBodyFlu_m_s2);

  algorithm
    when sample(0.0, imuSamplePeriod_s) then
      if time >= 0.5 * imuSamplePeriod_s then
        // Mirror the vehicle driver: the sample closing the interval is held
        // over it under the zero-order hold, while the first-order hold
        // interpolates from the sample that opened it.
        (zohDeltaPosition_m,
         zohDeltaVelocity_m_s,
         zohDeltaQuaternion,
         scratchRotationJacobian,
         scratchVelocityGyroscopeJacobian,
         scratchVelocityAccelerometerJacobian,
         scratchPositionGyroscopeJacobian,
         scratchPositionAccelerometerJacobian) :=
          Estimation.StrapdownINS.preintegrateImuStep(
            pre(zohDeltaPosition_m),
            pre(zohDeltaVelocity_m_s),
            pre(zohDeltaQuaternion),
            zeros(3, 3), zeros(3, 3), zeros(3, 3), zeros(3, 3), zeros(3, 3),
            plant.imu.angularVelocityBodyFlu_rad_s,
            plant.imu.specificForceBodyFlu_m_s2,
            zeros(3), zeros(3), imuSamplePeriod_s);
        (fohDeltaPosition_m,
         fohDeltaVelocity_m_s,
         fohDeltaQuaternion,
         scratchRotationJacobian,
         scratchVelocityGyroscopeJacobian,
         scratchVelocityAccelerometerJacobian,
         scratchPositionGyroscopeJacobian,
         scratchPositionAccelerometerJacobian) :=
          Estimation.StrapdownINS.preintegrateImuStep(
            pre(fohDeltaPosition_m),
            pre(fohDeltaVelocity_m_s),
            pre(fohDeltaQuaternion),
            zeros(3, 3), zeros(3, 3), zeros(3, 3), zeros(3, 3), zeros(3, 3),
            plant.imu.angularVelocityBodyFlu_rad_s,
            plant.imu.specificForceBodyFlu_m_s2,
            zeros(3), zeros(3), imuSamplePeriod_s,
            true,
            pre(previousAngularVelocitySample_rad_s),
            pre(previousSpecificForceSample_m_s2));
      end if;
      previousAngularVelocitySample_rad_s :=
        plant.imu.angularVelocityBodyFlu_rad_s;
      previousSpecificForceSample_m_s2 :=
        plant.imu.specificForceBodyFlu_m_s2;
      zohAttitudeError_rad := MathUtilities.norm3(
        LieGroups.SO3.Quat.log_map(LieGroups.SO3.Quat.product(
          LieGroups.SO3.Quat.inverse(plant.truth.quaternionWorldBody),
          zohDeltaQuaternion)));
      fohAttitudeError_rad := MathUtilities.norm3(
        LieGroups.SO3.Quat.log_map(LieGroups.SO3.Quat.product(
          LieGroups.SO3.Quat.inverse(plant.truth.quaternionWorldBody),
          fohDeltaQuaternion)));
      zohVelocityError_m_s := MathUtilities.norm3(
        zohDeltaVelocity_m_s - referenceVelocity_m_s);
      fohVelocityError_m_s := MathUtilities.norm3(
        fohDeltaVelocity_m_s - referenceVelocity_m_s);
    end when;
  end HoverCase;