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;
}
public PoseAbsReceivedEventArgs(PoseAbsData d)
{
this.d = d;
}
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));
}
}
