model

Vehicles.Cubs2.AvionicsPlant

CUBS2 PWM and odometry boundary around the physical plant

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default	Description
input	`pwm_us`	`Real`	`[9](start = { 1500.0, 1500.0, 1000.0, 1500.0, 1000.0, 0.0, 0.0, 0.0, 0.0})`
output	`time_s`	`Real`
output	`position_m`	`Real`	`[3]`
output	`velocity_m_s`	`Real`	`[3]`
output	`airspeed_m_s`	`Real`
output	`euler_rad`	`Real`	`[3]`	{roll, pitch, yaw}
output	`actuatorCommand`	`Real`	`[4]`	{aileron, elevator, rudder, throttle}
output	`stickCommand`	`Real`	`[4]`	{roll, pitch, yaw, throttle}
output	`armCommand`	`Real`		continuous normalized arm command [0, 1]
output	`autopilotDebug`	`Vehicles.Cubs2.Interfaces.AutopilotDebug`
output	`odometry`	`Vehicles.Cubs2.Interfaces.Odometry`

Modelica declarations and implementation

Show parameters, interfaces and source

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

model AvionicsPlant "CUBS2 PWM and odometry boundary around the physical plant"
  // Keep transitive CMM base packages visible to Rumoca source-root loading.
  import LieGroups;
  import RigidBody;

  Plant vehicle(
    q_start = {0.38268343236508984, 0.0, 0.0, -0.9238795325112867});
  OnboardStabilizerSurrogate onboardStabilizer;

  input Real pwm_us[9](start = {
    1500.0,
    1500.0,
    1000.0,
    1500.0,
    1000.0,
    0.0,
    0.0,
    0.0,
    0.0});

  output Real time_s;
  output Real position_m[3];
  output Real velocity_m_s[3];
  output Real airspeed_m_s;
  output Real euler_rad[3] "{roll, pitch, yaw}";
  output Real actuatorCommand[4] "{aileron, elevator, rudder, throttle}";
  output Real stickCommand[4] "{roll, pitch, yaw, throttle}";
  output Real armCommand "continuous normalized arm command [0, 1]";
  output Vehicles.Cubs2.Interfaces.AutopilotDebug autopilotDebug;
  output Vehicles.Cubs2.Interfaces.Odometry odometry;

protected
  Real eulerB321_rad[3] "{yaw, pitch, roll}";

equation
  stickCommand = {
    -Vehicles.Interfaces.centeredPwmToUnit(pwm_us[1]),
    -Vehicles.Interfaces.centeredPwmToUnit(pwm_us[2]),
    Vehicles.Interfaces.centeredPwmToUnit(pwm_us[4]),
    Vehicles.Interfaces.throttlePwmToUnit(pwm_us[3])};
  armCommand = MathUtilities.clip((pwm_us[3] - 1000.0) / 50.0, 0.0, 1.0);
  onboardStabilizer.armed = armCommand;
  onboardStabilizer.pilotCommand = stickCommand;
  onboardStabilizer.gyro = vehicle.gyro;
  onboardStabilizer.up_body = vehicle.up_body;
  onboardStabilizer.airspeed = vehicle.airspeed;

  actuatorCommand = onboardStabilizer.surfaceCommand;
  {vehicle.ail, vehicle.elev, vehicle.rud, vehicle.thr} = actuatorCommand;

  time_s = time;
  position_m = vehicle.position;
  velocity_m_s = vehicle.velocity;
  airspeed_m_s = vehicle.airspeed;
  eulerB321_rad = LieGroups.SO3.EulerB321.from_Quat(vehicle.quat);
  euler_rad = {eulerB321_rad[3], eulerB321_rad[2], eulerB321_rad[1]};

  autopilotDebug.currentWaypoint = pwm_us[6];
  autopilotDebug.desiredSpeed_m_s = pwm_us[7] / 1000.0;
  autopilotDebug.rollCommand_rad = pwm_us[8] / 1000.0;
  autopilotDebug.courseError_rad = pwm_us[9] / 1000.0;

  odometry.timestamp_ns = 1000000000.0 * time;
  odometry.position_m = vehicle.position;
  odometry.quaternion = vehicle.quat;
  odometry.velocity_m_s = vehicle.velocity;
  odometry.angularVelocity_rad_s = vehicle.gyro;
  odometry.flags = 15.0;
  odometry.status = 1.0;
  odometry.estimatorType = 1.0;
  odometry.resetCounter = 0.0;
  odometry.qualityPct = 100.0;
  odometry.timeStatus = 0.0;
end AvionicsPlant;