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)); } }