121 lines
6.4 KiB
Plaintext
121 lines
6.4 KiB
Plaintext
|
within Requirements;
|
|||
|
|
|
|||
|
|
model StabilityRequirement "SR-WIND-02: The UAS shall maintain lateral and yaw stability under wind disturbance and recover within limits"
|
|||
|
|
// ---- System-level parameters (traceable to SR-WIND-02) ----
|
|||
|
|
parameter Real hoverTolerance = 1.5 "Maximum allowed lateral deviation during hover [m]";
|
|||
|
|
parameter Real yawTolerance = 5 "Maximum allowed yaw error [deg]";
|
|||
|
|
parameter Real recoveryBand = 0.5 "Position error band for recovery after disturbance [m]";
|
|||
|
|
parameter Real recoveryTime = 3 "Time to remain within recovery band [s]";
|
|||
|
|
parameter Real eps = 1e-6 "Numerical regularization term to avoid division by zero when normalizing the trajectory direction.";
|
|||
|
|
|
|||
|
|
// ---- Inputs from the system ----
|
|||
|
|
Modelica.Blocks.Interfaces.RealInput posX "Measured X position [m]" annotation(
|
|||
|
|
Placement(transformation(origin = {22, 0}, extent = {{120, 40}, {80, 80}}), iconTransformation(origin = {20, 0}, extent = {{120, 40}, {80, 80}})));
|
|||
|
|
Modelica.Blocks.Interfaces.RealInput posY "Measured Y position [m]" annotation(
|
|||
|
|
Placement(transformation(origin = {20, 60}, extent = {{120, -80}, {80, -40}}), iconTransformation(origin = {20, 0}, extent = {{120, -80}, {80, -40}})));
|
|||
|
|
Modelica.Blocks.Interfaces.RealInput yaw "Measured yaw angle [deg]" annotation(
|
|||
|
|
Placement(transformation(origin = {20, -70}, extent = {{120, -10}, {80, 30}}), iconTransformation(origin = {20, -10}, extent = {{120, -10}, {80, 30}})));
|
|||
|
|
Modelica.Blocks.Interfaces.RealInput DronePositionConsign[3] "Commanded reference position [m]" annotation(
|
|||
|
|
Placement(transformation(origin = {-158, 120}, extent = {{-20, 120}, {20, 80}}, rotation = -90), iconTransformation(origin = {-40, 120},extent = {{-20, 120}, {20, 80}}, rotation = -90)));
|
|||
|
|
Modelica.Blocks.Interfaces.RealInput yawConsign "Commanded yaw angle [deg]" annotation(
|
|||
|
|
Placement(transformation(origin = {-40, 80}, extent = {{-60, 120}, {-20, 80}}, rotation = -90), iconTransformation(origin = {-158, 78},extent = {{-60, 120}, {-20, 80}}, rotation = -90)));
|
|||
|
|
// ---- Outputs for monitoring ----
|
|||
|
|
Modelica.Blocks.Interfaces.RealOutput lateralError "2D lateral position error magnitude [m]" annotation(
|
|||
|
|
Placement(transformation(origin = {-10, 20}, extent = {{-90, 50}, {-110, 70}}), iconTransformation(origin = {-10, 16}, extent = {{-90, 50}, {-110, 70}})));
|
|||
|
|
Modelica.Blocks.Interfaces.RealOutput yawError "Yaw error magnitude [deg]" annotation(
|
|||
|
|
Placement(transformation(origin = {-10, 20}, extent = {{-90, 20}, {-110, 40}}), iconTransformation(origin = {-10, 16}, extent = {{-90, 20}, {-110, 40}})));
|
|||
|
|
Modelica.Blocks.Interfaces.BooleanOutput withinHoverLimit "true if lateralError < hoverTolerance" annotation(
|
|||
|
|
Placement(transformation(origin = {-210, -34}, extent = {{-110, -10}, {-90, 10}}, rotation = 180), iconTransformation(origin = {-110, 16}, extent = {{10, -10}, {-10, 10}})));
|
|||
|
|
Modelica.Blocks.Interfaces.BooleanOutput withinYawLimit "true if yawError < yawTolerance" annotation(
|
|||
|
|
Placement(transformation(origin = {-210, -34}, extent = {{-110, -40}, {-90, -20}}, rotation = 180), iconTransformation(origin = {-110, -14}, extent = {{10, -10}, {-10, 10}})));
|
|||
|
|
Modelica.Blocks.Interfaces.BooleanOutput recovered "true if position remains within recoveryBand for recoveryTime" annotation(
|
|||
|
|
Placement(transformation(origin = {-210, -34}, extent = {{-110, -70}, {-90, -50}}, rotation = 180), iconTransformation(origin = {-110, -44}, extent = {{10, -10}, {-10, 10}})));
|
|||
|
|
Modelica.Blocks.Interfaces.BooleanOutput pass "true if all criteria are satisfied" annotation(
|
|||
|
|
Placement(transformation(origin = {-210, 22},extent = {{110, -100}, {90, -80}}, rotation = -0), iconTransformation(origin = {-110, -74}, extent = {{10, -10}, {-10, 10}})));
|
|||
|
|
|
|||
|
|
Modelica.Blocks.Interfaces.RealOutput envUpperX;
|
|||
|
|
Modelica.Blocks.Interfaces.RealOutput envUpperY;
|
|||
|
|
|
|||
|
|
Modelica.Blocks.Interfaces.RealOutput envLowerX;
|
|||
|
|
Modelica.Blocks.Interfaces.RealOutput envLowerY;
|
|||
|
|
Modelica.Blocks.Continuous.Derivative dRefX annotation(
|
|||
|
|
Placement(transformation(origin = {20, 30}, extent = {{-10, -10}, {10, 10}})));
|
|||
|
|
Modelica.Blocks.Continuous.Derivative dRefY annotation(
|
|||
|
|
Placement(transformation(origin = {18, -10}, extent = {{-10, -10}, {10, 10}})));
|
|||
|
|
protected
|
|||
|
|
Real t_since_exit(start = 0) "Elapsed time since last exit from recovery band";
|
|||
|
|
Boolean insideBand(start = true);
|
|||
|
|
Real vx;
|
|||
|
|
Real vy;
|
|||
|
|
Real norm;
|
|||
|
|
Real nx;
|
|||
|
|
Real ny;
|
|||
|
|
Boolean outsideBand(start=false);
|
|||
|
|
Boolean timeout;
|
|||
|
|
|
|||
|
|
equation
|
|||
|
|
|
|||
|
|
connect(DronePositionConsign[1], dRefX.u);
|
|||
|
|
connect(DronePositionConsign[2], dRefY.u);
|
|||
|
|
|
|||
|
|
vx = dRefX.y;
|
|||
|
|
vy = dRefY.y;
|
|||
|
|
|
|||
|
|
norm = sqrt(vx*vx + vy*vy + eps);
|
|||
|
|
|
|||
|
|
nx = -vy / norm;
|
|||
|
|
ny = vx / norm;
|
|||
|
|
|
|||
|
|
lateralError =
|
|||
|
|
abs((posX - DronePositionConsign[1]) * nx +
|
|||
|
|
(posY - DronePositionConsign[2]) * ny);
|
|||
|
|
|
|||
|
|
yawError = abs(Modelica.Math.wrapAngle(yaw - yawConsign));
|
|||
|
|
|
|||
|
|
withinHoverLimit = noEvent(lateralError < hoverTolerance);
|
|||
|
|
withinYawLimit = noEvent(yawError < yawTolerance);
|
|||
|
|
|
|||
|
|
insideBand = noEvent(lateralError <= recoveryBand);
|
|||
|
|
outsideBand = not insideBand;
|
|||
|
|
|
|||
|
|
der(t_since_exit) =
|
|||
|
|
if noEvent(lateralError > recoveryBand)
|
|||
|
|
then 1
|
|||
|
|
else -t_since_exit;
|
|||
|
|
|
|||
|
|
timeout = noEvent(t_since_exit > recoveryTime);
|
|||
|
|
recovered = not timeout;
|
|||
|
|
|
|||
|
|
pass = withinHoverLimit and withinYawLimit and recovered;
|
|||
|
|
|
|||
|
|
envUpperX = DronePositionConsign[1] + recoveryBand * nx;
|
|||
|
|
envUpperY = DronePositionConsign[2] + recoveryBand * ny;
|
|||
|
|
|
|||
|
|
envLowerX = DronePositionConsign[1] - recoveryBand * nx;
|
|||
|
|
envLowerY = DronePositionConsign[2] - recoveryBand * ny;
|
|||
|
|
|
|||
|
|
annotation(
|
|||
|
|
Documentation(info = "
|
|||
|
|
<html>
|
|||
|
|
<h4>SR-WIND-02 – Performance in Wind</h4>
|
|||
|
|
<p>
|
|||
|
|
Source: EASA Means of Compliance Light-UAS.2512,
|
|||
|
|
ISO 21384-3 (Clause 10 – Operational limitations).
|
|||
|
|
</p>
|
|||
|
|
<p>
|
|||
|
|
<b>Requirement:</b><br>
|
|||
|
|
Under a steady 8 m/s wind with 12 m/s gusts, the UAS shall:
|
|||
|
|
<ul>
|
|||
|
|
<li>Maintain lateral position error ≤ 1.5 m (95th percentile) during hover.</li>
|
|||
|
|
<li>Maintain yaw pointing error ≤ 5° (95th percentile).</li>
|
|||
|
|
<li>Recover to within 0.5 m acceptance enveloppe position error within 3 s after gust disturbance.</li>
|
|||
|
|
</ul>
|
|||
|
|
</p>
|
|||
|
|
<p>
|
|||
|
|
<b>Simulation Inputs:</b> Connect to AIDA_System position, yaw, and
|
|||
|
|
reference signals.
|
|||
|
|
</p>
|
|||
|
|
</html>"),
|
|||
|
|
Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "SR-WIND-02"), Rectangle(extent = {{-104, -2}, {-104, -2}}), Rectangle(origin = {0, -1}, extent = {{-100, 101}, {100, -99}})}, coordinateSystem(initialScale = 0.1)));
|
|||
|
|
end StabilityRequirement;
|