def recv_fdm(self):
'''receive FDM packet'''
try:
select.select([self.sim.fileno()], [], [], 1)
buf = self.sim.recv(1024)
except Exception as ex:
return False
(timestamp,
latitude, longitude,
altitude,
heading,
speedN, speedE, speedD,
xAccel, yAccel, zAccel,
rollRate, pitchRate, yawRate,
roll, pitch, yaw,
airspeed) = struct.unpack('<dddddddddddddddddd', buf)
self.accel_body = Vector3(xAccel, yAccel, zAccel)
self.gyro = Vector3(rollRate, pitchRate, yawRate)
self.velocity = Vector3(speedN, speedE, speedD)
dist = util.gps_distance(0, 0, latitude, longitude)
bearing = util.gps_bearing(0, 0, latitude, longitude)
self.position.x = dist * cos(radians(bearing))
self.position.y = dist * sin(radians(bearing))
self.position.z = -altitude
self.dcm.from_euler(roll, pitch, yaw)
self.time_now = timestamp + self.time_base
# auto-adjust to crrcsim frame rate
deltat = timestamp - self.last_timestamp
if deltat < 0.01 and deltat > 0:
self.adjust_frame_time(1.0/deltat)
self.last_timestamp = timestamp
def recv_fdm(self):
'''receive FDM packet'''
try:
select.select([self.sim.fileno()], [], [], 1)
buf = self.sim.recv(1024)
except Exception as ex:
return False
(timestamp, latitude, longitude, altitude, heading, speedN, speedE,
speedD, xAccel, yAccel, zAccel, rollRate, pitchRate, yawRate, roll,
pitch, yaw, airspeed) = struct.unpack('<dddddddddddddddddd', buf)
self.accel_body = Vector3(xAccel, yAccel, zAccel)
self.gyro = Vector3(rollRate, pitchRate, yawRate)
self.velocity = Vector3(speedN, speedE, speedD)
dist = util.gps_distance(0, 0, latitude, longitude)
bearing = util.gps_bearing(0, 0, latitude, longitude)
self.position.x = dist * cos(radians(bearing))
self.position.y = dist * sin(radians(bearing))
self.position.z = -altitude
self.dcm.from_euler(roll, pitch, yaw)
self.time_now = timestamp + self.time_base
# auto-adjust to crrcsim frame rate
deltat = timestamp - self.last_timestamp
if deltat < 0.01 and deltat > 0:
self.adjust_frame_time(1.0 / deltat)
self.last_timestamp = timestamp
def get_xy(pos, home):
distance = util.gps_distance(home[0], home[1], pos[0], pos[1])
bearing = util.gps_bearing(home[0], home[1], pos[0], pos[1])
x = distance * math.sin(math.radians(bearing))
y = distance * math.cos(math.radians(bearing))
return (x, y)
def get_xy(pos, home):
distance = util.gps_distance(home[0], home[1], pos[0], pos[1])
bearing = util.gps_bearing(home[0], home[1], pos[0], pos[1])
x = distance * math.sin(math.radians(bearing))
y = distance * math.cos(math.radians(bearing))
return (x,y)
