def run(self):

        # continuous loop that monitors the information coming from the quadcopter
        while 1:

            # stores message from the quadcopter
            msg = self.master.recv_match(blocking=False)
            if msg:
                # stores the message type
                msg_type = msg.get_type()
                #print msg_type # debugging to check which data streams are operational

                # checks if the message contains information
                if msg.get_type() == "BAD_DATA":
                    if mavutil.all_printable(msg.data):
                        sys.stdout.write(msg.data)
                        sys.stdout.flush()
                else:
                    # add in new messages to monitor below
                    if msg_type == "ATTITUDE":

                        # publishes information to the rostopic
                        self.pub_attitude.publish(msg.roll * 180 / 3.1415, msg.pitch * 180 / 3.1415,
                                                  msg.yaw * 180 / 3.1415, msg.rollspeed, msg.pitchspeed, msg.yawspeed)

                        #upadtes imu values
                        self.imu_yaw = msg.yaw * 180 / 3.14159
                        self.imu_roll = msg.roll * 180 / 3.14159
                        self.imu_pitch = msg.pitch * 180 / 3.14159

                    # updates the channel data
                    elif msg_type == "RC_CHANNELS_RAW":
                        self.ch1 = msg.chan1_raw
                        self.ch2 = msg.chan2_raw
                        self.ch3 = msg.chan3_raw
                        self.ch4 = msg.chan4_raw
                        self.ch5 = msg.chan5_raw
                        self.ch6 = msg.chan6_raw
                        self.ch7 = msg.chan7_raw
                        self.ch8 = msg.chan8_raw


            #checks if channel 5 is below the threshold and gives back control to the transmitter
            if self.ch5 < 1400:
                if not self.transmitter_control:
                    if self.have_control:

                        # informs via terminal
                        print "You no longer have control, Transmitter has taken over control"
                        self.transmitter_control = True
                        self.have_control = False

                        # changes the control message on the ui
                        ui.control_toggle_button.setText("Take Over Control")
                        self.disconnect_pwm_channels()
            else:
                # turns off transmitter control so that the program is able to take over when requested
                self.transmitter_control = False
            time.sleep(0.01)
Beispiel #2
0
def mainloop():
    rospy.init_node('roscopter')
    while not rospy.is_shutdown():
        rospy.sleep(0.001)
        msg = master.recv_match(blocking=False)
        if not msg:
            continue
        #print msg.get_type()
        if msg.get_type() == "BAD_DATA":
            if mavutil.all_printable(msg.data):
                sys.stdout.write(msg.data)
                sys.stdout.flush()
        else:
            msg_type = msg.get_type()
            if msg_type == "RC_CHANNELS_RAW":
                pub_rc.publish([
                    msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw,
                    msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw
                ])
            if msg_type == "HEARTBEAT":
                pub_state.publish(
                    msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED,
                    msg.base_mode
                    & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED,
                    mavutil.mode_string_v10(msg))
            if msg_type == "VFR_HUD":
                pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading,
                                    msg.throttle, msg.alt, msg.climb)

            if msg_type == "GPS_RAW_INT":
                fix = NavSatStatus.STATUS_NO_FIX
                if msg.fix_type >= 3:
                    fix = NavSatStatus.STATUS_FIX
                pub_gps.publish(
                    NavSatFix(
                        latitude=msg.lat / 1e07,
                        longitude=msg.lon / 1e07,
                        altitude=msg.alt / 1e03,
                        status=NavSatStatus(status=fix,
                                            service=NavSatStatus.SERVICE_GPS)))
            #pub.publish(String("MSG: %s"%msg))
            if msg_type == "ATTITUDE":
                pub_attitude.publish(msg.roll, msg.pitch, msg.yaw,
                                     msg.rollspeed, msg.pitchspeed,
                                     msg.yawspeed)

            if msg_type == "LOCAL_POSITION_NED":
                print "Local Pos: (%f %f %f) , (%f %f %f)" % (
                    msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz)

            if msg_type == "RAW_IMU":
                pub_raw_imu.publish(Header(), msg.time_usec, msg.xacc,
                                    msg.yacc, msg.zacc, msg.xgyro, msg.ygyro,
                                    msg.zgyro, msg.xmag, msg.ymag, msg.zmag)
