Vehicles/Rdd2/Plant.mo
Download original .moGitHub source
Documentation: Vehicles.Rdd2.Plant
within Vehicles.Rdd2;
model Plant "RDD2 quadrotor with native avionics-facing connectors"
constant Real pi = 2.0 * asin(1.0);
parameter Real omegaMax(unit = "rad/s") = 1100.0;
parameter Real vehicleMass_kg(unit = "kg") = 2.0;
parameter Real gravity_m_s2(unit = "m/s2") = 9.80665;
parameter Real thrustCoefficient_N_s2(unit = "N.s2") = 8.54858e-6
"Rotor thrust per squared rotor speed";
parameter Real initialGroundClearance_m(unit = "m") = 0.02
"Landing-leg clearance above the ground plane at initialisation; raising
it starts a study in the air instead of on its legs";
parameter Vehicles.Rdd2.RotorGeometry rotorGeometry =
Vehicles.Rdd2.RotorGeometry();
parameter Real rotorEffectiveness[4, 4] =
Control.Multirotor.Allocation.rotorEffectiveness(
rotorGeometry.positionBodyFlu_m,
rotorGeometry.yawMomentPerThrust_m);
parameter Boolean enableRotorVibration = false
"Add rotor unbalance and blade-pass vibration to the sensed IMU signals;
false reproduces the vibration-free plant exactly"
annotation(Evaluate = true);
parameter Real bladeCount(min = 1.0) = 2.0
"Blades per rotor. Real, not Integer: this model is exported through
FMI 3 for the board-in-the-loop plant and that backend has no Integer
scalar type. The value only ever scales a frequency or a phase, and it
is a whole number by construction."
annotation(Evaluate = true);
parameter Real rotorVibrationAngular_rad_s(unit = "rad/s") = 0.05
"Per-rotor angular-rate vibration amplitude at hover trim";
parameter Real rotorVibrationSpecificForce_m_s2(unit = "m/s2") = 1.0
"Per-rotor lateral specific-force vibration amplitude at hover trim";
parameter Boolean enableImuAntiAliasFilter = enableRotorVibration
"Run the sensor low-pass that precedes sampling in the real part. The
default follows the vibration switch so a vibration-free run keeps the
historical bit-identical trace, while any run that carries rotor content
gets the hardware bandwidth"
annotation(Evaluate = true);
parameter Real imuAntiAliasBandwidth_Hz(unit = "Hz") = 200.0
"ICM-45686 user-interface filter bandwidth, the 1600 Hz output data rate
divided by eight";
parameter Real imuAntiAliasDamping = 0.5 * sqrt(2.0)
"Second-order section damping; the default is maximally flat";
final parameter Real hoverRotorSpeed_rad_s(unit = "rad/s") =
sqrt(vehicleMass_kg * gravity_m_s2 / (4.0 * thrustCoefficient_N_s2))
"Rotor speed that trims four-rotor thrust against weight";
final parameter Real hoverRotorFrequency_Hz(unit = "Hz") =
hoverRotorSpeed_rad_s / (2.0 * pi) "Fundamental rotor tone at hover";
final parameter Real hoverBladePassFrequency_Hz(unit = "Hz") =
bladeCount * hoverRotorFrequency_Hz "Blade-pass tone at hover";
final parameter Real imuAntiAliasNatural_rad_s(unit = "rad/s") =
2.0 * pi * imuAntiAliasBandwidth_Hz;
Vehicles.Rdd2.ControllerInterfaces.MotorCommandInput commands;
Avionics.ImuSampleOutput imu;
Avionics.NavigationEstimateOutput truth;
output Real motorOmega_rad_s[4];
output Real rotorVibrationAngularVelocityBodyFlu_rad_s[3](
each unit = "rad/s") "Angular-rate vibration added to the sensed rate";
output Real rotorVibrationSpecificForceBodyFlu_m_s2[3](
each unit = "m/s2")
"Specific-force vibration added to the sensed specific force";
protected
Vehicles.Templates.QuadrotorPlant dynamics(
vehicle_mass = vehicleMass_kg,
gravity = gravity_m_s2,
vehicle_ixx = 0.02166666666666667,
vehicle_iyy = 0.02166666666666667,
vehicle_izz = 0.04000000000000001,
arm_length = rotorGeometry.armLength_m,
initial_ground_clearance = initialGroundClearance_m,
Ct = thrustCoefficient_N_s2,
Cm = rotorGeometry.rotorTorqueRatio_m,
motor_moment_map = rotorEffectiveness[2:4, :]
);
Vehicles.Rdd2.RotorVibration vibration(
enable = enableRotorVibration,
bladeCount = bladeCount,
rotorGeometry = rotorGeometry,
hoverRotorSpeed_rad_s = hoverRotorSpeed_rad_s,
hoverAngularVibration_rad_s = rotorVibrationAngular_rad_s,
hoverSpecificForceVibration_m_s2 = rotorVibrationSpecificForce_m_s2);
Real eulerB321_rad[3] "Lie-group B321 coordinates [yaw, pitch, roll]";
Real sensedAngularVelocityBodyFlu_rad_s[3]
"Body rate the sensor sees before its own low-pass";
Real sensedSpecificForceBodyFlu_m_s2[3]
"Specific force the sensor sees before its own low-pass";
Real filteredAngularVelocityBodyFlu_rad_s[3](each start = 0.0,
each fixed = enableImuAntiAliasFilter)
"Anti-alias filter output on the gyroscope channel";
Real filteredSpecificForceBodyFlu_m_s2[3](each start = 0.0,
each fixed = enableImuAntiAliasFilter)
"Anti-alias filter output on the accelerometer channel";
Real filteredAngularVelocityRate_rad_s2[3](each start = 0.0,
each fixed = enableImuAntiAliasFilter)
"Gyroscope filter section velocity state";
Real filteredSpecificForceRate_m_s3[3](each start = 0.0,
each fixed = enableImuAntiAliasFilter)
"Accelerometer filter section velocity state";
equation
dynamics.omega_cmd = omegaMax * {
MathUtilities.clip(commands.motor[motorIndex], 0.0, 1.0)
for motorIndex in 1:4};
vibration.rotorSpeed_rad_s = dynamics.omega_m;
rotorVibrationAngularVelocityBodyFlu_rad_s =
vibration.angularVelocityBodyFlu_rad_s;
rotorVibrationSpecificForceBodyFlu_m_s2 =
vibration.specificForceBodyFlu_m_s2;
// The airframe experiences the rotor vibration and the sensor experiences
// it with the airframe, so it enters the sensed signals ahead of the
// sensor's own bandwidth limit and never enters the rigid-body integration.
sensedAngularVelocityBodyFlu_rad_s =
dynamics.gyro + vibration.angularVelocityBodyFlu_rad_s;
sensedSpecificForceBodyFlu_m_s2 =
dynamics.accel + vibration.specificForceBodyFlu_m_s2;
if enableImuAntiAliasFilter then
der(filteredAngularVelocityBodyFlu_rad_s) =
filteredAngularVelocityRate_rad_s2;
der(filteredAngularVelocityRate_rad_s2) =
imuAntiAliasNatural_rad_s * imuAntiAliasNatural_rad_s
* (sensedAngularVelocityBodyFlu_rad_s
- filteredAngularVelocityBodyFlu_rad_s)
- 2.0 * imuAntiAliasDamping * imuAntiAliasNatural_rad_s
* filteredAngularVelocityRate_rad_s2;
der(filteredSpecificForceBodyFlu_m_s2) = filteredSpecificForceRate_m_s3;
der(filteredSpecificForceRate_m_s3) =
imuAntiAliasNatural_rad_s * imuAntiAliasNatural_rad_s
* (sensedSpecificForceBodyFlu_m_s2
- filteredSpecificForceBodyFlu_m_s2)
- 2.0 * imuAntiAliasDamping * imuAntiAliasNatural_rad_s
* filteredSpecificForceRate_m_s3;
else
filteredAngularVelocityBodyFlu_rad_s =
sensedAngularVelocityBodyFlu_rad_s;
filteredAngularVelocityRate_rad_s2 = {0.0, 0.0, 0.0};
filteredSpecificForceBodyFlu_m_s2 = sensedSpecificForceBodyFlu_m_s2;
filteredSpecificForceRate_m_s3 = {0.0, 0.0, 0.0};
end if;
imu.valid = true;
imu.fresh = true;
imu.timestamp_s = time;
imu.angularVelocityBodyFlu_rad_s = filteredAngularVelocityBodyFlu_rad_s;
imu.specificForceBodyFlu_m_s2 = filteredSpecificForceBodyFlu_m_s2;
// Literal arrays rather than zeros(): the board-in-the-loop plant is
// exported through FMI 3, whose backend renders a tensor equation one
// scalar at a time and has no scalar form for a fill node. The plant does
// not preintegrate, so its raw sample carries the identity preintegral.
imu.deltaAngleBodyFlu_rad = {0.0, 0.0, 0.0};
imu.deltaVelocityBodyFlu_m_s = {0.0, 0.0, 0.0};
imu.deltaPositionBodyFlu_m = {0.0, 0.0, 0.0};
imu.deltaQuaternionBodyFlu = {1.0, 0.0, 0.0, 0.0};
imu.integrationTime_s = 0.0;
imu.gyroscopeBiasLinearizationBodyFlu_rad_s = {0.0, 0.0, 0.0};
imu.accelerometerBiasLinearizationBodyFlu_m_s2 = {0.0, 0.0, 0.0};
imu.deltaRotationGyroscopeBiasJacobian_s =
{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}};
imu.deltaVelocityGyroscopeBiasJacobian_m =
{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}};
imu.deltaVelocityAccelerometerBiasJacobian_s =
{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}};
imu.deltaPositionGyroscopeBiasJacobian_m_s =
{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}};
imu.deltaPositionAccelerometerBiasJacobian_s2 =
{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}};
eulerB321_rad = LieGroups.SO3.EulerB321.from_Quat(dynamics.quat);
truth.valid = true;
truth.timestamp_s = time;
truth.positionWorldEnu_m = dynamics.position;
truth.velocityWorldEnu_m_s = dynamics.velocity;
truth.accelerationWorldEnu_m_s2 =
dynamics.R * dynamics.accel + {0.0, 0.0, -dynamics.gravity};
truth.quaternionWorldBody = dynamics.quat;
truth.rotationWorldBody = dynamics.R;
truth.eulerRpy_rad = {
eulerB321_rad[3], eulerB321_rad[2], eulerB321_rad[1]};
truth.angularVelocityBodyFlu_rad_s = dynamics.gyro;
truth.angularVelocityWorldEnu_rad_s = dynamics.R * dynamics.gyro;
motorOmega_rad_s = dynamics.omega_m;
annotation(Documentation(info = "<html>
<p>RDD2 quadrotor plant behind the avionics-facing IMU and truth
connectors.</p>
<h4>Rotor vibration</h4>
<p><code>enableRotorVibration</code> adds
<code>Vehicles.Rdd2.RotorVibration</code> to the sensed IMU signals. The
hover reference frequency is derived here rather than assumed: trimming
four rotors against weight gives
<code>hoverRotorSpeed_rad_s = sqrt(mass * gravity / (4 * Ct))</code>,
which for the RDD2 mass, gravity, and thrust coefficient puts the
fundamental near 120 Hz and the two-blade passage near 241 Hz. The
disturbance is driven from the live rotor speeds, so it tracks throttle
through a mission. Truth outputs stay vibration free because the motion
is an oscillation about the flight trim with no net displacement.</p>
<h4>Sensor bandwidth</h4>
<p><code>enableImuAntiAliasFilter</code> applies the second-order
low-pass that the ICM-45686 runs ahead of its sampler. The part is
configured at a 1600 Hz output data rate with the user-interface filter
at one eighth of that, so the default bandwidth is 200 Hz. Without this
filter the model hands unattenuated rotor content to a point sampler and
misrepresents what the hardware delivers. The switch defaults to the
vibration switch so vibration-free runs keep their historical trace, and
it can be forced either way to separate the filter's contribution from
the vibration's.</p>
</html>"));
end Plant;