private static object ParseV2AbsPacket(BinaryReader br)
{
PoseAbsData d = new PoseAbsData();
int ts_secs = br.ReadInt32();
int ts_ticks = br.ReadInt32();
d.timestamp = new CarTimestamp(ts_secs, ts_ticks);
int flags = br.ReadInt32();
d.correction_mode = (PoseCorrectionMode)(flags & 0x00000F00);
d.gps_pos_valid = (flags & 0x00000010) != 0;
d.sep_att_valid = (flags & 0x00000020) != 0;
d.hp_pos_valid = (flags & 0x00000040) != 0;
d.yaw = br.ReadDouble();
d.pitch = br.ReadDouble();
d.roll = br.ReadDouble();
d.ecef_px = br.ReadDouble();
d.ecef_py = br.ReadDouble();
d.ecef_pz = br.ReadDouble();
d.veh_vx = br.ReadDouble();
d.veh_vy = br.ReadDouble();
d.veh_vz = br.ReadDouble();
d.ecef_vx = br.ReadDouble();
d.ecef_vy = br.ReadDouble();
d.ecef_vz = br.ReadDouble();
d.bax = br.ReadDouble();
d.bay = br.ReadDouble();
d.baz = br.ReadDouble();
d.bwx = br.ReadDouble();
d.bwy = br.ReadDouble();
d.bwz = br.ReadDouble();
// skip the next 27 doubles, pose covariance
for (int i = 0; i < 27; i++)
{
br.ReadDouble();
}
d.gps_px = br.ReadDouble();
d.gps_py = br.ReadDouble();
d.gps_pz = br.ReadDouble();
d.hp_px = br.ReadDouble();
d.hp_py = br.ReadDouble();
d.hp_pz = br.ReadDouble();
d.sep_heading = br.ReadDouble();
d.sep_pitch = br.ReadDouble();
d.sep_roll = br.ReadDouble();
return(d);
}
void client_PoseAbsReceived(object sender, PoseAbsReceivedEventArgs e)
{
packetCount++;
Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);
//OperationalTrace.WriteVerbose("got absolute pose for time {0}", e.PoseAbsData.timestamp);
if (lastPacketTime.ts > e.PoseAbsData.timestamp.ts)
{
lastPacketTime = new CarTimestamp(-10000);
}
if (e.PoseAbsData.timestamp.ts - lastPacketTime.ts >= 0.02)
{
DatasetSource ds = Services.Dataset;
PoseAbsData d = e.PoseAbsData;
CarTimestamp now = e.PoseAbsData.timestamp;
if (Services.Projection != null)
{
Coordinates xy = Services.Projection.ECEFtoXY(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
ds.ItemAs <Coordinates>("xy").Add(xy, now);
AbsolutePose absPose = new AbsolutePose(xy, d.yaw, now);
Services.AbsolutePose.PushAbsolutePose(absPose);
}
if (!Settings.UseWheelSpeed)
{
ds.ItemAs <double>("speed").Add(d.veh_vx, now);
}
ds.ItemAs <double>("vel - y").Add(d.veh_vy, now);
ds.ItemAs <double>("heading").Add(BiasEstimator.CorrectHeading(d.yaw), now);
ds.ItemAs <double>("pitch").Add(d.pitch + 1.25 * Math.PI / 180.0, now);
ds.ItemAs <double>("roll").Add(d.roll, now);
ds.ItemAs <double>("ba - x").Add(d.bax, now);
ds.ItemAs <double>("ba - y").Add(d.bay, now);
ds.ItemAs <double>("ba - z").Add(d.baz, now);
ds.ItemAs <double>("bw - x").Add(d.bwx, now);
ds.ItemAs <double>("bw - y").Add(d.bwy, now);
ds.ItemAs <double>("bw - z").Add(d.bwz, now);
ds.ItemAs <PoseCorrectionMode>("correction mode").Add(d.correction_mode, now);
LLACoord lla = WGS84.ECEFtoLLA(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
ds.ItemAs <double>("altitude").Add(lla.alt, now);
if (Services.Projection != null)
{
ds.ItemAs <Coordinates>("gps xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.gps_px, d.gps_py, d.gps_pz)), now);
ds.ItemAs <Coordinates>("hp xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.hp_px, d.hp_py, d.hp_pz)), now);
}
ds.ItemAs <double>("sep heading").Add(d.sep_heading, now);
ds.ItemAs <double>("sep pitch").Add(d.sep_pitch, now);
ds.ItemAs <double>("sep roll").Add(d.sep_roll, now);
if (!double.IsNaN(lastYawRate) && lastPacketTime.ts > 0)
{
// get the enu velocity
Vector3 pECEF = new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz);
Vector3 vECEF = new Vector3(d.ecef_vx, d.ecef_vy, d.ecef_vz);
Matrix3 Recef2enu = Geocentric.Recef2enu(pECEF);
Vector3 vENU = Recef2enu * vECEF;
BiasEstimator.Update(lastYawRate, d.yaw, vENU, now.ts - lastPacketTime.ts, now);
Services.Dataset.ItemAs <double>("heading bias").Add(BiasEstimator.HeadingBias, now);
}
lastPacketTime = now;
}
}
private void ParseV1AbsPacket(BinaryReader br)
{
PoseAbsData d = new PoseAbsData();
int ts_secs = br.ReadInt32();
int ts_ticks = br.ReadInt32();
d.timestamp = new CarTimestamp(ts_secs, ts_ticks);
d.yaw = br.ReadDouble();
d.pitch = br.ReadDouble();
d.roll = br.ReadDouble();
d.ecef_px = br.ReadDouble();
d.ecef_py = br.ReadDouble();
d.ecef_pz = br.ReadDouble();
d.veh_vx = br.ReadDouble();
d.veh_vy = br.ReadDouble();
d.veh_vz = br.ReadDouble();
d.ecef_vx = br.ReadDouble();
d.ecef_vy = br.ReadDouble();
d.ecef_vz = br.ReadDouble();
d.bax = br.ReadDouble();
d.bay = br.ReadDouble();
d.baz = br.ReadDouble();
d.bwx = br.ReadDouble();
d.bwy = br.ReadDouble();
d.bwz = br.ReadDouble();
d.gps_px = br.ReadDouble();
d.gps_py = br.ReadDouble();
d.gps_pz = br.ReadDouble();
// skip the next 27 doubles, pose covariance
for (int i = 0; i < 27; i++)
{
br.ReadDouble();
}
// check if there is still stuff at the end
if (br.BaseStream.Length != br.BaseStream.Position)
{
d.hp_px = br.ReadDouble();
d.hp_py = br.ReadDouble();
d.hp_pz = br.ReadDouble();
}
if (br.BaseStream.Length != br.BaseStream.Position)
{
d.sep_heading = br.ReadDouble();
d.sep_pitch = br.ReadDouble();
d.sep_roll = br.ReadDouble();
}
// assume these flags are OK
d.gps_pos_valid = true;
d.hp_pos_valid = true;
d.sep_att_valid = true;
// assume HP correction mode
d.correction_mode = PoseCorrectionMode.HP;
if (PoseAbsReceived != null)
{
PoseAbsReceived(this, new PoseAbsReceivedEventArgs(d));
}
}
public PoseAbsReceivedEventArgs(PoseAbsData d)
{
this.d = d;
}
