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