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 display_values():
global ax1
global dev
global pos_d
global rtcm_thread
global satcount_string
global status_string
global svin_string
global log_fd
ax1.clear()
d_home = pos_d[0]
d_p1 = d_home
for d_p2 in pos_d:
if util.gps_distance(d_home[0], d_home[1], d_p2[0], d_p2[1]) < 5:
plot_line(d_p1, d_p2, d_home, 'ro')
d_p1 = d_p2
ax1.text(0.99,
0.62,
"\n".join([
"%.8f, %.8f" % (pos_d[0]), status_string, satcount_string,
svin_string
]),
horizontalalignment='right',
verticalalignment='bottom',
transform=ax1.transAxes)
log_fd.write("%s: %.8f, %.8f, %s %s %s\n" %
(datetime.datetime.utcnow().strftime('%Y%m%d%H%M%S'),
pos_d[0][0], pos_d[0][1], status_string, satcount_string,
svin_string.replace('\n', ', ')))
log_fd.flush()
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)
