コード例 #1
0
ファイル: roscopter.py プロジェクト: skchandra/roscopter
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)
コード例 #2
0
def show_messages(m):
    """show incoming mavlink messages"""
    while True:
        msg = m.recv_match(blocking=True)
        if not msg:
            return
        if msg.get_type() == "BAD_DATA":
            if mavutil.all_printable(msg.data):
                sys.stdout.write(msg.data)
                sys.stdout.flush()
        else:
            print(msg)
コード例 #3
0
def show_messages(m):
    '''show incoming mavlink messages'''
    while True:
        msg = m.recv_match(blocking=True)
        if not msg:
            return
        if msg.get_type() == "BAD_DATA":
            if mavutil.all_printable(msg.data):
                sys.stdout.write(msg.data)
                sys.stdout.flush()
        else:
            print(msg)
コード例 #4
0
	def get_messages(m):
	    '''show incoming mavlink messages'''
	    while True:
		msg = m.recv_match(blocking=True)
		if not msg:
		    return 'NULL'
		if msg.get_type() == "BAD_DATA":
		    if mavutil.all_printable(msg.data):
			sys.stdout.write(msg.data)
			sys.stdout.flush()
		else:
		    return msg                    
コード例 #5
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)
                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)
コード例 #6
0
ファイル: Mavlink_Node.py プロジェクト: dgitz/icarus_rover_rc
def mainloop():
    rospy.init_node('Mavlink_Node')
    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":
                #print "Got GPS Message. SV: %d eph: %d epv: %d" %(msg.satellites_visible,msg.eph,msg.epv)
                pub_gps_state.publish(msg.satellites_visible,msg.eph,msg.epv,msg.lat/1e07,msg.lon/1e07,msg.alt/1e03,msg.fix_type)
                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)
