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