def reset_callback(self, empty):
""" Reset the desired and current poses of the drone and set
desired velocities to zero """
self.current_position = Position(z=self.current_position.z)
self.desired_position = Position(z=self.current_position.z)
self.desired_velocity.x = 0
self.desired_velocity.y = 0
def position_control_callback(self, msg):
""" Set whether or not position control is enabled """
self.position_control = msg.data
if self.position_control:
self.desired_position = Position(self.current_position.x,self.current_position.y,self.current_position.z)
print "Position Control Activated Setpoint: ",self.desired_position
if (self.position_control != self.last_position_control):
print "Position Control", self.position_control
self.last_position_control = self.position_control
def reset(self):
''' Set desired_position to be current position, set
filtered_desired_velocity to be zero, and reset both the PositionPID
and VelocityPID
'''
# reset position control variables
self.position_error = Error(0, 0, 0)
self.desired_position = Position(self.current_position.x,
self.current_position.y, 0.3)
# reset velocity control_variables
self.velocity_error = Error(0, 0, 0)
self.desired_velocity = Velocity(0, 0, 0)
# reset the pids
self.pid.reset()
self.lr_pid.reset()
self.fb_pid.reset()
def reset(self):
''' Set desired_position to zero, set
filtered_desired_velocity to be zero, and reset both the PositionPID
and VelocityPID
'''
# reset position control variables
self.position_error = Error(0,0,0)
self.desired_position = Position(0,0,0.3)
# reset velocity control_variables
self.velocity_error = Error(0,0,0)
self.desired_velocity = Velocity(0,0,0)
# reset the pids
self.pid.reset()
self.lr_pid.reset()
self.fb_pid.reset()
print "Self reset called, desired position: ",self.desired_position
def __init__(self):
# Initialize the current and desired modes
self.current_mode = Mode('DISARMED')
self.desired_mode = Mode('DISARMED')
# Initialize in velocity control
self.position_control = False
# Initialize the current and desired positions
self.current_position = Position()
self.desired_position = Position(z=0.3)
self.last_desired_position = Position(z=0.3)
# Initialize the position error
self.position_error = Error()
# Initialize the current and desired velocities
self.current_velocity = Velocity()
self.desired_velocity = Velocity()
# Initialize the velocity error
self.velocity_error = Error()
# Set the distance that a velocity command will move the drone (m)
self.desired_velocity_travel_distance = 0.1
# Set a static duration that a velocity command will be held
self.desired_velocity_travel_time = 0.1
# Set a static duration that a yaw velocity command will be held
self.desired_yaw_velocity_travel_time = 0.25
# Store the start time of the desired velocities
self.desired_velocity_start_time = None
self.desired_yaw_velocity_start_time = None
# Initialize the primary PID
self.pid = PID()
# Initialize the error used for the PID which is vx, vy, z where vx and
# vy are velocities, and z is the error in the altitude of the drone
self.pid_error = Error()
# Initialize the 'position error to velocity error' PIDs:
# left/right (roll) pid
self.lr_pid = PIDaxis(kp=10.0,
ki=0.5,
kd=5.0,
midpoint=0,
control_range=(-10.0, 10.0))
# front/back (pitch) pid
self.fb_pid = PIDaxis(kp=10.0,
ki=0.5,
kd=5.0,
midpoint=0,
control_range=(-10.0, 10.0))
# Initialize the pose callback time
self.last_pose_time = None
# Initialize the desired yaw velocity
self.desired_yaw_velocity = 0
# Initialize the current and previous roll, pitch, yaw values
self.current_rpy = RPY()
self.previous_rpy = RPY()
# initialize the current and previous states
self.current_state = State()
self.previous_state = State()
# Store the command publisher
self.cmdpub = None
# a variable used to determine if the drone is moving between disired
# positions
self.moving = False
# a variable that determines the maximum magnitude of the position error
# Any greater position error will overide the drone into velocity
# control
self.safety_threshold = 1.5
# determines if the position of the drone is known
self.lost = False
# determines if the desired poses are aboslute or relative to the drone
self.absolute_desired_position = False
# determines whether to use open loop velocity path planning which is
# accomplished by calculate_travel_time
self.path_planning = True