public void writeEcefNed(ref BinaryWriter bw, Ecef ecef, Attitude attitude)
{
writeEcef(ref bw, ecef);
bw.Write(attitude.Yaw);
bw.Write(attitude.Pitch);
bw.Write(attitude.Roll);
}
private bool RecvVehicleInfo(ref VehicleInfo simStats)
{
if ((MessageId)m_receiveData[0] == MessageId.VehicleInfo)
{
MemoryStream memStream = new MemoryStream(m_receiveData);
BinaryReader binReader = new BinaryReader(memStream);
binReader.ReadByte();
simStats.ElapsedTime = binReader.ReadUInt64();
Ecef position = new Ecef(
binReader.ReadDouble(),
binReader.ReadDouble(),
binReader.ReadDouble());
simStats.Position = position;
Attitude attitude = new Attitude(
binReader.ReadDouble(),
binReader.ReadDouble(),
binReader.ReadDouble());
simStats.Attitude = attitude;
simStats.Speed = binReader.ReadDouble();
simStats.Heading = binReader.ReadDouble();
simStats.Odometer = binReader.ReadDouble();
return(true);
}
return(false);
}
public void PushEcefNed(double elapsedTime, Ecef position, Attitude attitude, Ecef velocity, Attitude angularVelocity, Ecef acceleration, Attitude angularAcceleration, Ecef jerk, Attitude angularJerk, string name = "")
{
if (!IsConnected)
{
throw new Exception("Cannot push ecef because you are not connected.");
}
MemoryStream memStream = new MemoryStream();
BinaryWriter bw = new BinaryWriter(memStream, Encoding.ASCII);
bw.Write((byte)MessageId.PushEcefNedDynamics);
bw.Write((byte)DynamicType.Jerk);
bw.Write(elapsedTime);
writeEcefNed(ref bw, position, attitude);
writeEcefNed(ref bw, velocity, angularVelocity);
writeEcefNed(ref bw, acceleration, angularAcceleration);
writeEcefNed(ref bw, jerk, angularJerk);
bw.Write(name.Length);
bw.Write(Encoding.ASCII.GetBytes(name));
bw.Seek(0, SeekOrigin.Begin);
BinaryReader br = new BinaryReader(memStream);
byte[] data = br.ReadBytes((int)memStream.Length);
m_udpClient.Send(data, data.Length);
}
public void PushTrackEcefNed(int elapsedTime, Ecef position, Attitude attitude)
{
if (!m_beginTrack)
{
throw new Exception("You must call beginTrackDefinition first.");
}
PostCommand(new PushTrackEcefNed(elapsedTime, position.X, position.Y, position.Z,
attitude.Yaw, attitude.Pitch, attitude.Roll));
}
public bool PushEcefNed(double elapsedTime, Ecef position, Attitude attitude, Ecef velocity, Attitude angularVelocity, Ecef acceleration, Attitude angularAcceleration, Ecef jerk, Attitude angularJerk, string name = "")
{
if (m_hil == null)
{
throw new Exception("Cannot send position to simulator because you are not connected.");
}
m_hil.PushEcefNed(elapsedTime, position, attitude, velocity, angularVelocity, acceleration, angularAcceleration, jerk, angularJerk, name);
return(HilCheck(elapsedTime));
}
public bool PushEcefNed(double elapsedTime, Ecef position, Attitude attitude, string name = "")
{
if (m_hil == null)
{
throw new Exception("Cannot send position to simulator because you are not connected.");
}
m_hil.PushEcefNed(elapsedTime, position, attitude, name);
return(HilCheck(elapsedTime));
}
public bool PushLlaNed(double elapsedTime, Lla lla, Attitude attitude, string name = "")
{
return(PushEcefNed(elapsedTime, lla.ToEcef(), attitude, name));
}
public void PushTrackLlaNed(int elapsedTime, Lla lla, Attitude attitude)
{
PushTrackEcefNed(elapsedTime, lla.ToEcef(), attitude);
}
