def target2body(self):
pos_interc = self.target.position(self.t_interc)
# hdg = self.target.heading_from(self.sim.system.full_state.position.x_earth, self.sim.system.full_state.position.y_earth, self.t_interc)
pos_interc[2] *= -1 # z_earth points down
vel = self.target.velocity._vel_NED
displace = pos_interc - self.sim.system.full_state.position.earth_coordinates
target_vel = hor2body(
vel, *self.sim.system.full_state.attitude.euler_angles)
target_pos = hor2body(
displace, *self.sim.system.full_state.attitude.euler_angles)
return target_pos, target_vel
def find_angles(self):
"""
get target world vector
convert to body frame
"""
target_pos = self.target.position
cur_pos = np.array([
self.sim.system.full_state.position.x_earth,
self.sim.system.full_state.position.y_earth,
-self.sim.system.full_state.position.z_earth
])
cur_vel = np.array([
self.sim.system.full_state.velocity.v_north,
self.sim.system.full_state.velocity.v_east,
-self.sim.system.full_state.velocity.v_down
])
cur_world_dir = cur_vel / np.linalg.norm(cur_vel)
target_world = target_pos - cur_pos
print('target world:', target_world)
target_world_dir = target_world / np.linalg.norm(target_world)
lead_point = self.lead_distance * target_world_dir
print('lead pt: ', lead_point)
target_body = hor2body(lead_point,
self.sim.system.full_state.attitude.theta,
self.sim.system.full_state.attitude.phi,
self.sim.system.full_state.attitude.psi)
#
# self.target_body_dir = target_body / np.linalg.norm(target_body)
# heading = np.atan2()
# heading_error =
# lead_world = (self.lead_distance * target_world_dir)
#
# cur_vel = np.array([self.sim.system.full_state.velocity.v_north,
# self.sim.system.full_state.velocity.v_east,
# self.sim.system.full_state.velocity.v_down])
# cur_dir = cur_vel / np.linalg.norm(cur_vel)
# target_body = hor2body(target_world,
# self.sim.system.full_state.attitude.theta,
# self.sim.system.full_state.attitude.phi,
# self.sim.system.full_state.attitude.psi)
# print('body target: ', body_frame_target)
self.target_body_dir = target_body / np.linalg.norm(target_body)
dx, dy, dz = self.target_body_dir
alt_error = np.arctan2(dz, dx)
heading_error = np.arctan2(dy, dx)
print('dir --', self.target_body_dir)
def update(self, state):
self._versor = hor2body(self._z_horizon,
theta=state.attitude.theta,
phi=state.attitude.phi,
psi=state.attitude.psi
)
self._vector = self.magnitude * self.versor
def update(self, state):
r_squared = (state.position.coord_geocentric @
state.position.coord_geocentric)
self._magnitude = STD_GRAVITATIONAL_PARAMETER / r_squared
self._versor = hor2body(self._z_horizon,
theta=state.attittude.theta,
phi=state.attittude.phi,
psi=state.attitude.psi
)
self._vector = self.magnitude * self._vector
def read_sensor(self):
cur_pos = self.target.position(self.sim.system.time)
x, y, z = hor2body(cur_pos,
*self.sim.system.full_state.attitude._euler_angles)
offset = np.linalg.norm(np.array([y, z]))
roll_angle = np.arctan2(-z, y)
# climb_angle = np.arctan2(offset, x)
climb_angle = np.arctan2(z, x)
print('roll: ', roll_angle)
print('climb: ', climb_angle)
def update(self, coords, attitude):
self._accel_NED[:] = coords
self._accel_body = hor2body(coords, attitude.theta, attitude.phi,
attitude.psi)
def update(self, value, attitude):
self._vel_NED[:] = value
self._vel_body = hor2body(value,
attitude.theta,
attitude.phi,
attitude.psi) # m/s