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)
prev_angy = new_angy prev_angt = new_angt # stefie10: Make sure this still exists and works in the refactor! if shouldILand(): print "Landing because a safety check failed." break except: print "BOARD ERRORS!!!!!!!!!!!!!!" print "BOARD ERRORS!!!!!!!!!!!!!!" print "BOARD ERRORS!!!!!!!!!!!!!!" print "BOARD ERRORS!!!!!!!!!!!!!!" print "BOARD ERRORS!!!!!!!!!!!!!!" print "BOARD ERRORS!!!!!!!!!!!!!!" sys.exit() # stefie10: None of your code will be called after # sys.exit, so this is nonsensical. it is almost # always better to raise an exception or break out of # the loop to close the board, to try to do a clean # exit. board.close() board.sendCMD(8, MultiWii.SET_RAW_RC, cmds) board.receiveDataPacket() time.sleep(0.01) print "Shutdown Recieved" board.disarm()
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()