コード例 #1
0
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!"
コード例 #2
0
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)
コード例 #3
0
                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()
コード例 #4
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()