def publish_cmd(self, cmd):
"""Publish the controls to /pidrone/fly_commands """
msg = RC()
msg.roll = cmd[0]
msg.pitch = cmd[1]
msg.yaw = cmd[2]
msg.throttle = cmd[3]
self.cmdpub.publish(msg)
def pid():
cmdpub = rospy.Publisher('/pidrone/commands', RC, queue_size=1)
rc = RC()
time_prev = millis()
while not rospy.is_shutdown():
global sp_global
global pos_global
# print pos_global.orientation
calced_yaw = -calc_yaw_from_quat(
(pos_global.orientation.x, pos_global.orientation.y,
pos_global.orientation.z, pos_global.orientation.w))
time.sleep(0.001)
time_elapsed = millis() - time_prev
time_prev = millis()
(sp_global_pitch, sp_global_yaw,
sp_global_roll) = tf.transformations.euler_from_quaternion([
sp_global.orientation.x, sp_global.orientation.y,
sp_global.orientation.z, sp_global.orientation.w
])
(pos_global_pitch, pos_global_yaw, pos_global_roll) = (0, calced_yaw,
0)
# convert to the quad's frame of reference from the global
global sp
global pos
# XXX jgo: is this supposed to be multiplication of the vector ~_global
# (in the xz plane) by the rotation matrix induced by ~_global_yaw? if
# so, could you be missing a minus sign in front of one of the sin functions?
# The bottom left sin is negative so that we get the negative rotation, since
# we are converting to relative coordinates later
sp['fb'] = math.cos(pos_global_yaw) * sp_global.position.z + math.sin(
pos_global_yaw) * sp_global.position.x
sp['lr'] = -math.sin(pos_global_yaw) * sp_global.position.z + math.cos(
pos_global_yaw) * sp_global.position.x
pos['fb'] = math.cos(
pos_global_yaw) * pos_global.position.z + math.sin(
pos_global_yaw) * pos_global.position.x
pos['lr'] = -math.sin(
pos_global_yaw) * pos_global.position.z + math.cos(
pos_global_yaw) * pos_global.position.x
sp['yaw'] = sp_global_yaw - pos_global_yaw
pos['yaw'] = 0.0
sp['alt'] = sp_global.position.y - pos_global.position.y
pos['alt'] = 0.0
# XXX jgo: also it seems like you are setting "sp" yaw and alt relative
# to the values held by "pos", and setting "pos" values to 0,
# indicating that both are in the reference frame of "pos". Yet, the fb
# and lr values of "sp" and "pos" are both set relative to their own
# yaw values. Do you perhaps mean to use pos_global_yaw rather than
# sp_global_yaw, and maybe -pos_global_yaw in both cases rather than
# pos_global_yaw? (this sign matter interacts with whether and where a minus sign
# should go in front of a sin term above). my suspicion is reinforced,
# figuring that the output feeds to the roll and pitch of the aircraft in its current
# position, it seems like you want the fb and lr of both "sp" and "pos"
# represented in the frame of "pos".
# XXX jgo: my apologies if I'm misinterpreting this.
global old_err
global old_pos_global
if pos_global != old_pos_global:
for key in sp.keys():
err[key] = sp[key] - pos[key] # update the error
# calc the PID components of each axis
Pterm[key] = err[key]
# XXX jgo: this is a very literal interpretation of the I term
# which might be difficult to tune for reasons which we can
# discuss. it is more typical to take the integral over a finite
# interval in the past. This can be implemented with a ring buffer,
# or quickly approximated with an exponential moving average.
Iterm[key] += err[key] * time_elapsed
# XXX jgo: the sign of Dterm * kd should act as a viscosity or
# resistance term. if our error goes from 5 to 4,
# then Dterm ~ 4 - 5 = -1; so it looks like kd should be a positive number.
Dterm[key] = (err[key] - old_err[key]) / time_elapsed
old_err[key] = err[key]
# XXX jgo: definitely get something working with I and D terms equal to 0 before using these
if key == 'alt' and Pterm[key] < 0:
output[key] = Pterm[key] * kp['alt_above'] + Iterm[
key] * ki[key] + Dterm[key] * kd[key]
else:
output[key] = Pterm[key] * kp[key] + Iterm[key] * ki[
key] + Dterm[key] * kd[key]
old_pos_global = deepcopy(pos_global)
pwm_bandwidth = 1
pwm_scale = 0.0005 * pwm_bandwidth
# calculate the thrust and desired angle
(thrust, theta) = calc_thrust_and_theta(output['lr'], output['alt'],
output['fb'])
# and use that to calculate roll pitch yaw
# (pitch, yaw, roll) = calc_roll_pitch_from_theta(output['lr'], output['fb'], theta)
rc.roll = max(1450, min(1500 + output['lr'] * pwm_scale, 1550))
rc.pitch = max(1450, min(1500 + output['fb'] * pwm_scale, 1550))
rc.yaw = max(1000, min(1500 + output['yaw'] * pwm_scale, 2000))
rc.throttle = max(1150, min(1200 + output['alt'], 2000))
rc.aux1 = 1800
rc.aux2 = 1500
rc.aux3 = 1500
rc.aux4 = 1500
# print Pterm['fb'], Pterm['lr'], Dterm['fb'], Dterm['lr']
print rc.roll, rc.pitch, rc.yaw, rc.throttle
#print(str(roll) + "\t" + str(pitch) + "\t" + str(yaw))
cmdpub.publish(rc)
def main(screen):
rospy.init_node('key_flight', anonymous=True)
pub = rospy.Publisher('/pidrone/commands', RC, queue_size=1)
msg = RC()
msg.roll = 1500
msg.pitch = 1500
msg.yaw = 1500
msg.throttle = 1000
msg.aux1 = 1500
msg.aux2 = 1500
msg.aux3 = 1500
msg.aux4 = 1500
screen.nodelay(True)
while not rospy.is_shutdown():
key = ''
try:
key = screen.getkey()
except curses.error:
pass # no keypress was ready
msg.roll = 1500
msg.pitch = 1500
msg.yaw = 1500
msg.aux1 = 1500
msg.aux2 = 1500
msg.aux3 = 1500
msg.aux4 = 1500
if key == 'w':
msg.roll = 1600
elif key == 's':
msg.roll = 1400
elif key == 'a':
msg.pitch = 1400
elif key == 'd':
msg.pitch = 1600
elif key == 'q':
msg.yaw = 1400
elif key == 'e':
msg.yaw = 1600
elif key == 'u':
if msg.throttle <= 1900:
msg.throttle += 100
elif key == 'j':
if msg.throttle >= 1100:
msg.throttle -= 100
elif key == 'h':
msg.aux4 = 1600
elif key == 'n':
msg.aux4 = 1400
screen.addstr(
0, 0, 'Commands: {}'.format(
[msg.roll, msg.pitch, msg.yaw, msg.throttle, msg.aux4]))
pub.publish(msg)
time.sleep(.1)
#!/usr/bin/env python
import rospy
from pidrone_pkg.msg import RC
rospy.init_node('test_pub', anonymous=True)
pub = rospy.Publisher('/pidrone/commands', RC, queue_size=1)
rc = RC()
rc.roll = 1500
rc.pitch = 1500
rc.yaw = 1500
rc.throttle = 1500
rc.aux1 = 1500
rc.aux2 = 1500
rc.aux3 = 1500
rc.aux4 = 1500
while not rospy.is_shutdown():
pub.publish(rc)
def pid():
rc = RC()
time_prev = millis()
while not rospy.is_shutdown():
global sp_global
global pos_global
calced_yaw = -calc_yaw_from_quat((pos_global.orientation.x, pos_global.orientation.y,
pos_global.orientation.z, pos_global.orientation.w))
time.sleep(0.001)
time_elapsed = millis() - time_prev
time_prev = millis()
# Throwaway variables
sp_global_yaw = sp_global.orientation.w
#(ahh, pls, sp_global_yaw) = tf.transformations.euler_from_quaternion([sp_global.orientation.x, sp_global.orientation.y, sp_global.orientation.z, sp_global.orientation.w])
(work, now, pos_global_yaw) = tf.transformations.euler_from_quaternion([pos_global.orientation.x, pos_global.orientation.y, pos_global.orientation.z, pos_global.orientation.w])
# (pos_global_pitch, pos_global_yaw, pos_global_roll) = (0, calced_yaw, 0)
# convert to the quad's frame of reference from the global
global sp
global pos
# XXX jgo: is this supposed to be multiplication of the vector ~_global
# (in the xz plane) by the rotation matrix induced by ~_global_yaw? if
# so, could you be missing a minus sign in front of one of the sin functions?
# The bottom left sin is negative so that we get the negative rotation, since
# we are converting to relative coordinates later
# sp['fb'] = math.cos(pos_global_yaw) * sp_global.position.y + math.sin(pos_global_yaw) * sp_global.position.x
# sp['lr'] = -math.sin(pos_global_yaw) * sp_global.position.y + math.cos(pos_global_yaw) * sp_global.position.x
# pos['fb'] = math.cos(pos_global_yaw) * pos_global.position.y + math.sin(pos_global_yaw) * pos_global.position.x
# pos['lr'] = -math.sin(pos_global_yaw) * pos_global.position.y + math.cos(pos_global_yaw) * pos_global.position.x
error = {'x': sp_global.position.x - pos_global.position.x, 'y':
sp_global.position.y - pos_global.position.y}
#sp['fb'] = math.cos(pos_global_yaw) * err['y'] + math.sin(pos_global_yaw) * err['x']
#sp['lr'] = - math.cos(pos_global_yaw) * err['x'] + math.sin(pos_global_yaw) * err['y']
#print sp['fb'], sp['lr']
pos['fb'] = 0
pos['lr'] = 0
#sp['yaw'] = sp_global_yaw - pos_global_yaw
pos['yaw'] = 0.0
sp['alt'] = sp_global.position.z - pos_global.position.z
pos['alt'] = 0.0
# DIFFERENCE OF ANGLES
err_norm = np.sqrt(error['x']**2 + error['y']**2)
err_angle = -math.atan2(error['x'], error['y'])
diff_angle = pos_global_yaw - err_angle
sp['fb'] = np.cos(diff_angle) * err_norm
sp['lr'] = np.sin(diff_angle) * err_norm
# XXX jgo: also it seems like you are setting "sp" yaw and alt relative
# to the values held by "pos", and setting "pos" values to 0,
# indicating that both are in the reference frame of "pos". Yet, the fb
# and lr values of "sp" and "pos" are both set relative to their own
# yaw values. Do you perhaps mean to use pos_global_yaw rather than
# sp_global_yaw, and maybe -pos_global_yaw in both cases rather than
# pos_global_yaw? (this sign matter interacts with whether and where a minus sign
# should go in front of a sin term above). my suspicion is reinforced,
# figuring that the output feeds to the roll and pitch of the aircraft in its current
# position, it seems like you want the fb and lr of both "sp" and "pos"
# represented in the frame of "pos".
# XXX jgo: my apologies if I'm misinterpreting this.
global old_err
global old_pos_global
if pos_global != old_pos_global:
for key in sp.keys():
err[key] = sp[key] - pos[key] # update the error
#if key == 'yaw':
# print(sp[key], pos[key])
# calc the PID components of each axis
Pterm[key] = err[key]
# XXX jgo: this is a very literal interpretation of the I term
# which might be difficult to tune for reasons which we can
# discuss. it is more typical to take the integral over a finite
# interval in the past. This can be implemented with a ring buffer,
# or quickly approximated with an exponential moving average.
Iterm[key] += err[key] * time_elapsed
if key == 'alt' and Iterm[key] < 0:
Iterm[key] = 0
# XXX jgo: the sign of Dterm * kd should act as a viscosity or
# resistance term. if our error goes from 5 to 4,
# then Dterm ~ 4 - 5 = -1; so it looks like kd should be a positive number.
if key != 'alt':
old_old_Dterm[key] = old_Dterm[key]
old_Dterm[key] = new_Dterm[key]
new_Dterm[key] = (err[key] - old_err[key])/time_elapsed
Dterm[key] = (new_Dterm[key] * 8.0 + old_Dterm[key] * 5.0 +
old_old_Dterm[key] * 2.0)/15.0
else:
Dterm[key] = (err[key] - old_err[key])/time_elapsed
# XXX jgo: definitely get something working with I and D terms equal to 0 before using these
old_err[key] = err[key]
if key == 'alt' and Pterm[key] < 0:
output[key] = Pterm[key] * kp['alt_above'] + Iterm[key] * ki[key] + Dterm[key] * kd[key]
elif key == 'alt':
output[key] = Pterm[key] * kp[key] + Iterm[key] * ki[key] + Dterm[key] * kd[key]
else:
output[key] = Pterm[key] * kp[key] + Iterm[key] * ki[key] + Dterm[key] * kd[key]
old_pos_global = deepcopy(pos_global)
# calculate the thrust and desired angle
# (thrust, theta) = calc_thrust_and_theta(output['lr'], output['alt'], output['fb'])
# and use that to calculate roll pitch yaw
# (pitch, yaw, roll) = calc_roll_pitch_from_theta(output['lr'], output['fb'], theta)
rc.roll = max(1000, min(1500 + output['lr'], 2000))
rc.pitch = max(1000, min(1500 + output['fb'], 2000))
rc.yaw = max(1000, min(1500 + output['yaw'], 2000))
rc.throttle = max(1150, min(1200 + output['alt'], 2000))
rc.aux1 = 1800
rc.aux2 = 1500
rc.aux3 = 1500
rc.aux4 = 1500
#print Pterm['alt'] * kp['alt'], Dterm['alt'] * kd['alt'], Iterm['alt'] * ki['alt']
print rc.roll, rc.pitch, rc.yaw, rc.throttle
#print rc.roll > 1500, rc.pitch > 1500
#print(str(roll) + "\t" + str(pitch) + "\t" + str(yaw))
# print pos_global
# print 'TIME ELAPSED:', rospy.Time.now().to_sec() - pos_time.to_sec()
cmdpub.publish(rc)
error = axes_err()
if homography.update_H(curr_img):
homography.set_z(300.0 - ultra_z)
vel, z = homography.get_vel_and_z()
if np.abs(np.linalg.norm(vel)) < 2500:
##print "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"
##print vel
smoothed_vel = (1 - alpha) * smoothed_vel + alpha * vel
error.x.err = smoothed_vel[0]
error.y.err = smoothed_vel[1]
print vel, smoothed_vel
else:
print "&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&"
#print np.linalg.norm(vel)
#smoothed_vel = (1 - alpha) * smoothed_vel + alpha * vel
error.x.err = 0
error.y.err = 0
#print vel, smoothed_vel
cmds = pid.step(error)
rc = RC()
rc.roll = cmds[0]
rc.pitch = cmds[1]
cmdpub.publish(rc)
else:
##print "###################################################################################################"
print "Couldn't update H"
print "Shutdown Recieved"
sys.exit()
except Exception as e:
raise
#!/usr/bin/env python
import rospy
from pidrone_pkg.msg import RC
import pygame
rospy.init_node('key_flight', anonymous=True)
pub = rospy.Publisher('/pidrone/commands', RC, queue_size=1)
msg = RC()
msg.roll = 1500
msg.pitch = 1500
msg.yaw = 1500
msg.throttle = 1000
msg.aux1 = 1500
msg.aux2 = 1500
msg.aux3 = 1500
msg.aux4 = 1500
stop = False
pygame.init()
size = width, height = 320, 240
screen = pygame.display.set_mode(size)
while not rospy.is_shutdown() and not stop:
print([msg.roll, msg.pitch, msg.yaw, msg.throttle, msg.aux4])
pub.publish(msg)
msg.aux4 = 1500
events = pygame.event.get()
for event in events:
if event.type == pygame.KEYDOWN:
print(event.key)
if event.key == pygame.K_w:
msg.roll = 1600