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)