within AIDAModelica;

model ComputationAccelerationModel "Computation Acceleration Model"
  // CP: 65001
  // SimulationX Version: 3.8.2.45319 x64
  Modelica.Blocks.Interfaces.RealInput TotalThrust(quantity = "Mechanics.Rotation.Torque", displayUnit = "Nm") "'input Real' as connector" annotation(
    Placement(transformation(extent = {{-75, 15}, {-35, 55}}), iconTransformation(extent = {{-95, 5}, {-55, 45}})));
  Modelica.Blocks.Interfaces.RealInput Attitude[3](quantity = "Mechanics.Rotation.Angle", displayUnit = "rad") "'input Real' as connector" annotation(
    Placement(transformation(extent = {{-75, 55}, {-35, 95}}), iconTransformation(origin = {-25, 75}, extent = {{-20, -20}, {20, 20}})));
  Modelica.Blocks.Interfaces.RealOutput DroneAcceleration[3](quantity = "Mechanics.Translation.Accel", displayUnit = "m/s²") "'output Real' as connector" annotation(
    Placement(transformation(extent = {{65, -15}, {85, 5}}), iconTransformation(extent = {{61.7, -10}, {81.7, 10}})));
  Modelica.Blocks.Interfaces.RealInput ExternalForce[3] "'input Real' as connector" annotation(
    Placement(transformation(extent = {{-60, -40}, {-20, 0}}), iconTransformation(extent = {{-95, -45}, {-55, -5}})));
  parameter Real m(quantity = "Basics.Mass", displayUnit = "kg") = 1.8 "Quadcopter mass";
  parameter Real g(quantity = "Mechanics.Translation.Accel", displayUnit = "m/s²") = 9.8 "Parameter g";
  parameter Real e3[3] = {0, 0, 1} "Unit vector";
  Real Reb[3, 3] "From the ABCF to the EFCF rotation matrix";
  Real Rz[3, 3] "Z rotation matrix";
  Real Ry[3, 3] "Y rotation matrix";
  Real Rx[3, 3] "X rotation matrix";
equation
// enter your equations here
  Rz = {{cos(Attitude[3]), sin(Attitude[3]), 0}, {-sin(Attitude[3]), cos(Attitude[3]), 0}, {0, 0, 1}};
  Ry = {{cos(Attitude[2]), 0, -sin(Attitude[2])}, {0, 1, 0}, {sin(Attitude[2]), 0, cos(Attitude[2])}};
  Rx = {{1, 0, 0}, {0, cos(Attitude[1]), sin(Attitude[1])}, {0, -sin(Attitude[1]), cos(Attitude[1])}};
  Reb = transpose(Rz) * transpose(Ry) * transpose(Rx);
  DroneAcceleration = g * e3 - TotalThrust / m * Reb * e3 + ExternalForce / m;
  annotation(
    TotalThrust(flags = 2),
    Attitude(flags = 2),
    DroneAcceleration(flags = 2),
    ExternalForce(flags = 2),
    Icon(coordinateSystem(extent = {{-75, -75}, {75, 75}}, initialScale = 0.1), graphics = {Rectangle(fillColor = {255, 255, 255}, fillPattern = FillPattern.Solid, extent = {{-76.7, 76.7}, {80, -76.7}}), Text(origin = {-6, 4}, extent = {{-12, 6}, {12, -6}}, textString = "%name")}),
    experiment(StopTime = 1, StartTime = 0, Interval = 0.002, MaxInterval = "0.001"));
end ComputationAccelerationModel;