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 PushTrackEcef(int elapsedTime, Ecef position)
{
if (!m_beginTrack)
{
throw new Exception("You must call beginTrackDefinition first.");
}
PostCommand(new PushTrackEcef(elapsedTime, position.X, position.Y, position.Z));
}
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 Ecef(Lla lla)
: this()
{
Ecef ecef = lla.ToEcef();
X = ecef.X;
Y = ecef.Y;
Z = ecef.Z;
}
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 PushEcef(double elapsedTime, Ecef position, Ecef velocity, Ecef acceleration, string name = "")
{
if (m_hil == null)
{
throw new Exception("Cannot send position to simulator because you are not connected.");
}
m_hil.PushEcef(elapsedTime, position, velocity, acceleration, name);
return(HilCheck(elapsedTime));
}
public Tuple<Ecef, Ecef> GeneratePositionAndVelocityAt(double elapsedTime)
{
double time = elapsedTime / 1000.0;
double posOnCircle = time * this.SPEED / RADIUS;
double e = Math.Cos(posOnCircle) * RADIUS;
double n = Math.Sin(posOnCircle) * RADIUS;
Lla llaPos = ORIGIN.AddEnu(new Enu(e, n, 0));
Ecef position = llaPos.ToEcef();
return new Tuple<Ecef, Ecef>(position, ComputeVelocity(posOnCircle));
}
public void PushRouteEcef(double speed, Ecef position)
{
if (!m_beginRoute)
{
throw new Exception("You must call beginRouteDefinition first.");
}
if (speed <= 0)
{
throw new Exception("A route node must have a speed limit greater than zero.");
}
PostCommand(new PushRouteEcef(speed, position.X, position.Y, position.Z));
}
public Ecef ToEcef()
{
double cos_lat = Math.Cos(Lat);
double tmp = (1 - GPS.EFLAT) * (1 - GPS.EFLAT);
double ex2 = (2 - GPS.EFLAT) * GPS.EFLAT / tmp;
double c = GPS.ESMAJ * Math.Sqrt(1 + ex2);
double n = c / Math.Sqrt(1 + ex2 * cos_lat * cos_lat);
Ecef ecef = new Ecef();
ecef.X = (n + Alt) * cos_lat * Math.Cos(Lon);
ecef.Y = (n + Alt) * cos_lat * Math.Sin(Lon);
ecef.Z = (tmp * n + Alt) * Math.Sin(Lat);
return(ecef);
}
public Ecef ToEcef(Lla origin)
{
Ecef originEcef = origin.ToEcef();
double sinLon = Math.Sin(origin.Lon);
double cosLon = Math.Cos(origin.Lon);
double sinLat = Math.Sin(origin.Lat);
double cosLat = Math.Cos(origin.Lat);
return(new Ecef(
-sinLon * East - sinLat * cosLon * North + cosLat * cosLon * Up + originEcef.X,
cosLon * East - sinLat * sinLon * North + cosLat * sinLon * Up + originEcef.Y,
cosLat * North + sinLat * Up + originEcef.Z
));
}
public void PushEcef(double elapsedTime, Ecef position, string name = "")
{
if (!IsConnected)
{
throw new Exception("Cannot push ecef because you are not connected.");
}
MemoryStream memStream = new MemoryStream();
BinaryWriter bw = new BinaryWriter(memStream);
bw.Write((byte)MessageId.PushEcef);
bw.Write(elapsedTime);
writeEcef(ref bw, position);
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 writeEcef(ref BinaryWriter bw, Ecef ecef)
{
bw.Write(ecef.X);
bw.Write(ecef.Y);
bw.Write(ecef.Z);
}
