def __init__(self, Ts, time_horizon):
# message sent to path follower
self.Ts = Ts
self.path = msg_path()
self.optimize = optimizer(Ts, time_horizon)
self.waypoints = msg_waypoints()
self.N = time_horizon
def _follow_straight_line(self, path=msg_path(), state=msg_state()):
q = np.copy(path.line_direction)
chi_q = atan2(q[1], q[0])
chi_q = self._wrap(chi_q, state.chi)
Rp_i = np.array([[cos(chi_q), sin(chi_q), 0],
[-sin(chi_q), cos(chi_q), 0], [0, 0, 1]])
r = path.line_origin
p = np.array([state.pn, state.pe, -state.h])
# chi_c: Course direction of path
ei_p = p - r
ep = Rp_i @ ei_p
chi_d = self.chi_inf * (2 / np.pi) * atan(self.k_path * ep[1])
chi_c = chi_q - chi_d
# h_c: Altitude
product = np.cross(q, np.array([0, 0, 1]))
n = product / np.linalg.norm(product)
s = ei_p - (ei_p @ n) * n
hc = -r[2] - np.sqrt(s[0]**2 + s[1]**2) / np.sqrt(q[0]**2 +
q[1]**2) * q[2]
self.autopilot_commands.airspeed_command = path.airspeed
self.autopilot_commands.course_command = chi_c
self.autopilot_commands.altitude_command = hc
self.autopilot_commands.phi_feedforward = 0
def __init__(self):
# message sent to path follower
self.path = msg_path()
# pointers to previous, current, and next waypoints
self.ptr_previous = 0
self.ptr_current = 1
self.ptr_next = 2
# flag that request new waypoints from path planner
self.flag_need_new_waypoints = True
self.num_waypoints = 0
self.halfspace_n = np.inf * np.ones((3,1))
self.halfspace_r = np.inf * np.ones((3,1))
# state of the manager state machine
self.manager_state = 1
# dubins path parameters
self.dubins_path = dubins_parameters()
def _follow_orbit(self, path=msg_path(), state=msg_state()):
p = np.array([state.pn, state.pe, -state.h]) # NED position
d = p - path.orbit_center # radial distance from orbit center
rho = path.orbit_radius
if path.orbit_direction == 'CW':
lmbda = 1
else:
lmbda = -1
var_phi = atan2(d[1], d[0])
var_phi = self._wrap(var_phi, state.chi)
chi_0 = var_phi + lmbda * (np.pi / 2)
chi_c = chi_0 + lmbda * atan(self.k_orbit *
(np.linalg.norm(d) - rho) / rho)
Vg = state.Vg
chi = state.chi
psi = state.psi
phi_feedfw = atan(Vg**2 / (self.gravity * rho * cos(chi - psi)))
self.autopilot_commands.airspeed_command = path.airspeed
self.autopilot_commands.course_command = chi_c
self.autopilot_commands.altitude_command = -path.orbit_center[2]
self.autopilot_commands.phi_feedforward = phi_feedfw
from chap2.video_writer import video_writer
video = video_writer(video_name="chap10_video.avi",
bounding_box=(0, 0, 1000, 1000),
output_rate=SIM.ts_video)
# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)
ctrl = autopilot(SIM.ts_simulation)
obsv = observer(SIM.ts_simulation)
path_follow = path_follower()
# path definition
from message_types.msg_path import msg_path
path = msg_path()
path.flag = 'line'
# path.flag = 'orbit'
if path.flag == 'line':
path.line_origin = np.array([[0.0, 0.0, -100.0]]).T
path.line_direction = np.array([[0.5, 1.0, 0.0]]).T
path.line_direction = path.line_direction / np.linalg.norm(
path.line_direction)
else: # path.flag == 'orbit'
path.orbit_center = np.array([[0.0, 0.0, -100.0]]).T # center of the orbit
path.orbit_radius = 300.0 # radius of the orbit
path.orbit_direction = 'CW' # orbit direction: 'CW'==clockwise, 'CCW'==counter clockwise
# initialize the simulation time
sim_time = SIM.start_time