def mainloop():
    rospy.init_node('roscopter')
    while not rospy.is_shutdown():
        rospy.sleep(0.001)
        msg = master.recv_match(blocking=False)
        if not msg:
            continue
        #print msg.get_type()
        if msg.get_type() == "BAD_DATA":
            if mavutil.all_printable(msg.data):
                sys.stdout.write(msg.data)
                sys.stdout.flush()
        else: 
            msg_type = msg.get_type()
            if msg_type == "RC_CHANNELS_RAW" :
                pub_rc.publish([msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw, msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw]) 
            if msg_type == "HEARTBEAT":
                pub_state.publish(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED, 
                                  msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED, 
                                  mavutil.mode_string_v10(msg))
            if msg_type == "VFR_HUD":
                pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading, msg.throttle, msg.alt, msg.climb)

            if msg_type == "GPS_RAW_INT":
                fix = NavSatStatus.STATUS_NO_FIX
                if msg.fix_type >=3:
                    fix=NavSatStatus.STATUS_FIX
                pub_gps.publish(NavSatFix(latitude = msg.lat/1e07,
                                          longitude = msg.lon/1e07,
                                          altitude = msg.alt/1e03,
                                          status = NavSatStatus(status=fix, service = NavSatStatus.SERVICE_GPS) 
                                          ))
            #pub.publish(String("MSG: %s"%msg))
            if msg_type == "ATTITUDE" :
                pub_attitude.publish(msg.roll, msg.pitch, msg.yaw, msg.rollspeed, msg.pitchspeed, msg.yawspeed)
                px4_time[0] = msg.time_boot_ms  # get the time from px4
                local_time[0] = time.time()     # get the local start time

            if msg_type == "LOCAL_POSITION_NED" :
                print "Local Pos: (%f %f %f) , (%f %f %f)" %(msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz)

            if msg_type == "RAW_IMU" :
                pub_raw_imu.publish (Header(), msg.time_usec, 
                                     msg.xacc, msg.yacc, msg.zacc, 
                                     msg.xgyro, msg.ygyro, msg.zgyro,
                                     msg.xmag, msg.ymag, msg.zmag)
Beispiel #4
0
def mainloop():
    print("Waiting")
    global hz_counter
    hz_counter=1;
    trate = 1.0/rate
    first_time_imu=True
    first_time_pos=True
    earliercmdtime = 0
    ## Set home values from Vicon/Kinect data ##
    rospy.sleep(.1)
    set_home()
    
    ## Integration and differentiation error initialized ##
    I_ex=0.0; I_ey=0.0; I_ez=0.0
    D_ex=0.0; D_ey=0.0; D_ez=0.0
    new_err_x=0.0; new_err_y=0.0; new_err_z=0.0;

    ## IMU values initialized ##
    imu_roll=0.0; imu_pitch=0.0; first_yaw=0.0;
    imu_yaw=0.0; first_roll=0.0; first_pitch=0.0

    ## Position values initialised ##
    pos_x=0.0; pos_y=0.0; pos_z=0.0
  
    
    while not rospy.is_shutdown(): # start loop
       
        #continue
        if float(time.time())-trate >= earliercmdtime:
                earliercmdtime = time.time()
               
                hz_counter =1;
		if connect_APM:      # enable these lines if you want to obtain the data from the arducopter on-the-fly
	#            msg = master.recv_match(blocking=False)
	#            if not msg:
	#                continue
		    if 0:#msg:
			    if msg.get_type() == "BAD_DATA":
				if mavutil.all_printable(msg.data):
				    sys.stdout.write(msg.data)
				    sys.stdout.flush()
			    else: 
				msg_type = msg.get_type()
				if msg_type == "ATTITUDE" :
				    pub_attitude.publish(msg.roll*180/3.1415, msg.pitch*180/3.1415, msg.yaw*180/3.1415, msg.rollspeed, msg.pitchspeed, msg.yawspeed)
		    	            if first_time_imu:
					first_time_imu=False
					first_yaw = msg.yaw
					first_roll = msg.roll
					first_pitch = msg.pitch
					imu_yaw = msg.yaw
					imu_roll = msg.roll
					imu_pitch = msg.pitch
				    else:
					imu_yaw = msg.yaw
					imu_roll = msg.roll
					imu_pitch = msg.pitch	   

		
                ## Update position values ##
		(old_pos_x,old_pos_y,old_pos_z,pos_x,pos_y,pos_z) = update_pos_values(pos_x,pos_y,pos_z,vicon_x,vicon_y,vicon_z,first_time_pos)	
		(err_x,err_y,err_z) = calc_pos_error(home_x,home_y,home_z,pos_x,pos_y,pos_z)   # home - pos
		(d_x, d_y, d_z) = calc_pos_error(old_pos_x,old_pos_y,old_pos_z,pos_x,pos_y,pos_z) 

		## Toggle for first time through loop ##
		if first_time_pos:
		    first_time_pos=False

		## PID controller  (roll+pitch and thrust, yaw later) ##
		I_ex = I_ex + err_x*trate
		I_ey = I_ey + err_y*trate
		I_ez = I_ez + err_z*trate

		V_x = d_x*trate	# units of m/s
		V_y = d_y*trate	# units of m/s
		V_z = d_z*trate	# units of m/s

		## Cap the integration error as well after certain value ##
		if (I_ex) > I_ex_MAX:
		    I_ex = I_ex_MAX #* I_ex / abs(I_ex)

		if (I_ex) < -I_ex_MAX:
		    I_ex = -I_ex_MAX #* I_ex / abs(I_ex)

		if (I_ey) > I_ey_MAX:
		    I_ey = I_ey_MAX #* I_ey / abs(I_ey)

		if (I_ey) < -I_ey_MAX:
		    I_ey = -I_ey_MAX #* I_ey / abs(I_ey)

		if (I_ez) > I_ez_MAX:
		    I_ez = I_ez_MAX #* I_ez / abs(I_ez)

		if (I_ez) < I_ez_MAX:
		    I_ez = -I_ez_MAX #* I_ez / abs(I_ez)
	     
		p_pitch = pitch_P * err_x
		i_pitch = pitch_I * I_ex
		d_pitch = pitch_D * V_x

		p_roll =  roll_P * err_y
		i_roll =  roll_I * I_ey
		d_roll =  roll_D * V_y

		p_th = thrust_P * err_z
		i_th = thrust_I * (I_ez)
		d_th = thrust_D * V_z

		##  Calculate PID values to send
		sendroll =  (p_roll + i_roll + d_roll) #

		
		sendpitch = (p_pitch + i_pitch + d_pitch) 
		###### neg pitch     === hexacopter will move forward
		###### pos pitch     === hexacopter will move backward
		###### neg roll     === hexacopter will move left
		###### pos roll     === hexacopter will move right

	        sendthrust = p_th + i_th + d_th
                sendyaw = yaw_P*(vicon_yaw  - home_yaw) #0 #-imu_yaw 

		## Cap the roll/pitch/yaw/thrust values ##
		if (sendroll) > roll_MAX:
		    sendroll = roll_MAX #* sendroll / abs(sendroll)

		if (sendroll) < -roll_MAX:
		    sendroll = -roll_MAX #* sendroll / abs(sendroll)
	 

		if (sendpitch) > pitch_MAX:
		    sendpitch = pitch_MAX #* sendpitch / abs(sendpitch)
		   
		if (sendpitch) < -pitch_MAX:
		    sendpitch = -pitch_MAX #* sendpitch / abs(sendpitch)



	  	if sendthrust > thrust_MAX:
		    sendthrust = thrust_MAX
		if sendthrust < 0:
		    sendthrust = 0

               
		## Send values to APM ##
		if connect_APM:
		    master.mav.set_roll_pitch_yaw_thrust_send(master.target_system, master.target_component, 
		                			      sendroll, sendpitch, sendyaw, sendthrust)

                send_odometry.transform.translation.x = sendroll * 180/3.14159
                send_odometry.transform.translation.y = sendpitch * 180/3.14159
                send_odometry.transform.translation.z = sendyaw * 180/3.14159
		## Print some output to display for debugging ##