コード例 #7
0
def update_device(m):
	global WaypointCount
	global Current_Yaw_rad
	global Current_Pitch_rad
	global Current_Roll_rad
	global fc_badpacket_counter
	global first_attitude_packet
	global Initial_Yaw_rad
	global my_MissionItems
	if (m.protocol == "MAVLINK") or (m.protocol == "APM_MAVLINK"): 
		msg = m.device.recv_match(blocking=False)
		#pub_data_from_fc.publish(str(msg.get_type()))
		if m.device == device_remote.device:
			if msg:
				#print msg
				device_remote.printtext(str(msg))
		if m.device == device_fc.device:
			print msg
		#if m == device_gcs.device:
		#	if msg:
		#		print msg
		if msg:
			if msg.get_type() == "BAD_DATA":            
				fc_badpacket_counter = fc_badpacket_counter + 1
			    	if mavutil.all_printable(msg.data):
					sys.stdout.write(msg.data)
					sys.stdout.flush()			  
			elif msg.get_type() == "HEARTBEAT": 
				if m.device == device_gcs.device:
					device_gcs.appenderror(calc_errorcode(SYSTEM_GCS,ERRORTYPE_NOERROR,SEVERITY_INFORMATION,MESSAGE_NOERROR))
					#device_gcs.display_errors()
					#print "device_gcs.device: {}".format(msg)
				if m.device == device_fc.device:
					device_gcs.appenderror(calc_errorcode(SYSTEM_GCS,ERRORTYPE_NOERROR,SEVERITY_INFORMATION,MESSAGE_NOERROR))
					device_fc.state = msg.system_status
					device_fc.mode = msg.base_mode
					#print "FC: {}".format(msg)
				
					#fc_state = msg.system_status
				if m.device == device_remote.device:
					device_remote.printtext("Remote: {}".format(msg))
					device_remote.appenderror(calc_errorcode(SYSTEM_REMOTE,ERRORTYPE_NOERROR,SEVERITY_INFORMATION,MESSAGE_NOERROR))			
			elif msg.get_type() == "ATTITUDE" :
				if first_attitude_packet:
					Initial_Yaw_rad = msg.yaw
					first_attitude_packet = False
				Current_Pitch_rad = msg.pitch
				Current_Yaw_rad = msg.yaw
				Current_Roll_rad = msg.roll
				pub_attitude.publish(msg.roll, msg.pitch, msg.yaw)
			elif msg.get_type() == "STATUSTEXT":
				dumb = 1
				#print msg
			elif msg.get_type() == "MISSION_CURRENT":
				if m == device_fc.device:
					dumb = 1
					#print "FC: {}".format(msg)			

			elif msg.get_type() == "MISSION_ITEM":
				if m.device == device_gcs.device:
					device_gcs.printtext(str(msg))
					waypoint_rcv_fsm(m,"NewWP",msg)
				elif m.device == device_fc.device:
					device_fc.printtext(str(msg))

			elif msg.get_type() == "MISSION_ACK":
				if m.device == device_gcs.device:
					waypoint_rcv_fsm(m,"Finish",msg)
				
				if m.device == device_fc.device:
					device_fc.printtext(str(msg))
					send_mission_request_list(device_fc.device)
			elif msg.get_type() == "GPS_RAW_INT":
				dumb = 1
			elif msg.get_type() == "RADIO":
				dumb = 1
			elif msg.get_type() == "AHRS":
				dumb = 1
			elif msg.get_type() == "HWSTATUS":
				dumb = 1
			elif msg.get_type() == "SYS_STATUS":
				dumb = 1
				#if m == fc:
				#	print msg
			elif msg.get_type() == "NAV_CONTROLLER_OUTPUT":
				dumb = 1
				#if m == fc:
				#	print msg
			elif msg.get_type() == "MEMINFO":
				dumb = 1
			elif msg.get_type() == "REQUEST_DATA_STREAM":
				if m.device == device_remote.device:
					device_remote.mav_data_streams.append(msg.req_stream_id)
					#device_remote.display_streams()
					device_remote.appenderror(calc_errorcode(SYSTEM_REMOTE,ERRORTYPE_NOERROR,SEVERITY_INFORMATION,MESSAGE_NOERROR))

			elif msg.get_type() == "GLOBAL_POSITION_INT":
				dumb = 1
			elif msg.get_type() == "RC_CHANNELS_SCALED":
				dumb = 1
			elif msg.get_type() == "SERVO_OUTPUT_RAW":
				dumb = 1
			elif msg.get_type() == "RC_CHANNELS_RAW":
				dumb = 1
			elif msg.get_type() == "VFR_HUD":
				dumb = 1
			elif msg.get_type() == "RAW_IMU":
				dumb = 1
			elif msg.get_type() == "SCALED_PRESSURE":
				dumb = 1
			elif msg.get_type() == "SENSOR_OFFSETS":
				dumb = 1
			elif msg.get_type() == "MISSION_REQUEST_LIST":
				for wp in my_MissionItems:
					wp.display()
				if m.device == device_gcs.device:
					device_pc.printtext("Going to send these waypoints: ")
				
					device_pc.printtext(str(msg))
					try:
						if (len(my_MissionItems)>0):
						
							waypoint_xmit_fsm(device_gcs.device,"Start",msg)
						else:
							waypoint_xmit_fsm(device_gcs.device,"Empty",msg)
					
					except NameError:
						dumb = 1
						#waypoint_xmit_fsm(device_gcs.device,"Empty",msg)
						#print "ERRORORRR"
				elif m.device == device_fc.device:
					device_fc.printtext(str(msg))
			elif msg.get_type() == "MISSION_REQUEST":
				if m.device == device_gcs.device:
					device_gcs.printtext(str(msg))
					waypoint_xmit_fsm(device_gcs.device,"NewWP",msg)
				elif m.device == device_fc.device:
					waypoint_xmit_fsm(device_fc.device,"NewWP",msg)
				
			elif msg.get_type() == "MISSION_CLEAR_ALL":
				if m.device == device_gcs.device:
					device_gcs.device.target_system = 0
					device_gcs.device.target_component = 0
					device_gcs.printtext(str(msg))
					my_MissionItems = []
					send_ack(device_gcs.device)
				elif m.device == device_fc.device:
					device_fc.printtext(str(msg))	
			elif msg.get_type() == "MISSION_COUNT":
				if m.device == device_gcs.device:
					device_gcs.printtext(str(msg))
					device_gcs.device.target_system = 0
					device_gcs.device.target_component = 0
					WaypointCount = int(msg.count)
					my_MissionItems = []		
				
					waypoint_rcv_fsm(device_gcs.device,"Start",msg)
				elif m.device == device_fc.device:
					device_fc.printtext(str(msg))
			elif msg.get_type() == "COMMAND_LONG":
				if m.device == device_gcs.device:
					print msg
				
					device_gcs.printtext(str(msg))
					if msg.command == mavlink.MAV_CMD_NAV_LAND:
						if device_pc.isarmed():
							device_gcs.printtext("MAV_CMD_NAV_LAND")
							device_pc.changecommand(msg.command)
							print "Need more code here"
						else:
							tempstr = "not even armed yet"
							send_text(device_gcs.device,tempstr)
							print tempstr
					
					elif msg.command == mavlink.MAV_CMD_NAV_TAKEOFF:
						if device_pc.isarmed():
							device_gcs.printtext("MAV_CMD_NAV_TAKEOFF")
							device_pc.changecommand(msg.command)
							#statemode_fsm(device_fc.device,mavlink.MAV_STATE_ACTIVE,mavlink.MAV_MODE_AUTO_ARMED)
						else:
							tempstr =  "Not even armed yet."
							send_text(device_gcs.device,tempstr)
							print tempstr
						
					elif msg.command == mavlink.MAV_CMD_OVERRIDE_GOTO:
						device_gcs.printtext("MAV_CMD_OVERRIDE_GOTO")
						if (str(msg.confirmation) == "1") and(str(msg.param1) == "0.0") and (str(msg.param2) == "2.0"):						
							if device_pc.isarmed():
								#device_pc.changecommand(msg.command)
								print "Need more code here"
								print "PAUSE"
							else:
								tempstr =  "Not even armed yet."
								send_text(device_gcs.device,tempstr)
								print tempstr						

						if (str(msg.confirmation) == "1") and (str(msg.param1) == "1.0") and (str(msg.param2) == "2.0"):	
							if device_pc.isarmed():
								print "Need mroe code here"
							else:
								tempstr =  "Not even armed yet."
								send_text(device_gcs.device,tempstr)
								print tempstr
				if m.device == device_fc.device:
					device_fc.printtext(str(msg))
			elif msg.get_type() == "SET_MODE":
				if m.device == device_gcs.device:
					print msg
					device_gcs.printtext(str(msg))
					print msg.base_mode
					print device_pc.isarmed()
					if str(msg.base_mode) == "132":
						if device_pc.isarmed() == False:
							print "trying to arm"
							device_pc.armdisarm(True)
							send_ack(device_gcs.device)
						else:
							device_pc.armdisarm(False)
					elif str(msg.base_mode) == "4":
						if device_pc.isarmed() == True:
							print "trying to disarm"
							device_pc.armdisarm(False)
							send_ack(device_gcs.device)
					else:
						if device_pc.isarmed() == True:
							device_pc.armdisarm(False)
						device_pc.changemode(msg.base_mode)
						if device_fc.enabled == True:
							device_fc.changemode(msg.base_mode)
				if m.device == device_remote.device:
					if device_pc.isarmed() == True:
						device_pc.armdisarm(False)
					if msg.custom_mode == APM_STABILIZE:
						device_pc.changemode(mavlink.MAV_MODE_STABILIZE_DISARMED)
						#device_pc.armdisarm(True)
					elif msg.custom_mode == APM_AUTO:
						device_pc.changemode(mavlink.MAV_MODE_AUTO_DISARMED)
					elif msg.custom_mode == APM_LAND:
						device_pc.changecommand(mavlink.MAV_CMD_NAV_LAND)
					elif msg.custom_mode == APM_LOITER:
						device_pc.changecommand(mavlink.MAV_CMD_NAV_LOITER_UNLIM)
					elif msg.custom_mode == APM_GUIDED:
						device_pc.changemode(mavlink.MAV_MODE_GUIDED_DISARMED)
			else:
				if m.device == device_gcs.device:
					device_gcs.printtext(str(msg))
				if m.device == device_remote.device:
					device_remote.printtext(str(msg))
				if m.device == device_fc.device:
					dumb = 1
					#print msg
				
			if msg == "STATUSTEXT":
				if m.device == device_fc.device:
					if (msg.find("flight plan received")>0):
						dumb = 1
						device_fc.printtext(str(msg))
				if m.device == device_fc.device:
					dumb = 1
					device_fc.printtext(str(msg))
	if m.protocol == "ICARUS":
		try:
			msg = device_mc.device.readline()
			
			#device_mc.printtext(msg)
			if msg[0] == "$" and msg[len(msg)-3]=="*":
				#print msg
				msg = msg[1:len(msg)-3]		
				contents = msg.split(",")
				if contents[0] == "STA":
					if contents[1] == "STATE":
						device_mc.state = int(contents[2])
						if device_mc.state == mavlink.MAV_STATE_ACTIVE:
							device_mc.armed = True
						else:
							device_mc.armed = False
						#print device_mc.state
					elif contents[1] == "MODE":
						device_mc.cur_mode = int(contents[2])
						#print device_mc.mode
						
				elif contents[0] == "ERR":
					device_mc.appenderror(contents[1])
				elif contents[0] == "CAL":
					dumb = 1
				elif contents[0] == "CON":
					dumb = 1
				elif contents[0] == "INF":
					dumb = 1
				elif contents[0] == "MOTOR":
					dumb = 1
				elif contents[0] == "NET":
					dumb = 1
				elif contents[0] == "SEN":
					dumb = 1
				elif contents[0] == "SRV":
					dumb = 1
				elif contents[0] == "PWMIN":
					print msg
				else:
					print msg
		except:
			device_mc.appenderror(calc_errorcode(system=SYSTEM_FLYER_MC,errortype=ERRORTYPE_COMMUNICATION,severity=SEVERITY_CAUTION,message=MESSAGE_DROPPEDPACKETS))
