def __init__(self, connection, tlog_name="TLog.txt"):
super().__init__(connection, tlog_name)
self.longitudinal_autopilot = LongitudinalAutoPilot()
self.lateral_autopilot = LateralAutoPilot()
#defined as [along_track_distance (meters), altitude (meters)]
self.longitudinal_gates = [
np.array([200.0, 200.0]),
np.array([1100.0, 300.0]),
np.array([1400.0, 280.0]),
np.array([2200.0, 200.0])
]
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.throttle_cmd = 0.0
self.elevator_cmd = 0.0
self.pitch_cmd = 0.0
self.aileron_cmd = 0.0
self.rudder_cmd = 0.0
self.roll_cmd = 0.0
self.sideslip_cmd = 0.0
self.yaw_cmd = 0.0
self.roll_ff = 0.0
self.course_cmd = 0.0
self.line_origin = np.array([0.0, 0.0, 0.0])
self.orbit_center = np.array([0.0, 0.0, 0.0])
self.orbit_cw = True
self.waypoints = [
np.array([0.0, 500.0, -400.0]),
np.array([2600.0, 500.0, -500.0]),
np.array([2600.0, -2500.0, -400.0]),
np.array([100.0, 500.0, -450.0]),
np.array([100.0, -2000.0, -450.0])
]
self.waypoint_tuple = (np.array([0.0, 0.0,
0.0]), np.array([0.0, 0.0, 0.0]),
np.array([0.0, 0.0, 0.0]))
self.scenario = Scenario.SANDBOX
self.time_cmd = 0.0
self.cmd_freq = 100.0
self.last_airspeed_time = None
self.last_position_time = None
self.last_attitude_time = None
# register all your callbacks here
self.register_callback(MsgID.LOCAL_POSITION,
self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.airspeed_callback)
self.register_callback(MsgID.STATE, self.state_callback)
self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
#self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)
#self.register_callback(MsgID.AIRSPEED, self.airspeed_callback)
self._scenario_started = False
class FixedWingProject(Udaciplane):
def __init__(self, connection, tlog_name="TLog.txt"):
super().__init__(connection, tlog_name)
self.longitudinal_autopilot = LongitudinalAutoPilot()
self.lateral_autopilot = LateralAutoPilot()
#defined as [along_track_distance (meters), altitude (meters)]
self.longitudinal_gates = [
np.array([200.0, 200.0]),
np.array([1100.0, 300.0]),
np.array([1400.0, 280.0]),
np.array([2200.0, 200.0])
]
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.throttle_cmd = 0.0
self.elevator_cmd = 0.0
self.pitch_cmd = 0.0
self.aileron_cmd = 0.0
self.rudder_cmd = 0.0
self.roll_cmd = 0.0
self.sideslip_cmd = 0.0
self.yaw_cmd = 0.0
self.roll_ff = 0.0
self.course_cmd = 0.0
self.line_origin = np.array([0.0, 0.0, 0.0])
self.orbit_center = np.array([0.0, 0.0, 0.0])
self.orbit_cw = True
self.waypoints = [
np.array([0.0, 500.0, -400.0]),
np.array([2600.0, 500.0, -500.0]),
np.array([2600.0, -2500.0, -400.0]),
np.array([100.0, 500.0, -450.0]),
np.array([100.0, -2000.0, -450.0])
]
self.waypoint_tuple = (np.array([0.0, 0.0,
0.0]), np.array([0.0, 0.0, 0.0]),
np.array([0.0, 0.0, 0.0]))
self.scenario = Scenario.SANDBOX
self.time_cmd = 0.0
self.cmd_freq = 100.0
self.last_airspeed_time = None
self.last_position_time = None
self.last_attitude_time = None
# register all your callbacks here
self.register_callback(MsgID.LOCAL_POSITION,
self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.airspeed_callback)
self.register_callback(MsgID.STATE, self.state_callback)
self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
#self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)
#self.register_callback(MsgID.AIRSPEED, self.airspeed_callback)
self._scenario_started = False
def state_callback(self):
if (self.scenario == Scenario.SANDBOX):
if (self.status != 0):
self.scenario = Scenario(self.status)
self.init_scenario()
if (self.scenario != Scenario.SANDBOX):
if (self._scenario_started == False):
self.take_control()
self.arm()
self._scenario_started = True
print('Start Scenario...')
elif (self.guided != True):
self.stop()
def airspeed_callback(self):
#Assuming no wind, for now...
self.airspeed = np.linalg.norm(self.local_velocity)
if (self.airspeed != 0.0):
rot_mat = euler2RM(self.attitude[0], self.attitude[1],
self.attitude[2])
side_velocity = rot_mat.transpose()[1,0]*self.local_velocity[0] +\
rot_mat.transpose()[1,1]*self.local_velocity[1] +\
rot_mat.transpose()[1,2]*self.local_velocity[2]
self.sideslip = np.arcsin(side_velocity / self.airspeed)
else:
self.sideslip = 0.0
dt = 0.0
if (self.last_airspeed_time != None):
dt = self.local_velocity_time - self.last_airspeed_time
if (dt <= 0.0):
return
self.last_airspeed_time = self.local_velocity_time
if (self._scenario_started == False):
return
if (self.scenario == Scenario.AIRSPEED):
self.throttle_cmd = self.longitudinal_autopilot.airspeed_loop(
self.airspeed, self.airspeed_cmd, dt)
self.cmd_longitude_mode(self.elevator_cmd, self.throttle_cmd, 0, 0,
self.last_airspeed_time)
if (self.scenario == Scenario.CLIMB):
self.pitch_cmd = self.longitudinal_autopilot.airspeed_pitch_loop(
self.airspeed, self.airspeed_cmd, dt)
if ((self.scenario == Scenario.LONGITUDINAL) |
(self.scenario == Scenario.FIXEDWING)):
altitude = -self.local_position[2]
[self.pitch_cmd, self.throttle_cmd] = \
self.longitudinal_autopilot.longitudinal_loop(
self.airspeed, altitude, self.airspeed_cmd,
self.altitude_cmd, dt)
if ((self.scenario == Scenario.ROLL) | (self.scenario == Scenario.TURN)
| (self.scenario == Scenario.YAW) |
(self.scenario == Scenario.LINE) |
(self.scenario == Scenario.ORBIT) |
(self.scenario == Scenario.LATERAL) |
(self.scenario == Scenario.FIXEDWING)):
self.rudder_cmd = self.lateral_autopilot.sideslip_hold_loop(
self.sideslip, dt)
if (self.scenario == Scenario.FLYINGCAR):
# TODO: Insert Flying Car Scenario code here
pass
def attitude_callback(self):
dt = 0.0
if (self.last_attitude_time != None):
dt = self.attitude_time - self.last_attitude_time
self.last_attitude_time = self.attitude_time
if (self._scenario_started == False):
return
if ((self.scenario == Scenario.ALTITUDE) |
(self.scenario == Scenario.AIRSPEED) |
(self.scenario == Scenario.CLIMB) |
(self.scenario == Scenario.LONGITUDINAL)):
self.elevator_cmd = self.longitudinal_autopilot.pitch_loop(
self.attitude[1], self.gyro_raw[1], self.pitch_cmd)
self.cmd_longitude_mode(self.elevator_cmd, self.throttle_cmd)
if (np.abs(self.gyro_raw[1]) >= 1):
print(self.gyro_raw)
if ((self.scenario == Scenario.YAW) | (self.scenario == Scenario.LINE)
| (self.scenario == Scenario.ORBIT) |
(self.scenario == Scenario.LATERAL) |
(self.scenario == Scenario.FIXEDWING)):
self.roll_cmd = self.lateral_autopilot.yaw_hold_loop(
self.yaw_cmd, self.attitude[2], dt, self.roll_ff)
if ((self.scenario == Scenario.ROLL) | (self.scenario == Scenario.TURN)
| (self.scenario == Scenario.YAW) |
(self.scenario == Scenario.LINE) |
(self.scenario == Scenario.ORBIT) |
(self.scenario == Scenario.LATERAL)):
self.aileron_cmd = self.lateral_autopilot.roll_attitude_hold_loop(
self.roll_cmd, self.attitude[0], self.gyro_raw[0])
self.cmd_lateral_mode(self.aileron_cmd, self.rudder_cmd,
self.altitude_cmd, self.airspeed_cmd)
if (self.scenario == Scenario.FIXEDWING):
self.aileron_cmd = self.lateral_autopilot.roll_attitude_hold_loop(
self.roll_cmd, self.attitude[0], self.gyro_raw[0])
self.elevator_cmd = self.longitudinal_autopilot.pitch_loop(
self.attitude[1], self.gyro_raw[1], self.pitch_cmd)
self.cmd_controls(self.aileron_cmd, self.elevator_cmd,
self.rudder_cmd, self.throttle_cmd)
if (self.scenario == Scenario.FLYINGCAR):
# TODO: Insert Flying Car Scenario code here
pass
def local_position_callback(self):
dt = 0.0
if (self.last_position_time != None):
dt = self.local_position_time - self.last_position_time
self.last_position_time = self.local_position_time
if (dt <= 0.0):
return
if (self._scenario_started == False):
return
if (self.scenario == Scenario.ALTITUDE):
altitude = -self.local_position[2]
self.pitch_cmd = self.longitudinal_autopilot.altitude_loop(
altitude, self.altitude_cmd, dt)
if ((self.scenario == Scenario.LONGITUDINAL)):
along_track = np.linalg.norm(self.local_position[0:2])
if (along_track > self.gate_target[0]):
if (len(self.longitudinal_gates) == 0):
self.stop()
else:
self.gate_target = self.longitudinal_gates.pop(0)
print('Gate Target: ', self.gate_target)
self.altitude_cmd = self.gate_target[1]
if (self.scenario == Scenario.LINE):
self.yaw_cmd = self.lateral_autopilot.straight_line_guidance(
self.line_origin, self.line_course, self.local_position)
self.roll_ff = 0.0
if (self.scenario == Scenario.ORBIT):
self.yaw_cmd = self.lateral_autopilot.orbit_guidance(
self.orbit_center, self.orbit_radius, self.local_position,
self.attitude[2], self.orbit_cw)
self.roll_ff = self.lateral_autopilot.coordinated_turn_ff(
self.airspeed_cmd, self.orbit_radius, self.orbit_cw)
if (self.scenario == Scenario.LATERAL):
(self.roll_ff, self.yaw_cmd) = self.lateral_autopilot.path_manager(
self.local_position, self.attitude[2], self.airspeed_cmd)
if (self.scenario == Scenario.FIXEDWING):
(self.roll_ff, self.yaw_cmd,
switch) = self.lateral_autopilot.waypoint_follower(
self.waypoint_tuple, self.local_position[0:2],
self.attitude[2], self.airspeed_cmd)
if (switch):
if (len(self.waypoints) == 0):
next_waypoint = self.waypoint_tuple[2]
else:
next_waypoint = self.waypoints.pop(0)
print('Adding waypoint: ', next_waypoint)
self.waypoint_tuple = (self.waypoint_tuple[1],
self.waypoint_tuple[2], next_waypoint)
self.altitude_cmd = -self.waypoint_tuple[0][2]
if (self.scenario == Scenario.FLYINGCAR):
# TODO: Insert Flying Car Scenario code here
pass
def init_scenario(self):
if (self.scenario == Scenario.SANDBOX):
pass
elif (self.scenario == Scenario.ALTITUDE):
print('Starting Altitude Hold Scenario')
self.throttle_cmd = 0.66
self.altitude_cmd = 450.0
elif (self.scenario == Scenario.AIRSPEED):
print('Starting Airspeed Hold Scenario')
self.elevator_cmd = 0.0
self.airspeed_cmd = 41.0
elif (self.scenario == Scenario.CLIMB):
print('Starting Climb Scenario')
self.airspeed_cmd = 41.0
self.throttle_cmd = 1.0
elif (self.scenario == Scenario.LONGITUDINAL):
print('Starting Longitudinal Challenge')
self.airspeed_cmd = 41.0
self.gate_target = self.longitudinal_gates.pop(0)
self.altitude_cmd = self.gate_target[1]
elif (self.scenario == Scenario.ROLL):
print('Starting Stabilize Roll Scenario')
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.roll_cmd = 0.0
self.rudder_cmd = 0.0
elif (self.scenario == Scenario.TURN):
print('Starting Coordinated Turn Scenario')
self.airpseed_cmd = 41.0
self.altitude_cmd = 450.0
self.roll_cmd = 45.0 * np.pi / 180.0
self.sideslip_cmd = 0.0
elif (self.scenario == Scenario.YAW):
print('Starting Yaw Hold Scenario')
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.yaw_cmd = 0.0
self.sideslip_cmd = 0.0
self.roll_ff = 0.0
elif (self.scenario == Scenario.LINE):
print('Starting Line Following Scenario')
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.line_course = 0.0
self.line_origin = np.array([0.0, 20.0, 450.0])
elif (self.scenario == Scenario.ORBIT):
print('Starting Orbit Following Scenario')
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.orbit_radius = 500.0
self.orbit_center = np.array([0.0, 500.0, -450.0])
self.orbit_cw = True
elif (self.scenario == Scenario.LATERAL):
print('Starting Lateral Challenge')
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
elif (self.scenario == Scenario.FIXEDWING):
print('Starting Fixed Wing Challenge')
self.airspeed_cmd = 41.0
prev_waypoint = self.waypoints.pop(0)
curr_waypoint = self.waypoints.pop(0)
next_waypoint = self.waypoints.pop(0)
self.waypoint_tuple = (prev_waypoint, curr_waypoint, next_waypoint)
self.altitude_cmd = -self.waypoint_tuple[0][2]
elif (self.scenario == Scenario.FLYINGCAR):
# TODO: Insert Flying Car Scenario code here
pass
else:
print('Invalid Scenario')
return
def run_scenario(self, scenario):
self.scenario = scenario
self.init_scenario()
self.start()
class FixedWingProject(Udaciplane):
def __init__(self, connection, tlog_name="TLog.txt"):
super().__init__(connection, tlog_name)
self.longitudinal_autopilot = LongitudinalAutoPilot()
self.lateral_autopilot = LateralAutoPilot()
self.flying_car_planner = FlyingCarPlanner()
# Defined as [along_track_distance (meters), altitude (meters)]
self.longitudinal_gates = [
np.array([200.0, 200.0]),
np.array([1100.0, 300.0]),
np.array([1400.0, 280.0]),
np.array([2200.0, 200.0])
]
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.throttle_cmd = 0.0
self.elevator_cmd = 0.0
self.pitch_cmd = 0.0
self.aileron_cmd = 0.0
self.rudder_cmd = 0.0
self.roll_cmd = 0.0
self.sideslip_cmd = 0.0
self.yaw_cmd = 0.0
self.roll_ff = 0.0
self.course_cmd = 0.0
self.line_origin = np.array([0.0, 0.0, 0.0])
self.orbit_center = np.array([0.0, 0.0, 0.0])
self.orbit_cw = True
self.waypoints = [
np.array([0.0, 500.0, -400.0]),
np.array([2600.0, 500.0, -500.0]),
np.array([2600.0, -2500.0, -400.0]),
np.array([100.0, 500.0, -450.0]),
np.array([100.0, -2000.0, -450.0])
]
self.waypoint_tuple = (np.array([0.0, 0.0,
0.0]), np.array([0.0, 0.0, 0.0]),
np.array([0.0, 0.0, 0.0]))
self.waypoints_flying_car = []
self.scenario = Scenario.SANDBOX
self.time_cmd = 0.0
self.cmd_freq = 100.0
self.last_airspeed_time = None
self.last_position_time = None
self.last_attitude_time = None
# Register all your callbacks here
self.register_callback(MsgID.LOCAL_POSITION,
self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.airspeed_callback)
self.register_callback(MsgID.STATE, self.state_callback)
self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
# self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)
# self.register_callback(MsgID.AIRSPEED, self.airspeed_callback)
self._scenario_started = False
def state_callback(self):
if self.scenario == Scenario.SANDBOX:
if self.status != 0:
self.scenario = Scenario(self.status)
self.init_scenario()
if self.scenario != Scenario.SANDBOX:
if not self._scenario_started:
self.take_control()
self.arm()
self._scenario_started = True
print('Start Scenario...')
elif not self.guided:
self.stop()
def airspeed_callback(self):
# Assuming no wind, for now...
self.airspeed = np.linalg.norm(self.local_velocity)
if self.airspeed != 0.0:
rot_mat = euler2RM(self.attitude[0], self.attitude[1],
self.attitude[2])
side_velocity = rot_mat.transpose()[1, 0] * self.local_velocity[0] +\
rot_mat.transpose()[1, 1] * self.local_velocity[1] + \
rot_mat.transpose()[1, 2] * self.local_velocity[2]
self.sideslip = np.arcsin(side_velocity / self.airspeed)
else:
self.sideslip = 0.0
dt = 0.0
if self.last_airspeed_time is not None:
dt = self.local_velocity_time - self.last_airspeed_time
if dt <= 0.0:
return
self.last_airspeed_time = self.local_velocity_time
if not self._scenario_started:
return
if self.scenario == Scenario.AIRSPEED:
self.throttle_cmd = self.longitudinal_autopilot.airspeed_loop(
self.airspeed, self.airspeed_cmd, dt)
self.cmd_longitude_mode(self.elevator_cmd, self.throttle_cmd, 0, 0,
self.last_airspeed_time)
if self.scenario == Scenario.CLIMB:
self.pitch_cmd = self.longitudinal_autopilot.airspeed_pitch_loop(
self.airspeed, self.airspeed_cmd, dt)
if (self.scenario == Scenario.LONGITUDINAL) | (self.scenario
== Scenario.FIXEDWING):
altitude = -self.local_position[2]
[self.pitch_cmd, self.throttle_cmd] = \
self.longitudinal_autopilot.longitudinal_loop(
self.airspeed, altitude, self.airspeed_cmd,
self.altitude_cmd, dt)
if ((self.scenario == Scenario.ROLL) | (self.scenario == Scenario.TURN)
| (self.scenario == Scenario.YAW) |
(self.scenario == Scenario.LINE) |
(self.scenario == Scenario.ORBIT) |
(self.scenario == Scenario.LATERAL) |
(self.scenario == Scenario.FIXEDWING)):
self.rudder_cmd = self.lateral_autopilot.sideslip_hold_loop(
self.sideslip, dt)
if self.scenario == Scenario.FLYINGCAR:
# TODO: Insert Flying Car Scenario code here
# Control only during flight mode
if len(self.waypoints_flying_car) != 0 \
and self.waypoints_flying_car[0]['cmd'] == FlyingCarPlanner.WaypointCommand.FLIGHT:
self.rudder_cmd = self.lateral_autopilot.sideslip_hold_loop(
self.sideslip, dt)
altitude = -self.local_position[2]
(self.pitch_cmd, self.throttle_cmd) = \
self.longitudinal_autopilot.longitudinal_loop(
self.airspeed, altitude, self.airspeed_cmd,
self.altitude_cmd, dt)
pass
def attitude_callback(self):
dt = 0.0
if self.last_attitude_time is not None:
dt = self.attitude_time - self.last_attitude_time
self.last_attitude_time = self.attitude_time
if not self._scenario_started:
return
if ((self.scenario == Scenario.ALTITUDE) |
(self.scenario == Scenario.AIRSPEED) |
(self.scenario == Scenario.CLIMB) |
(self.scenario == Scenario.LONGITUDINAL)):
self.elevator_cmd = self.longitudinal_autopilot.pitch_loop(
self.attitude[1], self.gyro_raw[1], self.pitch_cmd)
self.cmd_longitude_mode(self.elevator_cmd, self.throttle_cmd)
if np.abs(self.gyro_raw[1]) >= 1:
print(self.gyro_raw)
if ((self.scenario == Scenario.YAW) | (self.scenario == Scenario.LINE)
| (self.scenario == Scenario.ORBIT) |
(self.scenario == Scenario.LATERAL) |
(self.scenario == Scenario.FIXEDWING)):
self.roll_cmd = self.lateral_autopilot.yaw_hold_loop(
self.yaw_cmd, self.attitude[2], dt, self.roll_ff)
if ((self.scenario == Scenario.ROLL) | (self.scenario == Scenario.TURN)
| (self.scenario == Scenario.YAW) |
(self.scenario == Scenario.LINE) |
(self.scenario == Scenario.ORBIT) |
(self.scenario == Scenario.LATERAL)):
self.aileron_cmd = self.lateral_autopilot.roll_attitude_hold_loop(
self.roll_cmd, self.attitude[0], self.gyro_raw[0])
self.cmd_lateral_mode(self.aileron_cmd, self.rudder_cmd,
self.altitude_cmd, self.airspeed_cmd)
if self.scenario == Scenario.FIXEDWING:
self.aileron_cmd = self.lateral_autopilot.roll_attitude_hold_loop(
self.roll_cmd, self.attitude[0], self.gyro_raw[0])
self.elevator_cmd = self.longitudinal_autopilot.pitch_loop(
self.attitude[1], self.gyro_raw[1], self.pitch_cmd)
self.cmd_controls(self.aileron_cmd, self.elevator_cmd,
self.rudder_cmd, self.throttle_cmd)
if self.scenario == Scenario.FLYINGCAR:
# TODO: Insert Flying Car Scenario code here
# Control only during flight mode
if len(self.waypoints_flying_car) != 0 \
and self.waypoints_flying_car[0]['cmd'] == FlyingCarPlanner.WaypointCommand.FLIGHT:
self.aileron_cmd = self.lateral_autopilot.roll_attitude_hold_loop(
self.roll_cmd, self.attitude[0], self.gyro_raw[0])
self.elevator_cmd = self.longitudinal_autopilot.pitch_loop(
self.attitude[1], self.gyro_raw[1], self.pitch_cmd)
self.cmd_controls(self.aileron_cmd, self.elevator_cmd,
self.rudder_cmd, self.throttle_cmd)
def local_position_callback(self):
dt = 0.0
if self.last_position_time is not None:
dt = self.local_position_time - self.last_position_time
self.last_position_time = self.local_position_time
if dt <= 0.0:
return
if not self._scenario_started:
return
if self.scenario == Scenario.ALTITUDE:
altitude = -self.local_position[2]
self.pitch_cmd = self.longitudinal_autopilot.altitude_loop(
altitude, self.altitude_cmd, dt)
if self.scenario == Scenario.LONGITUDINAL:
along_track = np.linalg.norm(self.local_position[0:2])
if along_track > self.gate_target[0]:
if len(self.longitudinal_gates) == 0:
self.stop()
else:
self.gate_target = self.longitudinal_gates.pop(0)
print('Gate Target: ', self.gate_target)
self.altitude_cmd = self.gate_target[1]
if self.scenario == Scenario.LINE:
self.yaw_cmd = self.lateral_autopilot.straight_line_guidance(
self.line_origin, self.line_course, self.local_position)
self.roll_ff = 0.0
if self.scenario == Scenario.ORBIT:
self.yaw_cmd = self.lateral_autopilot.orbit_guidance(
self.orbit_center, self.orbit_radius, self.local_position,
self.attitude[2], self.orbit_cw)
self.roll_ff = self.lateral_autopilot.coordinated_turn_ff(
self.airspeed_cmd, self.orbit_radius, self.orbit_cw)
if self.scenario == Scenario.LATERAL:
(self.roll_ff, self.yaw_cmd) = self.lateral_autopilot.path_manager(
self.local_position, self.attitude[2], self.airspeed_cmd)
if self.scenario == Scenario.FIXEDWING:
(self.roll_ff, self.yaw_cmd,
switch) = self.lateral_autopilot.waypoint_follower(
self.waypoint_tuple, self.local_position[0:2],
self.attitude[2], self.airspeed_cmd)
if switch:
if len(self.waypoints) == 0:
next_waypoint = self.waypoint_tuple[2]
else:
next_waypoint = self.waypoints.pop(0)
print('Adding waypoint: ', next_waypoint)
self.waypoint_tuple = (self.waypoint_tuple[1],
self.waypoint_tuple[2], next_waypoint)
self.altitude_cmd = -self.waypoint_tuple[0][2]
if self.scenario == Scenario.FLYINGCAR:
# TODO: Insert Flying Car Scenario code here
if len(self.waypoints_flying_car) == 0:
return
(flying_car_cmd,
switch) = self.flying_car_planner.waypoint_follower(
target_waypoint=self.waypoints_flying_car[0],
local_position=self.local_position,
yaw=self.attitude[2],
airspeed=self.airspeed_cmd)
if switch:
completed_waypoint = self.waypoints_flying_car.pop(0)
print('Completed waypoint: ', completed_waypoint)
if len(self.waypoints_flying_car) != 0:
print('Next waypoint: ', self.waypoints_flying_car[0])
else:
current_cmd = self.waypoints_flying_car[0]['cmd']
if current_cmd == FlyingCarPlanner.WaypointCommand.TAKEOFF:
print('takeoff', flying_car_cmd)
self.takeoff(flying_car_cmd)
elif current_cmd == FlyingCarPlanner.WaypointCommand.VTOL:
# Use absolute position control
print('cmd_vtol_position', flying_car_cmd)
self.cmd_vtol_position(north=flying_car_cmd[0],
east=flying_car_cmd[1],
altitude=flying_car_cmd[2],
heading=flying_car_cmd[3])
elif current_cmd == FlyingCarPlanner.WaypointCommand.FLIGHT:
# Use straight line guidance
print('straight_line_guidance', flying_car_cmd)
self.yaw_cmd = self.lateral_autopilot.straight_line_guidance(
line_origin=flying_car_cmd[0],
line_course=flying_car_cmd[1],
local_position=self.local_position)
self.roll_ff = 0
elif current_cmd == FlyingCarPlanner.WaypointCommand.LANDING:
print('land')
self.land()
pass
def init_scenario(self):
if self.scenario == Scenario.SANDBOX:
pass
elif self.scenario == Scenario.ALTITUDE:
print('Starting Altitude Hold Scenario')
self.throttle_cmd = 0.66
self.altitude_cmd = 450.0
elif self.scenario == Scenario.AIRSPEED:
print('Starting Airspeed Hold Scenario')
self.elevator_cmd = 0.0
self.airspeed_cmd = 41.0
elif self.scenario == Scenario.CLIMB:
print('Starting Climb Scenario')
self.airspeed_cmd = 41.0
self.throttle_cmd = 1.0
elif self.scenario == Scenario.LONGITUDINAL:
print('Starting Longitudinal Challenge')
self.airspeed_cmd = 41.0
self.gate_target = self.longitudinal_gates.pop(0)
self.altitude_cmd = self.gate_target[1]
elif self.scenario == Scenario.ROLL:
print('Starting Stabilize Roll Scenario')
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.roll_cmd = 0.0
self.rudder_cmd = 0.0
elif self.scenario == Scenario.TURN:
print('Starting Coordinated Turn Scenario')
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.roll_cmd = 45.0 * np.pi / 180.0
self.sideslip_cmd = 0.0
elif self.scenario == Scenario.YAW:
print('Starting Yaw Hold Scenario')
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.yaw_cmd = 0.0
self.sideslip_cmd = 0.0
self.roll_ff = 0.0
elif self.scenario == Scenario.LINE:
print('Starting Line Following Scenario')
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.line_course = 0.0
self.line_origin = np.array([0.0, 20.0, 450.0])
elif self.scenario == Scenario.ORBIT:
print('Starting Orbit Following Scenario')
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
self.orbit_radius = 500.0
self.orbit_center = np.array([0.0, 500.0, -450.0])
self.orbit_cw = True
elif self.scenario == Scenario.LATERAL:
print('Starting Lateral Challenge')
self.airspeed_cmd = 41.0
self.altitude_cmd = 450.0
elif self.scenario == Scenario.FIXEDWING:
print('Starting Fixed Wing Challenge')
self.airspeed_cmd = 41.0
prev_waypoint = self.waypoints.pop(0)
curr_waypoint = self.waypoints.pop(0)
next_waypoint = self.waypoints.pop(0)
self.waypoint_tuple = (prev_waypoint, curr_waypoint, next_waypoint)
self.altitude_cmd = -self.waypoint_tuple[0][2]
elif self.scenario == Scenario.FLYINGCAR:
# TODO: Insert Flying Car Scenario code here
print('Starting Flying Car Challenge')
self.waypoints_flying_car = self.flying_car_planner.build_waypoints(
start_location=self.local_position,
start_yaw=0,
end_location=self.local_position +
np.array([1545, -1816, -80]),
end_yaw=np.pi)
print('Initial waypoints:', self.waypoints_flying_car)
else:
print('Invalid Scenario')
return
def run_scenario(self, scenario):
self.scenario = scenario
self.init_scenario()
self.start()