Avionics/package.mo
Download original .moGitHub source
Documentation: Avionics · Avionics.ImuSample · Avionics.MocapSample · Avionics.GpsSample · Avionics.MagnetometerSample · Avionics.BarometerSample · Avionics.OpticalFlowSample · Avionics.NavigationEstimate · Avionics.EstimatorStatus · Avionics.ImuSampleInput · Avionics.ImuSampleOutput · Avionics.MocapSampleInput · Avionics.MocapSampleOutput · Avionics.GpsSampleInput · Avionics.GpsSampleOutput · Avionics.MagnetometerSampleInput · Avionics.MagnetometerSampleOutput · Avionics.BarometerSampleInput · Avionics.BarometerSampleOutput · Avionics.OpticalFlowSampleInput · Avionics.OpticalFlowSampleOutput · Avionics.NavigationEstimateInput · Avionics.NavigationEstimateOutput · Avionics.EstimatorStatusOutput · Avionics.PartialNavigationEstimator
within;
package Avionics
"Pure-Modelica sensor and navigation contracts"
record ImuSample
"Calibrated inertial packet in body Forward-Left-Up axes"
Boolean valid "True when the sample can be used";
Boolean fresh "True for one estimator tick per new sample";
Real timestamp_s(unit = "s")
"End timestamp of the integration interval";
Real angularVelocityBodyFlu_rad_s[3](each unit = "rad/s");
Real specificForceBodyFlu_m_s2[3](each unit = "m/s2")
"Accelerometer specific force; stationary level value is +g on body z";
Real deltaAngleBodyFlu_rad[3](each unit = "rad")
"Coning-corrected angle increment over integrationTime_s";
Real deltaVelocityBodyFlu_m_s[3](each unit = "m/s")
"Sculling-corrected specific-force increment, expressed in the body frame at the start of integration";
Real deltaPositionBodyFlu_m[3](each unit = "m")
"Closed-form position increment expressed in the body frame at the start of integration";
Real deltaQuaternionBodyFlu[4](each unit = "1")
"Scalar-first relative rotation from the start body frame to the end body frame";
Real integrationTime_s(unit = "s")
"Accumulation interval for delta angle and delta velocity";
Real gyroscopeBiasLinearizationBodyFlu_rad_s[3](each unit = "rad/s")
"Gyroscope-bias anchor used to form this preintegral";
Real accelerometerBiasLinearizationBodyFlu_m_s2[3](each unit = "m/s2")
"Accelerometer-bias anchor used to form this preintegral";
Real deltaRotationGyroscopeBiasJacobian_s[3, 3](each unit = "s")
"Right-local rotation-increment sensitivity to gyroscope bias";
Real deltaVelocityGyroscopeBiasJacobian_m[3, 3](each unit = "m")
"Velocity-increment sensitivity to gyroscope bias";
Real deltaVelocityAccelerometerBiasJacobian_s[3, 3](each unit = "s")
"Velocity-increment sensitivity to accelerometer bias";
Real deltaPositionGyroscopeBiasJacobian_m_s[3, 3](each unit = "m.s")
"Position-increment sensitivity to gyroscope bias";
Real deltaPositionAccelerometerBiasJacobian_s2[3, 3](each unit = "s2")
"Position-increment sensitivity to accelerometer bias";
end ImuSample;
record MocapSample
"Motion-capture pose in the local East-North-Up world frame"
Boolean valid;
Boolean fresh;
Real timestamp_s(unit = "s");
Real positionWorldEnu_m[3](each unit = "m");
Real quaternionWorldBody[4](each unit = "1")
"Scalar-first Hamilton quaternion {w,x,y,z}, body FLU to world ENU";
Real positionCovarianceWorld_m2[3, 3](each unit = "m2");
Real attitudeCovarianceBody_rad2[3, 3](each unit = "rad2")
"Small-angle covariance expressed in body FLU";
end MocapSample;
record GpsSample
"GPS solution mapped into the estimator local frame"
Boolean valid;
Boolean fresh;
Boolean positionValid;
Boolean velocityValid;
Real timestamp_s(unit = "s");
Real geodetic_deg_m[3]
"Optional source coordinates {latitude_deg,longitude_deg,altitude_m}";
Real positionWorldEnu_m[3](each unit = "m");
Real velocityWorldEnu_m_s[3](each unit = "m/s");
Real positionCovarianceWorld_m2[3, 3](each unit = "m2");
Real velocityCovarianceWorld_m2_s2[3, 3](each unit = "m2/s2");
end GpsSample;
record MagnetometerSample
"Calibrated three-axis magnetic field in body FLU axes"
Boolean valid;
Boolean fresh;
Real timestamp_s(unit = "s");
Real magneticFieldBodyFlu_T[3](each unit = "T");
Real covarianceBody_T2[3, 3](each unit = "T2");
end MagnetometerSample;
record BarometerSample
"Pressure-derived altitude measurement in the local ENU frame"
Boolean valid;
Boolean fresh;
Real timestamp_s(unit = "s");
Real altitudeWorldEnu_m(unit = "m")
"Calibrated pressure altitude relative to the local-frame origin";
Real variance_m2(unit = "m2");
end BarometerSample;
record OpticalFlowSample
"Raw integrated optical flow with co-timed gyro and downward range"
Boolean valid;
Boolean fresh;
Real timestamp_s(unit = "s");
Real integratedLineOfSight_rad[2](each unit = "rad")
"Right-handed scene rotations about camera/body FLU x and y over integrationTime_s. For a nadir camera, forward motion produces +y and left motion produces -x after rotation compensation";
Real integratedLineOfSightCovariance_rad2[2, 2](each unit = "rad2");
Real integratedGyroscopeBodyFlu_rad[3](each unit = "rad")
"Actual FLU body-angle integral over the same exposure interval; add x/y to line-of-sight flow to remove camera rotation";
Real integratedGyroscopeCovariance_rad2[3, 3](each unit = "rad2");
Real integrationTime_s(unit = "s");
Real groundDistance_m(unit = "m");
Real groundDistanceVariance_m2(unit = "m2")
"Variance of the range along the camera boresight";
Real quality(min = 0.0, max = 1.0)
"Normalized driver quality indicator";
end OpticalFlowSample;
record NavigationEstimate
"Estimator-independent state consumed by guidance and control"
Boolean valid;
Real timestamp_s(unit = "s");
Real positionWorldEnu_m[3](each unit = "m");
Real velocityWorldEnu_m_s[3](each unit = "m/s");
Real accelerationWorldEnu_m_s2[3](each unit = "m/s2");
Real quaternionWorldBody[4](each unit = "1")
"Scalar-first Hamilton quaternion {w,x,y,z}, body FLU to world ENU";
Real rotationWorldBody[3, 3](each unit = "1")
"Direction-cosine matrix mapping body FLU vectors into world ENU";
Real eulerRpy_rad[3](each unit = "rad")
"Intrinsic 3-2-1 attitude ordered {roll,pitch,yaw}";
Real angularVelocityBodyFlu_rad_s[3](each unit = "rad/s");
Real angularVelocityWorldEnu_rad_s[3](each unit = "rad/s");
end NavigationEstimate;
record EstimatorStatus
"Algorithm-neutral estimator health and correction status"
Boolean initialized;
Boolean predictionAccepted;
Boolean mocapCorrectionAccepted;
Boolean gpsPositionCorrectionAccepted;
Boolean gpsVelocityCorrectionAccepted;
Boolean magnetometerCorrectionAccepted;
Boolean barometerCorrectionAccepted;
Boolean terrainCorrectionAccepted
"A fresh downward-range sample updated the terrain-altitude filter";
Boolean opticalFlowCorrectionAccepted;
Integer consecutiveRejectedCorrections
"Consecutive attempted aiding corrections rejected by conditioning
or innovation gating; zero after any accepted correction";
Real rejectionElapsed_s(unit = "s")
"Wall-clock time the anchor source has held without moving the
state. Advanced on ESTIMATOR TICKS, not on aiding attempts and not
on the sensors` valid duty cycle, so it means the same elapsed time
whatever rate the block is wired at and whatever waveform the
drivers put on `valid`. Cleared when the anchor accepts, when the
anchor changes, or when there is no anchor at all";
Integer mocapConsecutiveRejections
"Consecutive rejections of the mocap source alone";
Integer gpsConsecutiveRejections
"Consecutive rejections of the GPS source alone";
Integer opticalFlowConsecutiveRejections
"Consecutive rejections of the optical-flow source alone. The
per-source counters exist because the aggregate above cannot show
a failed sensor while a healthy one keeps resetting it: a source
rejected on every one of its own samples is visible here from the
first sample, whatever the other sources are doing";
Integer correctionOutcome
"Outcome of this tick's attempted aiding correction: 0 none
attempted, 1 accepted, 2 rejected as not finite, 3 rejected by the
innovation gate, 4 rejected because the innovation covariance did
not factor, 5 rejected because the sensor-reported measurement
covariance was not finite or claimed non-positive noise, 7 the state
was re-seeded from a fresh anchor sample by the automatic recovery
ladder. Every outcome is named rather than inferred from the absence
of other flags";
Integer acceptedCorrectionCount
"Monotonic count of SHIFTED FUSION INSTANTS since the last reset: the
number of estimator ticks on which at least one aiding correction was
accepted. AT MOST ONE PER ESTIMATOR TICK, and that is a contract on
this field rather than a property of any particular filter.
It counts instants and not measurements deliberately. The consumer this
field exists for is an output predictor, which must recompose its
buffered window once per tick on which the state it composes onto
MOVED; two measurements fused into the same tick move it once, and
counting them twice would ask for a second recomposition that has
nothing to recompose.
A filter fusing several sources in one tick coalesces their acceptance
flags here and must not increment per measurement: at a delayed fusion horizon
every ripe measurement is an accepted correction, and the aiding set of
a small multirotor offers well over a hundred a second against a
recomposition budget of single digits.
correctionOutcome is a LEVEL: it stands for the whole estimator
tick, which at a 100 Hz filter behind an 800 Hz consumer is
eight consumer ticks. A consumer that must act once per
correction cannot edge-detect that level -- back-to-back
accepted corrections hold it true across the boundary and the
second one is invisible -- so the boundary carries the count and
the consumer compares it with the one it last saw. That is the
only well-defined edge across a rate change, which is why it is
here and not reconstructed downstream";
Integer correctionSource
"Aiding source the outcome above refers to: 0 none, 1 mocap, 2 GPS,
3 optical flow, 4 magnetometer, 5 barometer. With multiple corrections,
ESKF reports the first attempted source in priority order mocap, GPS,
optical flow, barometer, magnetometer. Per-source acceptance flags
describe the other corrections independently";
Real normalizedInnovationSquared
"NIS of this tick's attempted aiding correction; zero if none was attempted";
Integer recoveryStage
"Automatic recovery ladder: 0 nominal, 1 covariance ramping toward
the mission envelope with the state untouched, 2 anchor still
divergent at the envelope.
THIS IS THE DEGRADED-MODE SIGNAL. It is the field a consumer reads
to decide that the navigation solution is no longer trustworthy
enough for position-holding flight, and it is deliberately separate
from NavigationEstimate.valid, which says only that the published
numbers are numbers. The separation is not cosmetic: on the
deployed stack the rate loop takes body rates from the estimator,
so clearing valid clears RatesValid, zeroes the motors and latches
the fault in every mode including ACRO. A degraded navigation
solution must be able to demote the flight mode without stopping
the vehicle flying, so stage 2 raises this field and leaves valid
alone.
An optional third stage, off by default and enabled per deployment,
re-seeds position and velocity from a fresh anchor sample once the
divergence has outlasted a longer window; it is reported on the
separate reseeded flag and reseedCount rather than as a distinct
recoveryStage value, so a filter with the stage disabled reports only
stages 0 to 2 and a commanded reset stays the only other re-seed";
Boolean imuPayloadHeld
"True when this tick`s published IMU-derived outputs -- angular
velocity, world acceleration and the estimate timestamp -- were held
from an earlier sample because the current inertial reading was not
finite.
Those three fields are computed from the raw sample rather than from
the filter state, so they bypass the state finiteness guard entirely;
holding them is what stops a NaN reaching a consumer. This flag, not
NavigationEstimate.valid, is the signal that it happened: dropping
valid clears RatesValid and latches a motors-zero fault in every
mode, so using it to report a transient inertial glitch would turn a
recoverable sample dropout into a crash";
Integer anchorSource
"Source currently anchoring the solution, by the same Source* codes
as correctionSource. The recovery ladder is timed on this source
alone, and a change of anchor restarts that timing";
Boolean reseeded
"True on the tick the automatic recovery ladder re-seeded the state
from a fresh anchor sample. It is an OPTIONAL, off-by-default stage,
so on a filter that leaves it disabled this is always false and the
commanded reset input is the only path that re-seeds. When it fires,
correctionOutcome carries the CorrectionReseeded code rather than an
acceptance code";
Integer reseedCount
"Monotonic count of automatic re-seeds since the last reset. Like
acceptedCorrectionCount it gives a consumer a well-defined edge
across a rate change, since reseeded is a per-tick level";
Integer alignmentSource
"Estimation.StrapdownINS.Alignment* code of the initial attitude
alignment in force: which sample leveled the filter and whether a
heading was available. Recorded so a flight log shows what the
attitude started from";
Real alignmentSpecificForceBodyFlu_m_s2[3](each unit = "m/s2")
"The low-pass specific force the quasi-static test reads and the
initial alignment levels on, zero until the first usable IMU sample";
Real quietElapsed_s(unit = "s")
"Time the low-pass IMU has reported a quasi-static vehicle: specific
force within tolerance of gravity and angular rate below the limit.
A tick that fails the test costs one tick and sustained motion drains
it to zero. Gates the initial alignment and the zero-velocity update";
Boolean pseudoPositionCorrectionAccepted
"True on a tick the synthetic hold-position measurement moved the
state";
Boolean zeroVelocityCorrectionAccepted
"True on a tick the synthetic zero-velocity measurement moved the
state";
end EstimatorStatus;
connector ImuSampleInput = input Avionics.ImuSample;
connector ImuSampleOutput = output Avionics.ImuSample;
connector MocapSampleInput = input Avionics.MocapSample;
connector MocapSampleOutput = output Avionics.MocapSample;
connector GpsSampleInput = input Avionics.GpsSample;
connector GpsSampleOutput = output Avionics.GpsSample;
connector MagnetometerSampleInput = input Avionics.MagnetometerSample;
connector MagnetometerSampleOutput = output Avionics.MagnetometerSample;
connector BarometerSampleInput = input Avionics.BarometerSample;
connector BarometerSampleOutput = output Avionics.BarometerSample;
connector OpticalFlowSampleInput = input Avionics.OpticalFlowSample;
connector OpticalFlowSampleOutput = output Avionics.OpticalFlowSample;
connector NavigationEstimateInput = input Avionics.NavigationEstimate;
connector NavigationEstimateOutput = output Avionics.NavigationEstimate;
connector EstimatorStatusOutput = output Avionics.EstimatorStatus;
partial block PartialNavigationEstimator
"Stable boundary implemented by interchangeable navigation algorithms"
parameter Real samplePeriod(unit = "s", min = 1.0e-9) = 0.001;
input Boolean reset;
Avionics.ImuSampleInput imu;
Avionics.MocapSampleInput mocap;
Avionics.GpsSampleInput gps;
Avionics.MagnetometerSampleInput magnetometer;
Avionics.BarometerSampleInput barometer;
Avionics.OpticalFlowSampleInput opticalFlow;
discrete Avionics.NavigationEstimateOutput estimate(
valid(start = false, fixed = true),
timestamp_s(start = 0.0, fixed = true),
positionWorldEnu_m(each start = 0.0, each fixed = true),
velocityWorldEnu_m_s(each start = 0.0, each fixed = true),
accelerationWorldEnu_m_s2(each start = 0.0, each fixed = true),
quaternionWorldBody(
start = {1.0, 0.0, 0.0, 0.0}, each fixed = true),
rotationWorldBody(
start = [1.0, 0.0, 0.0;
0.0, 1.0, 0.0;
0.0, 0.0, 1.0], each fixed = true),
eulerRpy_rad(each start = 0.0, each fixed = true),
angularVelocityBodyFlu_rad_s(each start = 0.0, each fixed = true),
angularVelocityWorldEnu_rad_s(each start = 0.0, each fixed = true));
discrete Avionics.EstimatorStatusOutput status(
initialized(start = false, fixed = true),
predictionAccepted(start = false, fixed = true),
mocapCorrectionAccepted(start = false, fixed = true),
gpsPositionCorrectionAccepted(start = false, fixed = true),
gpsVelocityCorrectionAccepted(start = false, fixed = true),
magnetometerCorrectionAccepted(start = false, fixed = true),
barometerCorrectionAccepted(start = false, fixed = true),
terrainCorrectionAccepted(start = false, fixed = true),
opticalFlowCorrectionAccepted(start = false, fixed = true),
consecutiveRejectedCorrections(start = 0, fixed = true),
rejectionElapsed_s(start = 0.0, fixed = true),
mocapConsecutiveRejections(start = 0, fixed = true),
gpsConsecutiveRejections(start = 0, fixed = true),
opticalFlowConsecutiveRejections(start = 0, fixed = true),
correctionOutcome(start = 0, fixed = true),
acceptedCorrectionCount(start = 0, fixed = true),
correctionSource(start = 0, fixed = true),
recoveryStage(start = 0, fixed = true),
anchorSource(start = 0, fixed = true),
imuPayloadHeld(start = false, fixed = true));
end PartialNavigationEstimator;
annotation(Documentation(info = "<html>
<p>These records are the dependency boundary between transport drivers,
estimators, and controllers. They intentionally contain no FlatBuffers or
Synapse types. A driver maps a transport message into these physical,
frame-explicit records; every estimator implements
<code>PartialNavigationEstimator</code>; every controller consumes
<code>NavigationEstimate</code>.</p>
<p>The canonical world/body convention is ENU/FLU. Redundant attitude
representations are deliberate at this boundary: the estimator guarantees
their consistency so consumers never depend on its internal state
representation. Filter covariance, bias states, and tangent ordering remain
private because they have no unambiguous meaning across algorithms.</p>
<h4>Timestamp and freshness contract</h4>
<p>An IMU timestamp identifies the end of its integration interval. Aiding
packets carry capture time in the estimator clock, not transport arrival time.
The integration duration describes the interval, and sensor latency describes
when its packet becomes available; these are separate quantities.</p>
<p><code>valid</code> says that a packet is usable. <code>fresh</code> indicates a
new arrival for one estimator tick. Holding a measurement's value is acceptable;
repeating its freshness would count correlated data as independent corrections.
An adapter must preserve frame, units, covariance and timestamp when converting
from an external transport.</p>
<h4>Consumers</h4>
<p>Guidance and control consume <code>NavigationEstimate</code> in one continuous
local world frame. Health decisions also read <code>EstimatorStatus</code>:
finite output numbers alone do not establish healthy position aiding. A delayed
fusion wrapper publishes its filtered horizon state and its propagated current
output separately; select the appropriate epoch for each consumer.</p>
</html>"));
end Avionics;