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;
|