#		stdscr.addstr(0, 0, "Position controller is running...")
#		if connect_APM:
#		    stdscr.addstr(1, 0, "Connection to APM established...")
#		else:
#		    stdscr.addstr(1, 0, "******Warning: No connection to APM!*************")

#		stdscr.addstr(3, 0, "The gains are:")
#		stdscr.addstr(4, 0, "              P       I       D")
#		stdscr.addstr(5, 0, "Roll          %.3f   %.3f   %.3f" % (roll_P, roll_I, roll_D))
#		stdscr.addstr(6, 0, "Pitch         %.3f   %.3f   %.3f" % (pitch_P, pitch_I, pitch_D))
#		stdscr.addstr(7, 0, "Yaw           %.3f   %.3f   %.3f" % (yaw_P, yaw_I, yaw_D))
#		stdscr.addstr(8, 0, "Thrust        %.3f   %.3f   %.3f" % (thrust_P, thrust_I, thrust_D))
#		stdscr.addstr(10, 0, "Home position is:    (%+.3f,%+.3f,%+.3f) m" % (home_x, home_y, home_z))
#		stdscr.addstr(11, 0, "Current position is: (%+.3f,%+.3f,%+.3f) m" % (pos_x, pos_y, pos_z))
#		stdscr.addstr(12, 0, "Position error is:   (%+.3f,%+.3f,%+.3f) m" % (err_x, err_y, err_z))
	#	if connect_APM:
	#	    stdscr.addstr(14, 0, "The IMU values are  (%+.3f,%+.3f,%+.3f) deg     " % (imu_roll/deg2pi, imu_pitch/deg2pi, imu_yaw/deg2pi))
	#	else:
	#	    stdscr.addstr(14, 0, "The IMU values are not available!")
#		stdscr.addstr(15, 0, "The Vicon values are  (%+.3f,%+.3f,%+.3f) deg      " % (vicon_roll/deg2pi,vicon_pitch/deg2pi,vicon_yaw/deg2pi))
#		stdscr.addstr(17, 0, "Sending control commands to APM:")
#		stdscr.addstr(18, 0, "Roll (deg)    Pitch (deg)    Yaw (deg)    Thrust (PWM)")
#		stdscr.addstr(19, 0, "%+.3f         %+.3f         %+.3f         %+.3f" % (sendroll/deg2pi, sendpitch/deg2pi, sendyaw/deg2pi, sendthrust))
#		stdscr.addstr(20, 0, "      \n")
#		stdscr.refresh()
 #       else:
 #               hz_counter =0; #stdscr.addstr(22, 0, " The rate of command is not correct")
                pub_odom .publish(send_odometry)