class ControlsFlyer(UnityDrone):
def __init__(self, connection):
super().__init__(connection)
self.controller = NonlinearController(
z_k_p=18, # kpPosZ
z_k_d=6.8, # kpVelZ
x_k_p=2.6, # kpPosXY
x_k_d=1.7, # kpVelXY
y_k_p=2.6, # kpPosXY
y_k_d=1.7, # kpVelXY
k_p_roll=5.4, # kpBank
k_p_pitch=5.4, # kpBank
k_p_yaw=1, # kpYaw
k_p_p=12, # kpPQR[0]
k_p_q=12, # kpPQR[1]
k_p_r=4.5 # kpPQR[2]
)
self.target_position = np.array([0.0, 0.0, 0.0])
self.all_waypoints = []
self.in_mission = True
self.check_state = {}
# initial state
self.flight_state = States.MANUAL
# register all your callbacks here
self.register_callback(MsgID.LOCAL_POSITION,
self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_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)
def position_controller(self):
(self.local_position_target, self.local_velocity_target,
yaw_cmd) = self.controller.trajectory_control(
self.position_trajectory, self.yaw_trajectory,
self.time_trajectory, time.time())
self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
acceleration_cmd = self.controller.lateral_position_control(
self.local_position_target[0:2], self.local_velocity_target[0:2],
self.local_position[0:2], self.local_velocity[0:2])
self.local_acceleration_target = np.array(
[acceleration_cmd[0], acceleration_cmd[1], 0.0])
def attitude_controller(self):
self.thrust_cmd = self.controller.altitude_control(
-self.local_position_target[2], -self.local_velocity_target[2],
-self.local_position[2], -self.local_velocity[2], self.attitude,
9.81)
roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
self.local_acceleration_target[0:2], self.attitude,
self.thrust_cmd)
yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
self.attitude[2])
self.body_rate_target = np.array(
[roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])
def bodyrate_controller(self):
moment_cmd = self.controller.body_rate_control(self.body_rate_target,
self.gyro_raw)
self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
self.thrust_cmd)
def attitude_callback(self):
if self.flight_state == States.WAYPOINT:
self.attitude_controller()
def gyro_callback(self):
if self.flight_state == States.WAYPOINT:
self.bodyrate_controller()
def local_position_callback(self):
if self.flight_state == States.MANUAL:
self.takeoff_transition()
if self.flight_state == States.TAKEOFF:
if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:
# self.all_waypoints = self.calculate_box()
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory) = self.load_test_trajectory(time_mult=1)
self.all_waypoints = self.position_trajectory.copy()
self.waypoint_number = -1
self.waypoint_transition()
elif self.flight_state == States.WAYPOINT:
if time.time() > self.time_trajectory[self.waypoint_number]:
if len(self.all_waypoints) > 0:
self.waypoint_transition()
else:
if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
self.landing_transition()
def velocity_callback(self):
if self.flight_state == States.LANDING:
if abs(self.local_position[2] < 0.01):
self.manual_transition()
if self.flight_state == States.WAYPOINT:
self.position_controller()
def state_callback(self):
if self.in_mission:
if self.flight_state == States.MANUAL:
self.arming_transition()
elif self.flight_state == States.ARMING:
if self.armed:
self.takeoff_transition()
elif self.flight_state == States.DISARMING:
if ~self.armed & ~self.guided:
self.manual_transition()
def calculate_box(self):
cp = self.local_position
cp[2] = 0
local_waypoints = [
cp + [0.2, 0.0, 0.3], cp + [0.2, 0.2, 0.3], cp + [0.0, 0.2, 0.3],
cp + [0.0, 0.0, 0.3]
]
return local_waypoints
def arming_transition(self):
print("arming transition")
self.take_control()
self.arm()
# set the current location to be the home position
self.set_home_position(self.global_position[0],
self.global_position[1],
self.global_position[2])
self.flight_state = States.ARMING
def takeoff_transition(self):
print("takeoff transition")
target_altitude = 3.0
self.target_position[2] = target_altitude
self.takeoff(target_altitude)
self.flight_state = States.TAKEOFF
def waypoint_transition(self):
print("waypoint transition")
self.waypoint_number = self.waypoint_number + 1
self.target_position = self.all_waypoints.pop(0)
print('target position', self.target_position)
# self.local_position_target = np.array([0.0, 0.0, -10.0])
self.local_position_target = np.array(
(self.target_position[0], self.target_position[1],
self.target_position[2]))
# self.local_position_target = np.array([0.0, 0.0, -3.0])
self.flight_state = States.WAYPOINT
def landing_transition(self):
print("landing transition")
self.land()
self.flight_state = States.LANDING
def disarming_transition(self):
print("disarm transition")
self.disarm()
self.release_control()
self.flight_state = States.DISARMING
def manual_transition(self):
print("manual transition")
self.stop()
self.in_mission = False
self.flight_state = States.MANUAL
def start(self):
self.start_log("Logs", "NavLog.txt")
# self.connect()
print("starting connection")
# self.connection.start()
super().start()
# Only required if they do threaded
# while self.in_mission:
# pass
self.stop_log()
class BackyardFlyer(Drone):
def __init__(self, connection):
super().__init__(connection)
self.target_position = np.array([0.0, 0.0, 0.0])
self.all_waypoints = []
self.in_mission = True
self.check_state = {}
self.controller = NonlinearController()
# initial state
self.flight_state = States.MANUAL
# register all your callbacks here
self.register_callback(MsgID.LOCAL_POSITION,
self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_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)
def attitude_callback(self):
if self.flight_state == States.WAYPOINT:
self.attitude_controller()
def gyro_callback(self):
if self.flight_state == States.WAYPOINT:
self.bodyrate_controller()
def local_position_callback(self):
if self.flight_state == States.TAKEOFF:
if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:
#self.all_waypoints = self.calculate_box()
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory) = self.load_test_trajectory(
time_mult=0.5)
self.all_waypoints = self.position_trajectory.copy()
self.waypoint_number = -1
self.waypoint_transition()
elif self.flight_state == States.WAYPOINT:
#if np.linalg.norm(self.target_position[0:2] - self.local_position[0:2]) < 1.0:
if time.time() > self.time_trajectory[self.waypoint_number]:
if len(self.all_waypoints) > 0:
self.waypoint_transition()
else:
if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
self.landing_transition()
def velocity_callback(self):
if self.flight_state == States.LANDING:
if self.global_position[2] - self.global_home[2] < 0.1:
if abs(self.local_position[2]) < 0.01:
self.disarming_transition()
if self.flight_state == States.WAYPOINT:
self.position_controller()
def state_callback(self):
if self.in_mission:
if self.flight_state == States.MANUAL:
self.arming_transition()
elif self.flight_state == States.ARMING:
if self.armed:
self.takeoff_transition()
elif self.flight_state == States.DISARMING:
if ~self.armed & ~self.guided:
self.manual_transition()
def calculate_box(self):
print("Setting Home")
local_waypoints = [[10.0, 0.0, 3.0], [10.0, 10.0, 3.0],
[0.0, 10.0, 3.0], [0.0, 0.0, 3.0]]
return local_waypoints
def arming_transition(self):
print("arming transition")
self.take_control()
self.arm()
self.set_home_position(
self.global_position[0], self.global_position[1],
self.global_position[2]
) # set the current location to be the home position
self.flight_state = States.ARMING
def takeoff_transition(self):
print("takeoff transition")
# self.global_home = np.copy(self.global_position) # can't write to this variable!
target_altitude = 3.0
self.target_position[2] = target_altitude
self.takeoff(target_altitude)
self.flight_state = States.TAKEOFF
def waypoint_transition(self):
print("waypoint transition")
self.target_position = self.all_waypoints.pop(0)
print('target position', self.target_position)
#self.cmd_position(self.target_position[0], self.target_position[1], self.target_position[2], 0.0)
self.local_position_target = np.array(
(self.target_position[0], self.target_position[1],
self.target_position[2]))
self.flight_state = States.WAYPOINT
self.waypoint_number = self.waypoint_number + 1
def landing_transition(self):
print("landing transition")
self.land()
self.flight_state = States.LANDING
def disarming_transition(self):
print("disarm transition")
self.disarm()
self.release_control()
self.flight_state = States.DISARMING
def manual_transition(self):
print("manual transition")
self.stop()
self.in_mission = False
self.flight_state = States.MANUAL
def start(self):
self.start_log("Logs", "NavLog.txt")
# self.connect()
print("starting connection")
# self.connection.start()
super().start()
# Only required if they do threaded
# while self.in_mission:
# pass
self.stop_log()
def position_controller(self):
"""Sets the local acceleration target using the local position and local velocity"""
(self.local_position_target, self.local_velocity_target,
yaw_cmd) = self.controller.trajectory_control(
self.position_trajectory, self.yaw_trajectory,
self.time_trajectory, time.time())
self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
acceleration_cmd = self.controller.lateral_position_control(
self.local_position_target[0:2], self.local_velocity_target[0:2],
self.local_position[0:2], self.local_velocity[0:2])
self.local_acceleration_target = np.array(
[acceleration_cmd[0], acceleration_cmd[1], 0.0])
def attitude_controller(self):
"""Sets the body rate target using the acceleration target and attitude"""
self.thrust_cmd = self.controller.altitude_control(
-self.local_position_target[2], -self.local_velocity_target[2],
-self.local_position[2], -self.local_velocity[2], self.attitude,
9.81)
roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
self.local_acceleration_target[0:2], self.attitude,
self.thrust_cmd)
yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
self.attitude[2])
self.body_rate_target = np.array(
[roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])
def bodyrate_controller(self):
"""Commands a moment to the vehicle using the body rate target and body rates"""
moment_cmd = self.controller.body_rate_control(self.body_rate_target,
self.gyro_raw)
self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
self.thrust_cmd)
class ControlsFlyer(UnityDrone):
def __init__(self, connection):
super().__init__(connection)
self.controller = NonlinearController()
self.target_position = np.array([0.0, 0.0, 0.0])
self.all_waypoints = []
self.in_mission = True
self.check_state = {}
# initial state
self.flight_state = States.MANUAL
# register all your callbacks here
self.register_callback(MsgID.LOCAL_POSITION,
self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_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)
# default opens up to http://localhost:8097
self.v = visdom.Visdom()
# # Plot NE
# ne = np.array(self.local_position[:2]).reshape(-1, 2)
# self.ne_plot = self.v.scatter(ne, opts=dict(
# title="Local position (north, east)",
# xlabel='North',
# ylabel='East'
# ))
# # Plot D
# d = np.array([self.local_position[2]])
# self.t = 1
# print(d.ndim)
# self.d_plot = self.v.line(d, X=np.array([self.t]), opts=dict(
# title="Altitude (meters)",
# xlabel='Timestep',
# ylabel='Down'
# ))
# for plotting realtime NEO data with visdom
# self.register_callback(MsgID.LOCAL_POSITION, self.update_ne_plot)
# self.register_callback(MsgID.LOCAL_POSITION, self.update_d_plot)
# # for plotting realtime NEO data with visdom
# def update_ne_plot(self):
# ne = np.array([self.local_position[0], self.local_position[1]]).reshape(-1, 2)
# self.v.scatter(ne, win=self.ne_plot, update='append')
# # for plotting realtime NEO data with visdom
# def update_d_plot(self):
# d = -np.array([self.local_position[2]])
# # update timestep
# self.t += 1
# self.v.line(d, X=np.array([self.t]), win=self.d_plot, update='append')
def position_controller(self):
(self.local_position_target, self.local_velocity_target,
yaw_cmd) = self.controller.trajectory_control(
self.position_trajectory, self.yaw_trajectory,
self.time_trajectory, time.time())
self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
acceleration_cmd = self.controller.lateral_position_control(
self.local_position_target[0:2], self.local_velocity_target[0:2],
self.local_position[0:2], self.local_velocity[0:2])
self.local_acceleration_target = np.array(
[acceleration_cmd[0], acceleration_cmd[1], 0.0])
def attitude_controller(self):
self.thrust_cmd = self.controller.altitude_control(
-self.local_position_target[2], -self.local_velocity_target[2],
-self.local_position[2], -self.local_velocity[2], self.attitude,
9.81)
roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
self.local_acceleration_target[0:2], self.attitude,
self.thrust_cmd)
yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
self.attitude[2])
self.body_rate_target = np.array(
[roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])
def bodyrate_controller(self):
moment_cmd = self.controller.body_rate_control(self.body_rate_target,
self.gyro_raw)
self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
self.thrust_cmd)
def attitude_callback(self):
if self.flight_state == States.WAYPOINT:
self.attitude_controller()
def gyro_callback(self):
if self.flight_state == States.WAYPOINT:
self.bodyrate_controller()
def local_position_callback(self):
if self.flight_state == States.TAKEOFF:
if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:
#self.all_waypoints = self.calculate_box()
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory) = self.load_test_trajectory(
time_mult=0.5)
self.all_waypoints = self.position_trajectory.copy()
self.waypoint_number = -1
self.waypoint_transition()
elif self.flight_state == States.WAYPOINT:
if time.time() > self.time_trajectory[self.waypoint_number]:
if len(self.all_waypoints) > 0:
self.waypoint_transition()
else:
if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
self.landing_transition()
def velocity_callback(self):
if self.flight_state == States.LANDING:
if self.global_position[2] - self.global_home[2] < 0.1:
if abs(self.local_position[2]) < 0.01:
self.disarming_transition()
if self.flight_state == States.WAYPOINT:
self.position_controller()
def state_callback(self):
if self.in_mission:
if self.flight_state == States.MANUAL:
self.arming_transition()
elif self.flight_state == States.ARMING:
if self.armed:
self.takeoff_transition()
elif self.flight_state == States.DISARMING:
if ~self.armed & ~self.guided:
self.manual_transition()
def calculate_box(self):
print("Setting Home")
local_waypoints = [[10.0, 0.0, -3.0], [10.0, 10.0, -3.0],
[0.0, 10.0, -3.0], [0.0, 0.0, -3.0]]
return local_waypoints
def arming_transition(self):
print("arming transition")
self.take_control()
self.arm()
# set the current location to be the home position
self.set_home_position(self.global_position[0],
self.global_position[1],
self.global_position[2])
self.flight_state = States.ARMING
def takeoff_transition(self):
print("takeoff transition")
target_altitude = 3.0
self.target_position[2] = target_altitude
self.takeoff(target_altitude)
self.flight_state = States.TAKEOFF
def waypoint_transition(self):
#print("waypoint transition")
self.waypoint_number = self.waypoint_number + 1
self.target_position = self.all_waypoints.pop(0)
self.flight_state = States.WAYPOINT
def landing_transition(self):
print("landing transition")
self.land()
self.flight_state = States.LANDING
def disarming_transition(self):
print("disarm transition")
self.disarm()
self.release_control()
self.flight_state = States.DISARMING
def manual_transition(self):
print("manual transition")
self.stop()
self.in_mission = False
self.flight_state = States.MANUAL
def start(self):
self.start_log("Logs", "NavLog.txt")
# self.connect()
print("starting connection")
# self.connection.start()
super().start()
# Only required if they do threaded
# while self.in_mission:
# pass
self.stop_log()
class ControlsFlyer(UnityDrone):
def __init__(self, connection):
super().__init__(connection)
self.controller = NonlinearController()
self.target_position = np.array([0.0, 0.0, 0.0])
self.all_waypoints = []
self.in_mission = True
self.check_state = {}
# initial state
self.flight_state = States.MANUAL
self.prev_time = time.time()
self.att_time = 0.0
self.pos_time = 0.0
self.pulse = False
# register callbacks
self.register_callback(MsgID.LOCAL_POSITION,
self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_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)
def position_controller(self):
(self.local_position_target, self.local_velocity_target, yaw_cmd,
accff) = self.controller.trajectory_control(self.position_trajectory,
self.yaw_trajectory,
self.time_trajectory,
time.time(), True)
self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
cur_time = time.time()
acceleration_cmd = self.controller.lateral_position_control(
self.local_position_target[0:2], self.local_velocity_target[0:2],
self.local_position[0:2], self.local_velocity[0:2], accff[0:2],
cur_time - self.prev_time)
self.prev_time = cur_time
self.local_acceleration_target = np.array(
[acceleration_cmd[0], acceleration_cmd[1], 0.0])
def attitude_controller(self):
self.thrust_cmd = self.controller.altitude_control(
-self.local_position_target[2], -self.local_velocity_target[2],
-self.local_position[2], -self.local_velocity[2], self.attitude,
9.81)
roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
self.local_acceleration_target[0:2], self.attitude,
self.thrust_cmd)
if self.pulse:
roll_pitch_rate_cmd[0] = 30.0
roll_pitch_rate_cmd[1] = 0.0
self.pulse = False
yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
self.attitude[2])
self.body_rate_target = np.array(
[roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])
def bodyrate_controller(self):
moment_cmd = self.controller.body_rate_control(self.body_rate_target,
self.gyro_raw)
self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
self.thrust_cmd)
def attitude_callback(self): # 40 frames per second
if time.time() - self.att_time > 0.05: # pace the calls to 20fps
if self.flight_state == States.WAYPOINT:
self.attitude_controller()
self.att_time = time.time()
def gyro_callback(self): # 40 frames per second
if self.flight_state == States.WAYPOINT:
self.bodyrate_controller()
def local_position_callback(self): # 40 frames per second
#
#print(time.time())
if self.flight_state == States.TAKEOFF:
if abs(self.local_position[2] +
self.target_position[2]) < 0.025 and np.linalg.norm(
self.local_velocity[0:2]) < 0.1:
if TRAJECTORY is 1:
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory) = self.calculate_box_trajectory()
elif TRAJECTORY is 2:
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory) = self.calculate_fig8_trajectory()
elif TRAJECTORY is 3:
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory) = self.calculate_hover()
elif TRAJECTORY is 4:
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory
) = self.calculate_diagonal_trajectory()
elif TRAJECTORY is 5:
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory
) = self.calculate_leftright_trajectory()
else: # otherwise, perform the default test trajectory
print("Loading Test Trajectory...")
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory) = self.load_test_trajectory(
time_mult=0.5)
self.all_waypoints = self.position_trajectory.copy()
self.waypoint_number = -1
self.waypoint_transition()
print("Begin flight...")
elif self.flight_state == States.WAYPOINT:
if time.time() > self.time_trajectory[self.waypoint_number]:
if len(self.all_waypoints) > 0:
if time.time(
) - self.pos_time > 0.04: # pace the calls to 25fps
self.waypoint_transition()
self.pos_time = time.time()
else:
if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
self.landing_transition()
def velocity_callback(self): # 80 frames per second
if time.time() - self.prev_time > 0.05: # pace the calls to 20fps
if self.flight_state == States.LANDING:
if self.global_position[2] - self.global_home[2] < 0.1:
if abs(self.local_position[2]) < 0.01:
self.disarming_transition()
if self.flight_state == States.WAYPOINT:
self.position_controller()
def state_callback(self): # 1 frame per second
if self.in_mission:
if self.flight_state == States.MANUAL:
self.arming_transition()
elif self.flight_state == States.ARMING:
if self.armed:
self.takeoff_transition()
elif self.flight_state == States.DISARMING:
if ~self.armed & ~self.guided:
self.manual_transition()
def calculate_fig8_trajectory(self):
print("Creating Figure 8 Trajectory..."
) # same distance and timing as test trajectory
total_time = 19.4
t = np.linspace(0.0, total_time,
int(total_time / 0.05)) # inspired by class exercise
xpath = 7.7186622 * np.sin(0.323875531 * (t + 2.35) * 2) - 7.70955541
ypath = 7.7186622 * np.cos(0.323875531 *
(t + 2.35)) - 7.7186622 + 2.12979
zpath = np.cos(0.323875531 * (t + 2.35)) - 4.0 + 0.2759274
position_trajectory = []
for n in range(0, len(xpath)):
position_trajectory.append(np.array([xpath[n], ypath[n],
zpath[n]]))
yaw_trajectory = []
for i in range(0, len(position_trajectory) - 1):
yaw_trajectory.append(
np.arctan2(
position_trajectory[i + 1][1] - position_trajectory[i][1],
position_trajectory[i + 1][0] - position_trajectory[i][0]))
yaw_trajectory.append(yaw_trajectory[-1])
current_time = time.time() #+ 0.18
time_trajectory = np.linspace(current_time, current_time + total_time,
int(total_time / 0.05))
return (position_trajectory, time_trajectory, yaw_trajectory)
def calculate_leftright_trajectory(self):
print("Creating Left-Right Trajectory..."
) # same distance and timing as test trajectory
position_trajectory = [self.local_position]
for i in range(12):
position_trajectory.append(np.array([0.0, 1.5, -3.5]))
position_trajectory.append(np.array([0.0, -1.5, -3.5]))
position_trajectory.append(np.array([0.0, 1.5, -3.5]))
position_trajectory.append(np.array([0.0, 0.0, -3.0]))
current_time = time.time()
time_trajectory = []
for i in range(27):
time_trajectory.append(current_time + 0.18 + i * 0.746154)
yaw_trajectory = []
for i in range(0, len(position_trajectory)):
yaw_trajectory.append(0.0)
return (position_trajectory, time_trajectory, yaw_trajectory)
def calculate_box_trajectory(self):
print("Creating Box Trajectory..."
) # same distance and timing as test trajectory
position_trajectory = [
self.local_position,
np.array([18.195, 0.0, -4.0]),
np.array([18.195, 18.195, -5.0]),
np.array([0.0, 18.195, -4.0]),
np.array([0.0, 0.0, -3.0])
]
current_time = time.time()
time_trajectory = [
current_time + 0.18, current_time + 4.85 + 0.18,
current_time + 9.7 + 0.18, current_time + 14.55 + 0.18,
current_time + 19.4 + 0.18
]
yaw_trajectory = []
for i in range(0, len(position_trajectory) - 1):
yaw_trajectory.append(
np.arctan2(
position_trajectory[i + 1][1] - position_trajectory[i][1],
position_trajectory[i + 1][0] - position_trajectory[i][0]))
yaw_trajectory.append(yaw_trajectory[-1])
return (position_trajectory, time_trajectory, yaw_trajectory)
def calculate_diagonal_trajectory(self):
print("Creating Box Trajectory..."
) # same distance and timing as test trajectory
position_trajectory = [
self.local_position,
np.array([57.2, 45.0, -3.0])
]
current_time = time.time()
time_trajectory = [current_time + 0.18, current_time + 19.4 + 0.18]
yaw_trajectory = []
for i in range(0, len(position_trajectory) - 1):
yaw_trajectory.append(
np.arctan2(
position_trajectory[i + 1][1] - position_trajectory[i][1],
position_trajectory[i + 1][0] - position_trajectory[i][0]))
yaw_trajectory.append(yaw_trajectory[-1])
return (position_trajectory, time_trajectory, yaw_trajectory)
def calculate_hover(self):
print("Creating hover trajectory...") # same timing as test trajectory
position_trajectory = [
np.array([0.0, 0.0, -3.0]),
np.array([0.0, 0.0, -3.0])
] #[self.local_position, self.local_position]
current_time = time.time()
time_trajectory = [current_time + 0.18, current_time + 19.4 + 0.18]
yaw_trajectory = []
for i in range(0, len(position_trajectory) - 1):
yaw_trajectory.append(
np.arctan2(
position_trajectory[i + 1][1] - position_trajectory[i][1],
position_trajectory[i + 1][0] - position_trajectory[i][0]))
yaw_trajectory.append(yaw_trajectory[-1])
return (position_trajectory, time_trajectory, yaw_trajectory)
def arming_transition(self):
print("arming transition")
self.take_control()
self.arm()
# set the current location to be the home position
self.set_home_position(self.global_position[0],
self.global_position[1],
self.global_position[2])
self.flight_state = States.ARMING
def takeoff_transition(self):
print("takeoff transition")
target_altitude = 3.0
self.target_position[2] = target_altitude
self.takeoff(target_altitude)
self.flight_state = States.TAKEOFF
def waypoint_transition(self):
self.waypoint_number = self.waypoint_number + 1
self.target_position = self.all_waypoints.pop(0)
#self.target_position = np.array([0.0, 0.0, -3.0]) # #######
#print("Waypoint transition:", self.waypoint_number, self.target_position)
self.flight_state = States.WAYPOINT
def landing_transition(self):
print("landing transition")
self.land()
self.flight_state = States.LANDING
def disarming_transition(self):
print("disarm transition")
self.disarm()
self.release_control()
self.flight_state = States.DISARMING
def manual_transition(self):
print("manual transition")
self.stop()
self.in_mission = False
self.flight_state = States.MANUAL
def start(self):
self.start_log("Logs", "NavLog.txt")
# self.connect()
print("starting connection")
# self.connection.start()
super().start()
# Only required if they do threaded
# while self.in_mission:
# pass
self.stop_log()
class ControlsFlyer(UnityDrone):
def __init__(self, connection):
super().__init__(connection)
self.target_position = np.array([0.0, 0.0, 0.0])
self.all_waypoints = []
self.in_mission = True
self.check_state = {}
# initial state
self.flight_state = States.MANUAL
self.controller = NonlinearController()
# debug counters
self.attitude_cnt = 0
self.gyro_cnt = 0
self.velocity_cnt = 0
self.start_time = time.time()
# register all your callbacks here
self.register_callback(MsgID.STATE, self.state_callback)
self.register_callback(MsgID.LOCAL_POSITION,
self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)
def state_callback(self):
if self.in_mission:
if self.flight_state == States.MANUAL:
self.arming_transition()
elif self.flight_state == States.ARMING:
if self.armed:
self.takeoff_transition()
elif self.flight_state == States.DISARMING:
if ~self.armed & ~self.guided:
self.manual_transition()
def local_position_callback(self):
if self.flight_state == States.TAKEOFF:
if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:
# PLAN PATH
print("plan trajectory")
#self.all_waypoints = self.calculate_box()
time_mult = 0.5
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory) = self.load_test_trajectory(time_mult)
self.all_waypoints = self.position_trajectory.copy()
self.waypoint_number = -1
# EXECUTE PATH: start
print(
"execute trajectory"
) # not quite. that just tracks where we should be in time, but does not drive the controls
self.waypoint_transition()
elif self.flight_state == States.WAYPOINT:
if time.time() > self.time_trajectory[self.waypoint_number]:
if len(self.all_waypoints) > 0:
# EXECUTE PATH: continue
self.waypoint_transition()
#pass
else:
if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
# EXECUTE PATH: finish
self.landing_transition()
def attitude_callback(self):
if self.flight_state == States.WAYPOINT:
self.attitude_cnt += 1
#self.attitude_controller()
self.thrust_cmd = self.controller.altitude_control(
-self.local_position_target[2], -self.local_velocity_target[2],
-self.local_position[2], -self.local_velocity[2],
self.attitude, 9.81)
roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
self.local_acceleration_target[0:2], self.attitude,
self.thrust_cmd)
yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
self.attitude[2])
self.body_rate_target = np.array(
[roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])
def gyro_callback(self):
if self.flight_state == States.WAYPOINT:
#self.bodyrate_controller()
# use body_rate_target set in attitude callback
# this controller runs a faster loop
self.gyro_cnt += 1
if self.gyro_cnt % 20 == 0:
print('time: {:.4f} secs'.format(time.time() -
self.start_time))
print('gyro {}, att {}, vel {}'.format(self.gyro_cnt,
self.attitude_cnt,
self.velocity_cnt))
moment_cmd = self.controller.body_rate_control(
self.body_rate_target, self.gyro_raw)
# print("alt {:.4f}/{:.4f}; alt_vel {:.4f}/{:.4f}; thrust {:.4f}".format(
# self.local_position[2], self.local_position_target[2],
# self.local_velocity[2], self.local_velocity_target[2],
# self.thrust_cmd))
# print("roll {:.4f}/{:.4f}; pitch {:.4f}/{:.4f}; yaw {:.4f}/{:.4f}; moments {:.4f}, {:.4f}, {:.4f}; thrust {:.4f}".format(
# self.attitude[0], self.attitude_target[0],
# self.attitude[1], self.attitude_target[1],
# self.attitude[2], self.attitude_target[2],
# moment_cmd[0], moment_cmd[1], moment_cmd[2],
# self.thrust_cmd
# )
# )
self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
self.thrust_cmd)
def velocity_callback(self):
if self.flight_state == States.LANDING:
# landed? disarm
if self.global_position[2] - self.global_home[2] < 0.1:
if abs(self.local_position[2]) < 0.01:
self.disarming_transition()
if self.flight_state == States.WAYPOINT:
# in mission? control position and speed
#self.position_controller()
self.velocity_cnt += 1
(self.local_position_target, self.local_velocity_target,
yaw_cmd) = self.controller.trajectory_control(
self.position_trajectory, self.yaw_trajectory,
self.time_trajectory, time.time())
# print('time: {:.4f}; target position: {:.4f}/{:.4f}; target vel: {:.4f}/{:.4f}'.format(time.time() - self.start_time,
# self.local_position_target[0], self.local_position_target[1],
# self.local_velocity_target[0], self.local_velocity_target[1]))
self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
acceleration_cmd = self.controller.lateral_position_control(
self.local_position_target[0:2],
self.local_velocity_target[0:2], self.local_position[0:2],
self.local_velocity[0:2])
#acceleration_cmd = np.array([0.0, 0.0])
# print(" acc:{:.4f}/{:.4f} --- pos diff:{:.4f}/{:.4f} --- vel diff:{:.4f}/{:.4f}".format(
# acceleration_cmd[0], acceleration_cmd[1],
# self.local_position_target[0] - self.local_position[0], self.local_position_target[1] - self.local_position[1],
# self.local_velocity_target[0] - self.local_velocity[0], self.local_velocity_target[1] - self.local_velocity[1]
# ))
self.local_acceleration_target = np.array(
[acceleration_cmd[0], acceleration_cmd[1], 0.0])
def arming_transition(self):
print("arming transition")
self.take_control()
self.arm()
# set the current location to be the home position
self.set_home_position(self.global_position[0],
self.global_position[1],
self.global_position[2])
self.flight_state = States.ARMING
def takeoff_transition(self):
print("takeoff transition")
target_altitude = 3.0
self.target_position[2] = target_altitude
self.takeoff(target_altitude)
self.flight_state = States.TAKEOFF
def waypoint_transition(self):
self.waypoint_number = self.waypoint_number + 1
self.target_position = self.all_waypoints.pop(0)
print('time: {:.4f}; planned waypoint position: {}, {}'.format(
time.time() - self.start_time, self.target_position[0],
self.target_position[1]))
self.flight_state = States.WAYPOINT
def landing_transition(self):
print("landing transition")
self.land()
self.flight_state = States.LANDING
def disarming_transition(self):
print("disarm transition")
self.disarm()
self.release_control()
self.flight_state = States.DISARMING
def manual_transition(self):
print("manual transition")
self.stop()
self.in_mission = False
self.flight_state = States.MANUAL
def start(self):
self.start_log("Logs", "NavLog.txt")
# self.connect()
print("starting connection")
# self.connection.start()
super().start()
# Only required if they do threaded
# while self.in_mission:
# pass
self.stop_log()
class BackyardFlyer(UnityDrone):
def __init__(self, connection):
super().__init__(connection)
self.target_position = np.array([0.0, 0.0, 0.0])
self.all_waypoints = []
self.in_mission = True
self.check_state = {}
# initial state
self.flight_state = States.MANUAL
# TODO: Register all your callbacks here
self.register_callback(MsgID.LOCAL_POSITION,
self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
self.register_callback(MsgID.STATE, self.state_callback)
# register callbacks for RAW_GYROSCOPE, ATTITUDE, and LOCAL_VELOCITY messages.
self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
# Add a controller object
self.controller = NonlinearController()
# add three controller methods
def position_controller(self):
"""Sets the local acceleration target using the local position and local velocity"""
(self.local_position_target, self.local_velocity_target,
yaw_cmd) = self.controller.trajectory_control(
self.position_trajectory, self.yaw_trajectory,
self.time_trajectory, time.time())
self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
acceleration_cmd = self.controller.lateral_position_control(
self.local_position_target[0:2], self.local_velocity_target[0:2],
self.local_position[0:2], self.local_velocity[0:2])
self.local_acceleration_target = np.array(
[acceleration_cmd[0], acceleration_cmd[1], 0.0])
def attitude_controller(self):
"""Sets the body rate target using the acceleration target and attitude"""
self.thrust_cmd = self.controller.altitude_control(
-self.local_position_target[2], -self.local_velocity_target[2],
-self.local_position[2], -self.local_velocity[2], self.attitude,
9.81)
roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
self.local_acceleration_target[0:2], self.attitude,
self.thrust_cmd)
yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
self.attitude[2])
self.body_rate_target = np.array(
[roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])
def bodyrate_controller(self):
"""Commands a moment to the vehicle using the body rate target and body rates"""
moment_cmd = self.controller.body_rate_control(self.body_rate_target,
self.gyro_raw)
self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
self.thrust_cmd)
def local_position_callback(self):
"""
TODO: Implement this method
This triggers when `MsgID.LOCAL_POSITION` is received and self.local_position contains new data
"""
if self.flight_state == States.TAKEOFF:
# coordinate conversion to positive value
altitude = -1.0 * self.local_position[
2] # self.local_position[2] is vertical coordinate, positive pointing down
# check if altitude is within 95% of target
if altitude > 0.95 * self.target_position[2]:
# self.all_waypoints = self.calculate_box()
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory) = self.load_test_trajectory(
time_mult=0.5)
self.all_waypoints = self.position_trajectory.copy()
self.waypoint_number = -1
self.waypoint_transition() # call state transition function
elif self.flight_state == States.WAYPOINT:
#when approaching target position within 1m
# if np.linalg.norm(self.target_position[0:2] - self.local_position[0:2]) < 1.0:
if time.time() > self.time_trajectory[self.waypoint_number]:
if len(self.all_waypoints
) > 0: # not yet finished way points following
self.waypoint_transition()
else:
# when velocity less than 1.0, land down
if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
self.landing_transition()
def attitude_callback(self):
if self.flight_state == States.WAYPOINT:
self.attitude_controller()
def gyro_callback(self):
if self.flight_state == States.WAYPOINT:
self.bodyrate_controller()
def velocity_callback(self):
"""
TODO: Implement this method
This triggers when `MsgID.LOCAL_VELOCITY` is received and self.local_velocity contains new data
"""
if self.flight_state == States.LANDING:
if ((self.global_position[2] - self.global_home[2] < 0.1)
and abs(self.local_position[2]) < 0.01):
self.disarming_transition()
# for controller
if self.flight_state == States.WAYPOINT:
self.position_controller()
def state_callback(self):
"""
TODO: Implement this method
This triggers when `MsgID.STATE` is received and self.armed and self.guided contain new data
"""
if not self.in_mission:
return
if self.flight_state == States.MANUAL:
self.arming_transition()
elif self.flight_state == States.ARMING:
if self.armed:
self.takeoff_transition()
elif self.flight_state == States.DISARMING:
if not self.armed:
self.manual_transition()
def calculate_box(self):
"""TODO: Fill out this method
1. Return waypoints to fly a box
"""
print("Setting Home")
local_waypoints = [[5.0, 0.0, 5.0], [5.0, 5.0, 5.0], [0.0, 5.0, 5.0],
[0.0, 0.0, 5.0]]
return local_waypoints
def arming_transition(self):
"""TODO: Fill out this method
1. Take control of the drone
2. Pass an arming command
3. Set the home location to current position
4. Transition to the ARMING state
"""
print("arming transition")
self.take_control()
self.arm()
# set the current location to be the home position
self.set_home_position(self.global_position[0],
self.global_position[1],
self.global_position[2])
self.flight_state = States.ARMING
def takeoff_transition(self):
"""TODO: Fill out this method
1. Set target_position altitude to 5.0m
2. Command a takeoff to 5.0m
3. Transition to the TAKEOFF state
"""
print("takeoff transition")
target_altitude = 5.0
self.target_position[2] = target_altitude
self.takeoff(target_altitude)
self.flight_state = States.TAKEOFF
def waypoint_transition(self):
"""TODO: Fill out this method
1. Command the next waypoint position
2. Transition to WAYPOINT state
"""
print("waypoint transition")
# get the waypoints for target_position
self.target_position = self.all_waypoints.pop(0)
print('target position', self.target_position)
# input the global coordinates (vertical coordinate is positive when pointing upwards)
# self.cmd_position(self.target_position[0], self.target_position[1], self.target_position[2], 0.0)
# setting the target local position
self.local_position_target = np.array(
(self.target_position[0], self.target_position[1],
self.target_position[2]))
self.flight_state = States.WAYPOINT
self.waypoint_number = self.waypoint_number + 1
def landing_transition(self):
"""TODO: Fill out this method
1. Command the drone to land
2. Transition to the LANDING state
"""
print("landing transition")
self.land()
self.flight_state = States.LANDING
def disarming_transition(self):
"""TODO: Fill out this method
1. Command the drone to disarm
2. Transition to the DISARMING state
"""
print("disarm transition")
self.disarm()
self.flight_state = States.DISARMING
def manual_transition(self):
"""This method is provided
1. Release control of the drone
2. Stop the connection (and telemetry log)
3. End the mission
4. Transition to the MANUAL state
"""
print("manual transition")
self.release_control()
self.stop()
self.in_mission = False
self.flight_state = States.MANUAL
originDistance = 100 * np.linalg.norm(
self.local_position) # vector norm from numpy
print(f'Distance to origin : {originDistance:5.3f} cm')
def start(self):
"""This method is provided
1. Open a log file
2. Start the drone connection
3. Close the log file
"""
print("Creating log file")
self.start_log("Logs", "NavLog.txt")
print("starting connection")
self.connection.start()
print("Closing log file")
self.stop_log()
class ControlsFlyer(UnityDrone):
def __init__(self, connection):
super().__init__(connection)
self.controller = NonlinearController()
self.target_position = np.array([0.0, 0.0, 0.0])
self.all_waypoints = []
self.in_mission = True
self.check_state = {}
# initial state
self.flight_state = States.MANUAL
# register all your callbacks here
self.register_callback(MsgID.LOCAL_POSITION,
self.local_position_callback)
self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_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)
# flight history
self.target_traj = []
self.actual_traj = []
self.traj_t = []
self.target_v = []
self.actual_v = []
self.v_t = []
def position_controller(self):
(self.local_position_target, self.local_velocity_target,
yaw_cmd) = self.controller.trajectory_control(
self.position_trajectory, self.yaw_trajectory,
self.time_trajectory, time.time())
self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
acceleration_cmd = self.controller.lateral_position_control(
self.local_position_target[0:2], self.local_velocity_target[0:2],
self.local_position[0:2], self.local_velocity[0:2])
self.local_acceleration_target = np.array(
[acceleration_cmd[0], acceleration_cmd[1], 0.0])
def attitude_controller(self):
self.thrust_cmd = self.controller.altitude_control(
-self.local_position_target[2], -self.local_velocity_target[2],
-self.local_position[2], -self.local_velocity[2], self.attitude,
9.81)
roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
self.local_acceleration_target[0:2], self.attitude,
self.thrust_cmd)
yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
self.attitude[2])
self.body_rate_target = np.array(
[roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])
def bodyrate_controller(self):
moment_cmd = self.controller.body_rate_control(self.body_rate_target,
self.gyro_raw)
self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
self.thrust_cmd)
def attitude_callback(self):
if self.flight_state == States.WAYPOINT:
self.attitude_controller()
def gyro_callback(self):
if self.flight_state == States.WAYPOINT:
self.bodyrate_controller()
def local_position_callback(self):
if self.flight_state == States.TAKEOFF:
if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:
#self.all_waypoints = self.calculate_box()
(self.position_trajectory, self.time_trajectory,
self.yaw_trajectory) = self.load_test_trajectory(
time_mult=0.5)
self.all_waypoints = self.position_trajectory.copy()
self.waypoint_number = -1
self.waypoint_transition()
elif self.flight_state == States.WAYPOINT:
t = time.time()
self.traj_t.append(t)
self.target_traj.append(self.local_position_target)
self.actual_traj.append(self.local_position)
if time.time() > self.time_trajectory[self.waypoint_number]:
if len(self.all_waypoints) > 0:
self.waypoint_transition()
else:
if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
self.landing_transition()
def velocity_callback(self):
if self.flight_state == States.LANDING:
if self.global_position[2] - self.global_home[2] < 0.1:
if abs(self.local_position[2]) < 0.01:
self.disarming_transition()
if self.flight_state == States.WAYPOINT:
# print("target v: {}, actual v: {}".format(np.linalg.norm(self.local_velocity_target), np.linalg.norm(self.local_velocity)))
t = time.time()
self.v_t.append(t)
self.target_v.append(self.local_velocity_target)
self.actual_v.append(self.local_velocity)
self.position_controller()
def state_callback(self):
if self.in_mission:
if self.flight_state == States.MANUAL:
self.arming_transition()
elif self.flight_state == States.ARMING:
if self.armed:
self.takeoff_transition()
elif self.flight_state == States.DISARMING:
if ~self.armed & ~self.guided:
self.manual_transition()
def calculate_box(self):
print("Setting Home")
local_waypoints = [[10.0, 0.0, -3.0], [10.0, 10.0, -3.0],
[0.0, 10.0, -3.0], [0.0, 0.0, -3.0]]
return local_waypoints
def arming_transition(self):
print("arming transition")
self.take_control()
self.arm()
# set the current location to be the home position
self.set_home_position(self.global_position[0],
self.global_position[1],
self.global_position[2])
self.flight_state = States.ARMING
def takeoff_transition(self):
print("takeoff transition")
target_altitude = 3.0
self.target_position[2] = target_altitude
self.takeoff(target_altitude)
self.flight_state = States.TAKEOFF
def waypoint_transition(self):
#print("waypoint transition")
self.waypoint_number = self.waypoint_number + 1
self.target_position = self.all_waypoints.pop(0)
self.flight_state = States.WAYPOINT
def landing_transition(self):
print("landing transition")
self.land()
self.flight_state = States.LANDING
def disarming_transition(self):
print("disarm transition")
self.disarm()
self.release_control()
self.flight_state = States.DISARMING
def manual_transition(self):
print("manual transition")
self.stop()
self.in_mission = False
self.flight_state = States.MANUAL
def write_flight_log(self):
import pickle
logs = [
self.traj_t, self.target_traj, self.actual_traj, self.v_t,
self.target_v, self.actual_v
]
with open('flight_log', 'wb') as f:
pickle.dump(logs, f)
def start(self):
self.start_log("Logs", "NavLog.txt")
# self.connect()
print("starting connection")
# self.connection.start()
super().start()
# Only required if they do threaded
# while self.in_mission:
# pass
self.stop_log()
self.write_flight_log()