def getBoard(self):
""" Connect to the flight controller board """
# (if the flight cotroll usb is unplugged and plugged back in,
# it becomes .../USB1)
try:
board = MultiWii('/dev/ttyUSB0')
except SerialException:
try:
board = MultiWii('/dev/ttyUSB1')
except SerialException:
print '\nCannot connect to the flight controller board.'
print 'The USB is unplugged. Please check connection.'
sys.exit()
return board
def __init__(self):
# Initial setpoint for the z-position of the drone
self.initial_set_z = 30.0
# Setpoint for the z-position of the drone
self.set_z = self.initial_set_z
# Current z-position of the drone according to sensor data
self.current_z = 0
# Tracks x, y velocity error and z-position error
self.error = axes_err()
# PID controller
self.pid = PID()
# Setpoint for the x-velocity of the drone
self.set_vel_x = 0
# Setpoint for the y-velocity of the drone
self.set_vel_y = 0
# Time of last heartbeat from the web interface
self.last_heartbeat = None
# Commanded yaw velocity of the drone
self.cmd_yaw_velocity = 0
# Tracks previous drone attitude
self.prev_angx = 0
self.prev_angy = 0
# Time of previous attitude measurement
self.prev_angt = None
# Angle compensation values (to account for tilt of drone)
self.mw_angle_comp_x = 0
self.mw_angle_comp_y = 0
self.mw_angle_alt_scale = 1.0
self.mw_angle_coeff = 10.0
self.angle = TwistStamped()
# Desired and current mode of the drone
self.commanded_mode = self.DISARMED
self.current_mode = self.DISARMED
# Flight controller that receives commands to control motion of the drone
self.board = MultiWii("/dev/ttyUSB0")
def __init__(self, camera, bridge):
picamera.array.PiMotionAnalysis.__init__(self, camera)
self.bridge = bridge
self.br = tf.TransformBroadcaster()
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty,
self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty,
self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel',
Mode,
queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image",
Image,
queue_size=1,
latch=True)
self.prev_img = None
self.prev_kp = None
self.prev_des = None
self.first = True
self.lr_err = ERR()
self.fb_err = ERR()
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0]
self.z = 0.075
self.smoothed_yaw = 0.0
self.yaw_observed = 0.0
self.iacc_yaw = 0.0
self.transforming = False
self.last_first_time = None
self.target_x = 0
self.target_y = 0
self.first_counter = 0
self.max_first_counter = 0
self.mode = Mode()
self.i = 1
self.board = MultiWii("/dev/ttyUSB0")
def main():
board = MultiWii("/dev/ttyUSB0")
print "Calibrate ACC... make sure we are level and still."
time.sleep(1)
board.sendCMD(0, MultiWii.ACC_CALIBRATION, [])
board.receiveDataPacket()
time.sleep(2)
print "Done!"
def main():
board = MultiWii("/dev/ttyUSB0")
# mw_data = board.getData(MultiWii.ATTITUDE)
# print mw_data
# cmds = [1500, 1500, 1000, 900]
# board.sendCMD(8, MultiWii.SET_RAW_RC, cmds)
# time.sleep(1)
# cmds = [1500, 1500, 1000, 1000]
# board.sendCMD(8, MultiWii.SET_RAW_RC, cmds)
# time.sleep(1)
# cmds = [1500, 1500, 1500, 1500]
# board.sendCMD(8, MultiWii.SET_RAW_RC, cmds)
# time.sleep(1)
print "Calibrate ACC... make sure we are level and still."
board.sendCMD(0, MultiWii.ACC_CALIBRATION, [])
board.receiveDataPacket()
time.sleep(2)
# swap these two lines to switch to the new multiwii.
# from pyMultiwii import MultiWii
from h2rMultiWii import MultiWii
from sys import stdout
from geometry_msgs.msg import Pose
from sensor_msgs.msg import Imu
from pidrone_pkg.msg import RC
import sys
import tf
import numpy as np
import time
import math
board = MultiWii("/dev/ttyACM0")
cmds = [1500, 1500, 1500, 1000, 1500, 1500, 1500, 1500]
int_pose = Pose()
int_vel = [0, 0, 0]
int_pos = [0, 0, 0]
millis = lambda: int(round(time.time() * 1000))
# rotate vector v1 by quaternion q1
def qv_mult(q1, v1):
v1 = tf.transformations.unit_vector(v1)
q2 = list(v1)
q2.append(0.0)
return tf.transformations.quaternion_multiply(
pid_terms = [yaml_data['Kp'],yaml_data['Ki'],yaml_data['Kd'],yaml_data['K']]
except yaml.YAMLError as exc:
print exc
print 'Failed to load PID terms! Exiting.'
sys.exit(1)
global throttle_pid
throttle_pid = student_PID(pid_terms[0], pid_terms[1], pid_terms[2]. [pid_terms[3]])
# stefie10: PUt all this code in a main() method. The globals
# should be fields in a class for the controller, as should all
# the callbacks.
global mw_angle_comp_x, mw_angle_comp_y, mw_angle_coeff, mw_angle_alt_scale
global current_mode
rospy.init_node('state_controller')
rospy.Subscriber("/pidrone/plane_err", axes_err, plane_callback)
board = MultiWii("/dev/ttyUSB0")
rospy.Subscriber("/pidrone/infrared", Range, ultra_callback)
rospy.Subscriber("/pidrone/set_mode_vel", Mode, mode_callback)
rospy.Subscriber("/pidrone/heartbeat", String, heartbeat_callback)
global last_heartbeat
last_heartbeat = rospy.Time.now()
signal.signal(signal.SIGINT, ctrl_c_handler)
imupub = rospy.Publisher('/pidrone/imu', Imu, queue_size=1, tcp_nodelay=False)
markerpub = rospy.Publisher('/pidrone/imu_visualization_marker', Marker, queue_size=1, tcp_nodelay=False)
markerarraypub = rospy.Publisher('/pidrone/imu_visualization_marker_array', MarkerArray, queue_size=1, tcp_nodelay=False)
statepub = rospy.Publisher('/pidrone/state', State, queue_size=1, tcp_nodelay=False)
prev_angx = 0
prev_angy = 0
class StateController(object):
# Possible modes
ARMED = 0
DISARMED = 4
FLYING = 5
def __init__(self):
# Initial setpoint for the z-position of the drone
self.initial_set_z = 30.0
# Setpoint for the z-position of the drone
self.set_z = self.initial_set_z
# Current z-position of the drone according to sensor data
self.current_z = 0
# Tracks x, y velocity error and z-position error
self.error = axes_err()
# PID controller
self.pid = PID()
# Setpoint for the x-velocity of the drone
self.set_vel_x = 0
# Setpoint for the y-velocity of the drone
self.set_vel_y = 0
# Time of last heartbeat from the web interface
self.last_heartbeat = None
# Commanded yaw velocity of the drone
self.cmd_yaw_velocity = 0
# Tracks previous drone attitude
self.prev_angx = 0
self.prev_angy = 0
# Time of previous attitude measurement
self.prev_angt = None
# Angle compensation values (to account for tilt of drone)
self.mw_angle_comp_x = 0
self.mw_angle_comp_y = 0
self.mw_angle_alt_scale = 1.0
self.mw_angle_coeff = 10.0
self.angle = TwistStamped()
# Desired and current mode of the drone
self.commanded_mode = self.DISARMED
self.current_mode = self.DISARMED
# Flight controller that receives commands to control motion of the drone
self.board = MultiWii("/dev/ttyUSB0")
def arm(self):
"""Arms the drone by sending the arm command to the flight controller"""
arm_cmd = [1500, 1500, 2000, 900]
self.board.sendCMD(8, MultiWii.SET_RAW_RC, arm_cmd)
self.board.receiveDataPacket()
rospy.sleep(1)
def disarm(self):
"""Disarms the drone by sending the disarm command to the flight controller"""
disarm_cmd = [1500, 1500, 1000, 900]
self.board.sendCMD(8, MultiWii.SET_RAW_RC, disarm_cmd)
self.board.receiveDataPacket()
rospy.sleep(1)
def idle(self):
"""Enables the drone to continue arming until commanded otherwise"""
idle_cmd = [1500, 1500, 1500, 1000]
self.board.sendCMD(8, MultiWii.SET_RAW_RC, idle_cmd)
self.board.receiveDataPacket()
def fly(self, fly_cmd):
"""Enables flight by sending the calculated flight command to the flight controller"""
self.board.sendCMD(8, MultiWii.SET_RAW_RC, fly_cmd)
self.board.receiveDataPacket()
def update_fly_velocities(self, msg):
"""Updates the desired x, y, yaw velocities and z-position"""
if msg is not None:
self.set_vel_x = msg.x_velocity
self.set_vel_y = msg.y_velocity
self.cmd_yaw_velocity = msg.yaw_velocity
new_set_z = self.set_z + msg.z_velocity
if 0.0 < new_set_z < 49.0:
self.set_z = new_set_z
def heartbeat_callback(self, msg):
"""Updates the time of the most recent heartbeat sent from the web interface"""
self.last_heartbeat = rospy.Time.now()
def infrared_callback(self, msg):
"""Updates the current z-position of the drone as measured by the infrared sensor"""
# Scales infrared sensor reading to get z accounting for tilt of the drone
self.current_z = msg.range * self.mw_angle_alt_scale * 100
self.error.z.err = self.set_z - self.current_z
def vrpn_callback(self, msg):
"""Updates the current z-position of the drone as measured by the motion capture rig"""
# Mocap uses y-axis to represent the drone's z-axis motion
self.current_z = msg.pose.position.y * 100
self.error.z.err = self.set_z - self.current_z
def plane_callback(self, msg):
"""Calculates error for x, y planar motion of the drone"""
self.error.x.err = (msg.twist.linear.x - self.mw_angle_comp_x
) * self.current_z + self.set_vel_x
self.error.y.err = (msg.twist.linear.y + self.mw_angle_comp_y
) * self.current_z + self.set_vel_y
def mode_callback(self, msg):
"""Updates commanded mode of the drone and updates targeted velocities if the drone is flying"""
self.commanded_mode = msg.mode
if self.current_mode == self.FLYING:
self.update_fly_velocities(msg)
def calc_angle_comp_values(self, mw_data):
"""Calculates angle compensation values to account for the tilt of the drone"""
new_angt = time.time()
new_angx = mw_data['angx'] / 180.0 * np.pi
new_angy = mw_data['angy'] / 180.0 * np.pi
# Passes angle of drone and correct velocity of drone
self.angle.twist.angular.x = new_angx
self.angle.twist.angular.y = new_angy
self.angle.header.stamp = rospy.get_rostime()
dt = new_angt - self.prev_angt
self.mw_angle_comp_x = np.tan(
(new_angx - self.prev_angx) * dt) * self.mw_angle_coeff
self.mw_angle_comp_y = np.tan(
(new_angy - self.prev_angy) * dt) * self.mw_angle_coeff
self.mw_angle_alt_scale = np.cos(new_angx) * np.cos(new_angy)
self.pid.throttle.mw_angle_alt_scale = self.mw_angle_alt_scale
self.prev_angx = new_angx
self.prev_angy = new_angy
self.prev_angt = new_angt
def shouldIDisarm(self):
"""Disarms the drone if it has not received a heartbeat in the last five seconds"""
if rospy.Time.now() - self.last_heartbeat > rospy.Duration.from_sec(5):
return True
else:
return False
def ctrl_c_handler(self, signal, frame):
"""Disarms the drone and exits the program if ctrl-c is pressed"""
print "Caught ctrl-c! About to Disarm!"
self.disarm()
sys.exit()
def main():
board = MW('/dev/ttyUSB0')
data = board.getData(MW.ANALOG)
print 'BATTERY VOLTAGE IS\t{}v'.format(voltage(data))
board.close()
from h2rMultiWii import MultiWii
import time
board = MultiWii("/dev/ttyUSB0")
def g():
return board.getData(MultiWii.ATTITUDE)
board.arm()
board.disarm()
print g()
if init_z is None:
init_z = vrpn_pos.pose.position.z
def ultra_callback(data):
global ultra_z
if data.range != -1:
ultra_z = data.range
if __name__ == '__main__':
rospy.init_node('velocity_flight')
rospy.Subscriber("/pidrone/est_pos", PoseStamped, vrpn_update_pos)
rospy.Subscriber("/pidrone/ultrasonic", Range, ultra_callback)
homography = Homography()
pid = PID()
first = True
board = MultiWii("/dev/ttyACM0")
while vrpn_pos is None:
if not rospy.is_shutdown():
print "No VRPN :("
time.sleep(0.01)
else:
print "Shutdown Recieved"
sys.exit()
stream = streamPi()
try:
while not rospy.is_shutdown():
curr_img = cv2.cvtColor(np.array(stream.next()), cv2.COLOR_RGB2GRAY)
if first:
homography.update_H(curr_img, curr_img)
first = False
homography.set_z(vrpn_pos.pose.position.z)
self.prev_time = time.time()
self.ang_coefficient = 1.0 # the amount that roll rate factors in
self.x_motion = 0
self.y_motion = 0
self.z_motion = 0
self.yaw_motion = 0
self.max_flow = camera_wh[0] / 16.0 * camera_wh[1] / 16.0 * 2**7
self.norm_flow_to_cm = flow_scale # the conversion from flow units to cm
self.flow_coeff = self.norm_flow_to_cm / self.max_flow
self.pub = pub
self.alpha = 0.3
if self.pub is not None:
self.velocity = axes_err()
if __name__ == '__main__':
board = MultiWii("/dev/ttyACM0")
with picamera.PiCamera(framerate=90) as camera:
with AnalyzeFlow(camera) as flow_analyzer:
rate = rospy.Rate(90)
camera.resolution = (320, 240)
flow_analyzer.setup(camera.resolution)
output = SplitFrames(width, height)
camera.start_recording('/dev/null',
format='h264',
motion_output=flow_analyzer)
prev_angx = 0
prev_angy = 0
prev_time = time.time()
while not rospy.is_shutdown():
#!/usr/bin/env python
from h2rMultiWii import MultiWii
import time
print "making board."
board = MultiWii("/dev/ttyACM0")
#print "arming."
#board.arm()
#print "armed."
#for i in range(1000):
# print "Ident: "
# print board.getData(MultiWii.IDENT)
# print "Attitude: "
# print board.getData(MultiWii.ATTITUDE)
# print "RC: "
# print board.getData(MultiWii.RC)
# print board.getData(MultiWii.RC)
# print "sending RC"
# board.sendCMD(16,MultiWii.SET_RAW_RC, [1500,1500,2000,1000, 1500, 1500, 1500, 1500])
#print "RC2"
#print board.getData(MultiWii.RC)
#print board.getData(MultiWii.STATUS)
#while True:
#print board.getData(MultiWii.RAW_IMU)
from h2rMultiWii import MultiWii as MW
def voltage(board):
return float(data['vbat'])/10.0
if __name__ == '__main__':
board = MW('/dev/ttyUSB0')
data = board.getData(MW.ANALOG)
print 'BATTERY VOLTAGE IS\t{}v'.format(voltage(board))
board.close()
raise
def plane_callback(data):
global error
error.x.err = data.x.err * ultra_z + set_vel_x
error.y.err = data.y.err * ultra_z + set_vel_y
def ctrl_c_handler(signal, frame):
print "Land Recieved"
disarm()
sys.exit()
if __name__ == '__main__':
rospy.init_node('velocity_flight_sonar')
rospy.Subscriber("/pidrone/plane_err", axes_err, plane_callback)
board = MultiWii("/dev/ttyUSB0")
rospy.Subscriber("/pidrone/infrared", Range, ultra_callback)
rospy.Subscriber("/pidrone/set_mode", Mode, mode_callback)
signal.signal(signal.SIGINT, ctrl_c_handler)
while not rospy.is_shutdown():
print current_mode, cmds
if current_mode != 4:
board.sendCMD(8, MultiWii.SET_RAW_RC, cmds)
print "Shutdown Recieved"
land()
# board.disarm()
sys.exit()
class AnalyzePhase(picamera.array.PiMotionAnalysis):
def __init__(self, camera, bridge):
picamera.array.PiMotionAnalysis.__init__(self, camera)
self.bridge = bridge
self.br = tf.TransformBroadcaster()
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty,
self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty,
self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel',
Mode,
queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image",
Image,
queue_size=1,
latch=True)
self.prev_img = None
self.prev_kp = None
self.prev_des = None
self.first = True
self.lr_err = ERR()
self.fb_err = ERR()
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0]
self.z = 0.075
self.smoothed_yaw = 0.0
self.yaw_observed = 0.0
self.iacc_yaw = 0.0
self.transforming = False
self.last_first_time = None
self.target_x = 0
self.target_y = 0
self.first_counter = 0
self.max_first_counter = 0
self.mode = Mode()
self.i = 1
self.board = MultiWii("/dev/ttyUSB0")
def write(self, data):
curr_img = np.reshape(np.fromstring(data, dtype=np.uint8),
(CAMERA_HEIGHT, CAMERA_WIDTH, 3))
curr_rostime = rospy.Time.now()
curr_time = curr_rostime.to_sec()
self.prev_img = curr_img
self.prev_time = curr_time
self.prev_rostime = curr_rostime
def range_callback(self, data):
if data.range != -1:
self.z = data.range
def reset_callback(self, data):
print "Taking picture"
cv2.imwrite("img" + str(self.i) + ".jpg", self.prev_img)
self.i += 1
mw_data = self.board.getData(MultiWii.ATTITUDE)
curr_angx = mw_data['angx'] / 180.0 * np.pi
curr_angy = mw_data['angy'] / 180.0 * np.pi
print curr_angx, curr_angy, self.z
def toggle_callback(self, data):
self.transforming = not self.transforming
print "Position hold", "enabled." if self.transforming else "disabled."
def mode_callback(self, data):
self.mode.mode = data.mode
if not self.transforming or data.mode == 4 or data.mode == 3:
print "VELOCITY"
self.pospub.publish(data)
else:
# TODO 4 is used for what ? Should the target be accumulated ?
self.target_x = data.x_velocity * 4.
self.target_y = data.y_velocity * 4.
print "POSITION", self.target_x, self.target_y