コード例 #8
0
    def next_message(self):
        '''called as each msg is ready'''
        
        msg = self.msg
        if msg is None:
            self.paused = True

        if self.paused:
            self.root.after(100, self.next_message)
            return

        speed = float(self.playback.get())
        timestamp = getattr(msg, '_timestamp')

        now = time.strftime("%H:%M:%S", time.localtime(timestamp))
        self.clock.configure(text=now)

        if speed == 0.0:
            self.root.after(200, self.next_message)
        else:
            self.root.after(int(1000*(timestamp - self.last_timestamp) / speed), self.next_message)
        self.last_timestamp = timestamp

        while True:
            self.msg = self.mlog.recv_match(condition=opts.condition)
            if self.msg is None and self.mlog.f.tell() > self.filesize - 10:
                self.paused = True
                return
            if self.msg is not None and self.msg.get_type() != "BAD_DATA":
                break
        
        pos = float(self.mlog.f.tell()) / self.filesize
        self.slider.set(pos)
        self.filepos = self.slider.get()

        if msg.get_type() != "BAD_DATA":
            for m in self.mout:
                m.write(struct.pack('>Q', timestamp*1.0e6))
                m.write(msg.get_msgbuf().tostring())

        if msg.get_type() == "GPS_RAW":
            self.fdm.set('latitude', msg.lat, units='degrees')
            self.fdm.set('longitude', msg.lon, units='degrees')
            if opts.gpsalt:
                self.fdm.set('altitude', msg.alt, units='meters')

        if msg.get_type() == "VFR_HUD":
            if not opts.gpsalt:
                self.fdm.set('altitude', msg.alt, units='meters')
            self.fdm.set('num_engines', 1)
            self.fdm.set('vcas', msg.airspeed, units='mps')

        if msg.get_type() == "ATTITUDE":
            self.fdm.set('phi', msg.roll, units='radians')
            self.fdm.set('theta', msg.pitch, units='radians')
            self.fdm.set('psi', msg.yaw, units='radians')
            self.fdm.set('phidot', msg.rollspeed, units='rps')
            self.fdm.set('thetadot', msg.pitchspeed, units='rps')
            self.fdm.set('psidot', msg.yawspeed, units='rps')

        if msg.get_type() == "RC_CHANNELS_SCALED":
            self.fdm.set("right_aileron", msg.chan1_scaled*0.0001)
            self.fdm.set("left_aileron", -msg.chan1_scaled*0.0001)
            self.fdm.set("rudder",        msg.chan4_scaled*0.0001)
            self.fdm.set("elevator",      msg.chan2_scaled*0.0001)
            self.fdm.set('rpm',           msg.chan3_scaled*0.01)

        if msg.get_type() == 'STATUSTEXT':
            print("APM: %s" % msg.text)

        if msg.get_type() == 'SYS_STATUS':
            self.flightmode.configure(text=self.mlog.flightmode)

        if msg.get_type() == "BAD_DATA":
            if mavutil.all_printable(msg.data):
                sys.stdout.write(msg.data)
                sys.stdout.flush()

        if self.fdm.get('latitude') != 0:
            for f in self.fgout:
                f.write(self.fdm.pack())
