function

Estimation.FusionHorizon.unpackGps

Expand one GPS queue row into connector fields

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`row`	`Real`	`[GpsMeasurementLength]`
output	`timestamp_s`	`Real`
output	`positionValid`	`Boolean`
output	`velocityValid`	`Boolean`
output	`geodetic_deg_m`	`Real`	`[3]`
output	`positionWorldEnu_m`	`Real`	`[3]`
output	`velocityWorldEnu_m_s`	`Real`	`[3]`
output	`positionCovarianceWorld_m2`	`Real`	`[3, 3]`
output	`velocityCovarianceWorld_m2_s2`	`Real`	`[3, 3]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function unpackGps "Expand one GPS queue row into connector fields"
  input Real row[GpsMeasurementLength];
  output Real timestamp_s;
  output Boolean positionValid;
  output Boolean velocityValid;
  output Real geodetic_deg_m[3];
  output Real positionWorldEnu_m[3];
  output Real velocityWorldEnu_m_s[3];
  output Real positionCovarianceWorld_m2[3, 3];
  output Real velocityCovarianceWorld_m2_s2[3, 3];
algorithm
  timestamp_s := row[1];
  // Recovered by comparison rather than by equality with one, because the row
  // came back out of a masked multiply-add walk: the stored value is exactly
  // one or exactly zero in exact arithmetic and a half-way test says the same
  // thing without depending on that.
  positionValid := row[2] > 0.5;
  velocityValid := row[3] > 0.5;
  geodetic_deg_m := row[4:6];
  positionWorldEnu_m := row[7:9];
  velocityWorldEnu_m_s := row[10:12];
  positionCovarianceWorld_m2 :=
    [row[13], row[14], row[15];
     row[16], row[17], row[18];
     row[19], row[20], row[21]];
  velocityCovarianceWorld_m2_s2 :=
    [row[22], row[23], row[24];
     row[25], row[26], row[27];
     row[28], row[29], row[30]];
end unpackGps;