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()