コード例 #9
0
ファイル: arproxy.py プロジェクト: satriabismiansyah/ar2mav
    def handle_mavlink(self):
        """Main loop for handling all mavlink communication

           Incoming mavlink messages may be from drone, host, or
           QGroundControl. This method dispatches and processes
           messages separately in each case.

           This runs inside its own thread.
        """

        #**********************************************************************
        #   Process mavlink messages (forever)
        #**********************************************************************
        while True:

            #******************************************************************
            #   Wait (forever) until we receive a mavlink message
            #******************************************************************
            msg = None
            try:
                msg = self.connection.recv_match(blocking=True)
            except socket.error as e:
                print("[AR2MAV]%s: caught socket error while trying to"
                        " receive mavlink message: %s" %
                        (self.name, str(e)) )
                traceback.print_exc()
                time.sleep(1)

            #******************************************************************
            # If recv_match returns without a message, then try again
            # This shouldn't really happen because we've asked to block until
            # a message is available
            #******************************************************************
            if msg is None:
                print("[AR2MAV]%s: mavlink unexpectedly returned nothing." %
                      self.name)

            #******************************************************************
            # Write out bad data (shouldn't happen either)
            #******************************************************************
            elif msg.get_type() == "BAD_DATA":
                print("[AR2MAV]%s: Received bad data -- ignoring." %
                      self.name)
                if mavutil.all_printable(msg.data):
                    sys.stdout.write(msg.data)
                    sys.stdout.flush()

            #******************************************************************
            # Process data from drone
            #******************************************************************
            elif self.connection.last_address == self.drone:
                try:
                    self.process_from_drone(msg)
                except socket.error as e:
                    print("[AR2MAV]%s: caught socket error while processing"
                            " last message from drone: %s" %
                            (self.name, str(e)) )
                    traceback.print_exc()

            #******************************************************************
            # Process data from host
            #******************************************************************
            elif self.connection.last_address == self.host:
                try:
                    self.process_from_host(msg)
                except socket.error as e:
                    print("[AR2MAV]%s: caught socket error while processing"
                            " last message from host: %s" %
                            (self.name, str(e)) )
                    traceback.print_exc()

            #******************************************************************
            # Process data from QGroundControl (we should be able to treat
            # this just like data from the host)
            #******************************************************************
            elif self.qgc and \
                 self.connection.last_address == (self.host[0], QGC_PORT):
                try:
                    self.process_from_host(msg)
                except socket.error as e:
                    print("[AR2MAV]%s: caught socket error while processing"
                            " last message from QGroundControl: %s" %
                            (self.name, str(e)) )
                    traceback.print_exc()

            #******************************************************************
            # Ignore data from anyone else
            #******************************************************************
            else:
                if self.verbose > 0:
                    print("[AR2MAV]%s: Unregistered AUV with IP(MAV): %s" %
                          (self.name, self.connection.last_address[0]))
