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