コード例 #10
0
ファイル: driver.py プロジェクト: tuloski/SherpaHighLevel
def mainloop():
    global gps_msg, autonomous_enable
    rospy.init_node('casy_rover')
    
    if opts.enable_watchdog:
        rospy.Timer(rospy.Duration(1/opts.watchdog_rate), watchdog_timer_cb)

    # SEND IF YOU DESIRE A LIST OF ALL PARAMS (TODO: Publish params to a topic)
    #master.mav.param_request_list_send(master.target_system, master.target_component)

    # Permette di settare un parametro come in mission planner    
    #master.param_set_send('FS_ACTION', 0, 2)

    # Riceve una lista di tutti i parametri disponibili sotto forma di messaggi PARAM_VALUE
    #master.param_fetch_all()
    
    r = rospy.Rate(opts.rate)
    while not rospy.is_shutdown():    
        r.sleep()
        msg = master.recv_match(blocking=False)
        if not msg:
            continue
        
        # Parse incoming message
        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" :
            
                # If using Autonomous Control safety switch, use specified channel
                # to enable or disable autonomous control.  Autonomous control
                # allows mode, rc, and waypoint controls.
                if (opts.enable_autonomous_safety_switch):
                    if (msg.chan6_raw >= 1500):
                        autonomous_enable = True
                    elif (msg.chan6_raw < 1500):
                        autonomous_enable = False
                        # Give control back to controller
                        master.mav.rc_channels_override_send(master.target_system,
                             master.target_component, 0, 0, 0, 0, 0, 0, 0, 0)                  
            
                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]) 
            elif 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))

                state_msg.armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
                state_msg.mode = mavutil.mode_string_v10(msg)
            elif msg_type == "VFR_HUD":
                pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading, msg.throttle, msg.alt, msg.climb)

            elif msg_type == "GPS_RAW_INT":
                fix = NavSatStatus.STATUS_NO_FIX
                if msg.fix_type >=3:
                    fix=NavSatStatus.STATUS_FIX

                header = Header()
                header.frame_id = 'base_link'# '/gps'
                header.stamp = rospy.Time.now()

                #rospy.loginfo("Hdop is %d", msg.eph)
                #rospy.loginfo("Vdop is %d", msg.epv)

                sigma = math.sqrt((3.04 * msg.eph**2)**2 + 3.57**2)
                position_covariance = [0] * 9
                position_covariance[0] = sigma #9999
                position_covariance[4] = sigma #9999
                position_covariance[8] = sigma #9999

                pub_gps.publish(NavSatFix(header = header,
                                          latitude = msg.lat/1e07,
                                          longitude = msg.lon/1e07,
                                          altitude = msg.alt/1e03,
                                          position_covariance=position_covariance,
                                          position_covariance_type=NavSatFix.COVARIANCE_TYPE_APPROXIMATED,
                                          status = NavSatStatus(status=fix, service = NavSatStatus.SERVICE_GPS) 
                                          ))

                gps_msg.latitude = msg.lat
                gps_msg.longitude = msg.lon

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

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

            elif msg_type == "SYS_STATUS":
                status_msg = casy_rover.msg.Status()
                status_msg.header.stamp = rospy.Time.now()
                status_msg.battery_voltage = msg.voltage_battery
                status_msg.battery_current = msg.current_battery
                status_msg.battery_remaining = msg.battery_remaining
                status_msg.sensors_enabled = msg.onboard_control_sensors_enabled
                pub_status.publish(status_msg)
                
            elif msg_type == "GLOBAL_POSITION_INT":
                header = Header()
                header.stamp = rospy.Time.now()
                filtered_pos_msg.header = header
                filtered_pos_msg.latitude = msg.lat
                filtered_pos_msg.longitude = msg.lon
                filtered_pos_msg.altitude = msg.alt
                filtered_pos_msg.relative_altitude = msg.relative_alt
                filtered_pos_msg.ground_x_speed = msg.vx
                filtered_pos_msg.ground_y_speed = msg.vy
                filtered_pos_msg.ground_z_speed = msg.vz
                filtered_pos_msg.heading = msg.hdg
                pub_filtered_pos.publish(filtered_pos_msg)                                         
        
            elif msg_type == "COMMAND_ACK":
                rospy.loginfo ("COMMAND_ACK: Command Message ACK with result - " + str(msg.result))
              
            elif msg_type == "STATUSTEXT":
                rospy.loginfo ("STATUSTEXT: Status severity is %d. Text Message is %s" %(msg.severity, msg.text)) 

            elif msg_type == "PARAM_VALUE":
                rospy.loginfo ("PARAM_VALUE: ID = %s, Value = %d, Type = %d, Count = %d, Index = %d"
                    %(msg.param_id, msg.param_value, msg.param_type, msg.param_count, msg.param_index))
コード例 #11
0
ファイル: sue_dump.py プロジェクト: trigrass2/lino-udb4
def show_messages(m):
    '''show incoming mavlink messages'''
    while True:
        msg = m.recv_match(blocking=True)
        if not msg:
            return
        if msg.get_type() == "BAD_DATA":
            if mavutil.all_printable(msg.data):
                sys.stdout.write(msg.data)
                sys.stdout.flush()
        elif msg.get_type() == 'SERIAL_UDB_EXTRA_F2_A':
            print >> f, "F2:T%li:S%s:N%li:E%li:A%li:W%i:a%i:b%i:c%i:d%i:e%i:f%i:g%i:h%i" \
                     ":i%i:c%u:s%i:cpu%u:bmv%i:as%u:wvx%i:wvy%i:wvz%i:ma%i:mb%i:mc%i:svs%i:hd%i:" % \
                  ( msg.sue_time, bstr( msg.sue_status ), msg.sue_latitude, msg.sue_longitude, \
                    msg.sue_altitude, msg.sue_waypoint_index,  \
                    msg.sue_rmat0, msg.sue_rmat1, msg.sue_rmat2, \
                    msg.sue_rmat3, msg.sue_rmat4, msg.sue_rmat5, \
                    msg.sue_rmat6, msg.sue_rmat7, msg.sue_rmat8, \
                    msg.sue_cog, msg.sue_sog, msg.sue_cpu_load, msg.sue_voltage_milis, \
                    msg.sue_air_speed_3DIMU, \
                    msg.sue_estimated_wind_0, msg.sue_estimated_wind_1, msg.sue_estimated_wind_2, \
                    msg.sue_magFieldEarth0, msg.sue_magFieldEarth1, msg.sue_magFieldEarth2, \
                    msg.sue_svs, msg.sue_hdop ),
        elif msg.get_type() == 'SERIAL_UDB_EXTRA_F2_B':
            sys.stdout.softspace=False # This stops a space being inserted between print statements
            print >> f, "p1i%i:p2i%i:p3i%i:p4i%i:p5i%i:p6i%i:p7i%i:p8i%i:p9i%i:p10i%i:" \
                        "p1o%i:p2o%i:p3o%i:p4o%i:p5o%i:p6o%i:p7o%i:p8o%i:p9o%i:p10o%i:" \
                        "imx%i:imy%i:imz%i:fgs%X:ofc%i:tx%i:ty%i:tz%i:G%d,%d,%d:stk%d:\r\n" % \
                  ( msg.sue_pwm_input_1, msg.sue_pwm_input_2, msg.sue_pwm_input_3, msg.sue_pwm_input_4, msg.sue_pwm_input_5, \
                    msg.sue_pwm_input_6, msg.sue_pwm_input_7, msg.sue_pwm_input_8, msg.sue_pwm_input_9, msg.sue_pwm_input_10,\
                    msg.sue_pwm_output_1, msg.sue_pwm_output_2, msg.sue_pwm_output_3, \
                    msg.sue_pwm_output_4, msg.sue_pwm_output_5, msg.sue_pwm_output_6, \
                    msg.sue_pwm_output_7, msg.sue_pwm_output_8, msg.sue_pwm_output_9, \
                    msg.sue_pwm_output_10, msg.sue_imu_location_x, msg.sue_imu_location_y, msg.sue_imu_location_z,  \
                    msg.sue_flags, msg.sue_osc_fails,                                         \
	            msg.sue_imu_velocity_x, msg.sue_imu_velocity_y, msg.sue_imu_velocity_z,   \
	            msg.sue_waypoint_goal_x, msg.sue_waypoint_goal_y, msg.sue_waypoint_goal_z,\
                    msg.sue_memory_stack_free ),
        elif msg.get_type() == 'SERIAL_UDB_EXTRA_F4' :
            print >> f, "F4:R_STAB_A=%i:R_STAB_RD=%i:P_STAB=%i:Y_STAB_R=%i:Y_STAB_A=%i:AIL_NAV=%i:" \
                      "RUD_NAV=%i:AH_STAB=%i:AH_WP=%i:RACE=%i:\r\n" % \
	          ( msg.sue_ROLL_STABILIZATION_AILERONS, msg.sue_ROLL_STABILIZATION_RUDDER,   \
                    msg.sue_PITCH_STABILIZATION, msg.sue_YAW_STABILIZATION_RUDDER,            \
                    msg.sue_YAW_STABILIZATION_AILERON, msg.sue_AILERON_NAVIGATION,            \
                    msg.sue_RUDDER_NAVIGATION, msg.sue_ALTITUDEHOLD_STABILIZED,               \
                    msg.sue_ALTITUDEHOLD_WAYPOINT, msg.sue_RACING_MODE ),
        elif msg.get_type() == 'SERIAL_UDB_EXTRA_F5' :
            print >> f, "F5:YAWKP_A=%5.3f:YAWKD_A=%5.3f:ROLLKP=%5.3f:ROLLKD=%5.3f:A_BOOST=%3.1f:\r\n" % \
                  ( msg.sue_YAWKP_AILERON, msg.sue_YAWKD_AILERON, msg.sue_ROLLKP, \
                    msg.sue_ROLLKD, msg.sue_AILERON_BOOST ),
        elif msg.get_type() == 'SERIAL_UDB_EXTRA_F6' :
            print >> f, "F6:P_GAIN=%5.3f:P_KD=%5.3f:RUD_E_MIX=%5.3f:ROL_E_MIX=%5.3f:E_BOOST=%3.1f:\r\n" % \
                  ( msg.sue_PITCHGAIN, msg.sue_PITCHKD, msg.sue_RUDDER_ELEV_MIX, \
                    msg.sue_ROLL_ELEV_MIX, msg.sue_ELEVATOR_BOOST),
        elif msg.get_type() == 'SERIAL_UDB_EXTRA_F7' :
            print >> f, "F7:Y_KP_R=%5.4f:Y_KD_R=%5.3f:RLKP_RUD=%5.3f:RLKD_RUD=%5.3f:" \
                  "RUD_BOOST=%5.3f:RTL_PITCH_DN=%5.3f:\r\n"  % \
                   ( msg.sue_YAWKP_RUDDER, msg.sue_YAWKD_RUDDER, msg.sue_ROLLKP_RUDDER , \
                     msg.sue_ROLLKD_RUDDER , msg.sue_RUDDER_BOOST, msg.sue_RTL_PITCH_DOWN),
        elif msg.get_type() == 'SERIAL_UDB_EXTRA_F8' :
            print >> f, "F8:H_MAX=%6.1f:H_MIN=%6.1f:MIN_THR=%3.2f:MAX_THR=%3.2f:PITCH_MIN_THR=%4.1f:" \
                  "PITCH_MAX_THR=%4.1f:PITCH_ZERO_THR=%4.1f:\r\n" % \
                  ( msg.sue_HEIGHT_TARGET_MAX, msg.sue_HEIGHT_TARGET_MIN, \
                    msg.sue_ALT_HOLD_THROTTLE_MIN, msg.sue_ALT_HOLD_THROTTLE_MAX, \
                    msg.sue_ALT_HOLD_PITCH_MIN, msg.sue_ALT_HOLD_PITCH_MAX, \
                    msg.sue_ALT_HOLD_PITCH_HIGH),
        elif msg.get_type() == 'SERIAL_UDB_EXTRA_F13' :
            print >> f, "F13:week%i:origN%li:origE%li:origA%li:\r\n" % \
                  (msg.sue_week_no, msg.sue_lat_origin, msg.sue_lon_origin, msg.sue_alt_origin),
        elif msg.get_type() == 'SERIAL_UDB_EXTRA_F14' :
            print  "\r\nF14:WIND_EST=%i:GPS_TYPE=%i:DR=%i:BOARD_TYPE=%i:AIRFRAME=%i:RCON=0x%X:TRAP_FLAGS=0x%X:TRAP_SOURCE=0x%lX:ALARMS=%i:"  \
                       "CLOCK=%i:FP=%d:\r\n" % \
                  ( msg.sue_WIND_ESTIMATION, msg.sue_GPS_TYPE, msg.sue_DR, msg.sue_BOARD_TYPE, \
                    msg.sue_AIRFRAME, msg.sue_RCON, msg.sue_TRAP_FLAGS, msg.sue_TRAP_SOURCE, \
                    msg.sue_osc_fail_count, msg.sue_CLOCK_CONFIG, msg.sue_FLIGHT_PLAN_TYPE ),
        elif msg.get_type() == 'SERIAL_UDB_EXTRA_F15' :
            print >> f, "F15:IDA=%s:IDB=%s:\r\n" % \
                  ( msg.sue_ID_VEHICLE_MODEL_NAME, msg.sue_ID_VEHICLE_REGISTRATION ),
        elif msg.get_type() == 'SERIAL_UDB_EXTRA_F16' :
            print >> f, "F16:IDC=%s:IDD=%s:\r\n" % \
                  ( msg.sue_ID_LEAD_PILOT, msg.sue_ID_DIY_DRONES_URL ),
        else :
            pass