handle an incoming mavlink packet
428223
def mavlink_packet(self, msg):
        ''''''
        if not isinstance(self.checklist, mp_checklist.CheckUI):
            return
        if not self.checklist.is_alive():
            return

        type = msg.get_type()
        master = self.master

        if type == 'HEARTBEAT':
            '''beforeEngineList - APM booted'''
            if self.mpstate.status.heartbeat_error == True:
                self.checklist.set_check("Pixhawk Booted", 0)
            else:
                self.checklist.set_check("Pixhawk Booted", 1)

        '''beforeEngineList - Flight mode MANUAL'''
        if self.mpstate.status.flightmode == "MANUAL":
            self.checklist.set_check("Flight mode MANUAL", 1)
        else:
            self.checklist.set_check("Flight mode MANUAL", 0)

        if type in [ 'GPS_RAW', 'GPS_RAW_INT' ]:
            '''beforeEngineList - GPS lock'''
            if ((msg.fix_type >= 3 and master.mavlink10()) or
                (msg.fix_type == 2 and not master.mavlink10())):
                self.checklist.set_check("GPS lock", 1)
            else:
                self.checklist.set_check("GPS lock", 0)

        '''beforeEngineList - Radio Links > 6db margin TODO: figure out how to read db levels'''
        if type in ['RADIO', 'RADIO_STATUS']:
            if msg.rssi < msg.noise+6 or msg.remrssi < msg.remnoise+6:
                self.checklist.set_check("Radio links > 6db margin", 0)
            else:
                self.checklist.set_check("Radio Links > 6db margin", 0)

        if type == 'HWSTATUS':
            '''beforeEngineList - Avionics Battery'''
            if msg.Vcc >= 4600 and msg.Vcc <= 5300:
                self.checklist.set_check("Avionics Power", 1)
            else:
                self.checklist.set_check("Avionics Power", 0)

        if type == 'POWER_STATUS':
            '''beforeEngineList - Servo Power'''
            if msg.Vservo >= 4900 and msg.Vservo <= 6500:
                self.checklist.set_check("Servo Power", 1)
            else:
                self.checklist.set_check("Servo Power", 0)

        '''beforeEngineList - Waypoints Loaded'''
        if type == 'HEARTBEAT':
            if self.module('wp').wploader.count() == 0:
                self.checklist.set_check("Waypoints Loaded", 0)
            else:
                self.checklist.set_check("Waypoints Loaded", 1)

		'''beforeTakeoffList - Compass active'''
        if type == 'GPS_RAW':
            if math.fabs(msg.hdg - master.field('VFR_HUD', 'heading', '-')) < 10 or math.fabs(msg.hdg - master.field('VFR_HUD', 'heading', '-')) > 355:
                self.checklist.set_check("Compass active", 1)
            else:
                self.checklist.set_check("Compass active", 0)

		'''beforeCruiseList - Airspeed > 10 m/s , Altitude > 30 m'''
        if type == 'VFR_HUD':
            rel_alt = master.field('GLOBAL_POSITION_INT', 'relative_alt', 0) * 1.0e-3
            if rel_alt > 30:
                self.checklist.set_check("Altitude > 30 m", 1)
            else:
                self.checklist.set_check("Altitude > 30 m", 0)
            if msg.airspeed > 10 or msg.groundspeed > 10:
                self.checklist.set_check("Airspeed > 10 m/s", 1)
            else:
                self.checklist.set_check("Airspeed > 10 m/s", 0)

		'''beforeEngineList - IMU'''
        if type in ['SYS_STATUS']:
            sensors = { 'AS'  : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE,
                        'MAG' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_MAG,
                        'INS' : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_ACCEL | mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_GYRO,
                        'AHRS' : mavutil.mavlink.MAV_SYS_STATUS_AHRS}
            bits = sensors['INS']
            present = ((msg.onboard_control_sensors_enabled & bits) == bits)
            healthy = ((msg.onboard_control_sensors_health & bits) == bits)
            if not present or not healthy:
                self.checklist.set_check("IMU Check", 1)
            else:
                self.checklist.set_check("IMU Check", 0)
134048
def handle(self, pkt, raddress, rport):
        ""
        if isinstance(pkt, TftpPacketACK):
            log.debug("Received ACK for packet %d" % pkt.blocknumber)
            # Is this an ack to the one we just sent?
            if self.context.next_block == pkt.blocknumber:
                if self.context.pending_complete:
                    log.info("Received ACK to final DAT, we're done.")
                    return None
                else:
                    log.debug("Good ACK, sending next DAT")
                    self.context.next_block += 1
                    log.debug("Incremented next_block to %d",
                        self.context.next_block)
                    self.context.pending_complete = self.sendDAT()

            elif pkt.blocknumber < self.context.next_block:
                log.warning("Received duplicate ACK for block %d"
                    % pkt.blocknumber)
                self.context.metrics.add_dup(pkt)

            else:
                log.warning("Oooh, time warp. Received ACK to packet we "
                         "didn't send yet. Discarding.")
                self.context.metrics.errors += 1
            return self
        elif isinstance(pkt, TftpPacketERR):
            log.error("Received ERR packet from peer: %s" % str(pkt))
            raise TftpException("Received ERR packet from peer: %s" % str(pkt))
        else:
            log.warning("Discarding unsupported packet: %s" % str(pkt))
            return self
192951
def mavlink_packet(self, m):
        ''''''
        if m.get_type() == 'GLOBAL_POSITION_INT':
            if self.settings.target_system == 0 or self.settings.target_system == m.get_srcSystem():
                self.packets_mytarget += 1
            else:
                self.packets_othertarget += 1
472083
def packet_in_handler(self, evt):
        """"""
        msg = evt.msg
        dpid = msg.datapath.id

        req_pkt = packet.Packet(msg.data)
        req_igmp = req_pkt.get_protocol(igmp.igmp)
        if req_igmp:
            if self._querier.dpid == dpid:
                self._querier.packet_in_handler(req_igmp, msg)
            else:
                self._snooper.packet_in_handler(req_pkt, req_igmp, msg)
        else:
            self.send_event_to_observers(EventPacketIn(msg))
528024
def mavlink_packet(self, msg):
        ''''''
        if not isinstance(self.console, wxconsole.MessageConsole):
            return
        if not self.console.is_alive():
            self.mpstate.console = textconsole.SimpleConsole()
            return
        type = msg.get_type()

        master = self.master
        # add some status fields
        if type in [ 'GPS_RAW', 'GPS_RAW_INT' ]:
            if type == "GPS_RAW":
                num_sats1 = master.field('GPS_STATUS', 'satellites_visible', 0)
            else:
                num_sats1 = msg.satellites_visible
            num_sats2 = master.field('GPS2_RAW', 'satellites_visible', -1)
            if num_sats2 == -1:
                sats_string = "%u" % num_sats1
            else:
                sats_string = "%u/%u" % (num_sats1, num_sats2)
            if ((msg.fix_type == 3 and master.mavlink10()) or
                (msg.fix_type == 2 and not master.mavlink10())):
                self.console.set_status('GPS', 'GPS: OK (%s)' % sats_string, fg='green')
            else:
                self.console.set_status('GPS', 'GPS: %u (%s)' % (msg.fix_type, sats_string), fg='red')
            if master.mavlink10():
                gps_heading = int(self.mpstate.status.msgs['GPS_RAW_INT'].cog * 0.01)
            else:
                gps_heading = self.mpstate.status.msgs['GPS_RAW'].hdg
            self.console.set_status('Heading', 'Hdg %s/%u' % (master.field('VFR_HUD', 'heading', '-'), gps_heading))
        elif type == 'VFR_HUD':
            if master.mavlink10():
                alt = master.field('GPS_RAW_INT', 'alt', 0) / 1.0e3
            else:
                alt = master.field('GPS_RAW', 'alt', 0)
            if self.module('wp').wploader.count() > 0:
                wp = self.module('wp').wploader.wp(0)
                home_lat = wp.x
                home_lng = wp.y
            else:
                home_lat = master.field('HOME', 'lat') * 1.0e-7
                home_lng = master.field('HOME', 'lon') * 1.0e-7
            lat = master.field('GLOBAL_POSITION_INT', 'lat', 0) * 1.0e-7
            lng = master.field('GLOBAL_POSITION_INT', 'lon', 0) * 1.0e-7
            rel_alt = master.field('GLOBAL_POSITION_INT', 'relative_alt', 0) * 1.0e-3
            agl_alt = None
            if self.settings.basealt != 0:
                agl_alt = self.console.ElevationMap.GetElevation(lat, lng)
                if agl_alt is not None:
                    agl_alt = self.settings.basealt - agl_alt
            else:
                try:
                    agl_alt_home = self.console.ElevationMap.GetElevation(home_lat, home_lng)
                except Exception as ex:
                    print(ex)
                    agl_alt_home = None
                if agl_alt_home is not None:
                    agl_alt = self.console.ElevationMap.GetElevation(lat, lng)
                if agl_alt is not None:
                    agl_alt = agl_alt_home - agl_alt
            if agl_alt is not None:
                agl_alt += rel_alt
                vehicle_agl = master.field('TERRAIN_REPORT', 'current_height', None)
                if vehicle_agl is None:
                    vehicle_agl = '---'
                else:
                    vehicle_agl = int(vehicle_agl)
                self.console.set_status('AGL', 'AGL %u/%s' % (agl_alt, vehicle_agl))
            self.console.set_status('Alt', 'Alt %u' % rel_alt)
            self.console.set_status('AirSpeed', 'AirSpeed %u' % msg.airspeed)
            self.console.set_status('GPSSpeed', 'GPSSpeed %u' % msg.groundspeed)
            self.console.set_status('Thr', 'Thr %u' % msg.throttle)
            t = time.localtime(msg._timestamp)
            flying = False
            if self.mpstate.vehicle_type == 'copter':
                flying = self.master.motors_armed()
            else:
                flying = msg.groundspeed > 3
            if flying and not self.in_air:
                self.in_air = True
                self.start_time = time.mktime(t)
            elif flying and self.in_air:
                self.total_time = time.mktime(t) - self.start_time
                self.console.set_status('FlightTime', 'FlightTime %u:%02u' % (int(self.total_time)/60, int(self.total_time)%60))
            elif not flying and self.in_air:
                self.in_air = False
                self.total_time = time.mktime(t) - self.start_time
                self.console.set_status('FlightTime', 'FlightTime %u:%02u' % (int(self.total_time)/60, int(self.total_time)%60))
        elif type == 'ATTITUDE':
            self.console.set_status('Roll', 'Roll %u' % math.degrees(msg.roll))
            self.console.set_status('Pitch', 'Pitch %u' % math.degrees(msg.pitch))
        elif type in ['SYS_STATUS']:
            sensors = { 'AS'   : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE,
                        'MAG'  : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_MAG,
                        'INS'  : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_ACCEL | mavutil.mavlink.MAV_SYS_STATUS_SENSOR_3D_GYRO,
                        'AHRS' : mavutil.mavlink.MAV_SYS_STATUS_AHRS,
                        'RC'   : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER,
                        'TERR' : mavutil.mavlink.MAV_SYS_STATUS_TERRAIN,
                        'RNG'  : mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION}
            announce = [ 'RC' ]
            for s in sensors.keys():
                bits = sensors[s]
                present = ((msg.onboard_control_sensors_enabled & bits) == bits)
                healthy = ((msg.onboard_control_sensors_health & bits) == bits)
                if not present:
                    fg = 'grey'
                elif not healthy:
                    fg = 'red'
                else:
                    fg = 'green'
                # for terrain show yellow if still loading
                if s == 'TERR' and fg == 'green' and master.field('TERRAIN_REPORT', 'pending', 0) != 0:
                    fg = 'yellow'
                self.console.set_status(s, s, fg=fg)
            for s in announce:
                bits = sensors[s]
                present = ((msg.onboard_control_sensors_enabled & bits) == bits)
                healthy = ((msg.onboard_control_sensors_health & bits) == bits)
                was_healthy = ((self.last_sys_status_health & bits) == bits)
                if present and not healthy and was_healthy:
                    self.say("%s fail" % s)
            self.last_sys_status_health = msg.onboard_control_sensors_health

        elif type == 'WIND':
            self.console.set_status('Wind', 'Wind %u/%.2f' % (msg.direction, msg.speed))

        elif type == 'EKF_STATUS_REPORT':
            highest = 0.0
            vars = ['velocity_variance',
                    'pos_horiz_variance',
                    'pos_vert_variance',
                    'compass_variance',
                    'terrain_alt_variance']
            for var in vars:
                v = getattr(msg, var, 0)
                highest = max(v, highest)
            if highest >= 1.0:
                fg = 'red'
            elif highest >= 0.5:
                fg = 'orange'
            else:
                fg = 'green'
            self.console.set_status('EKF', 'EKF', fg=fg)

        elif type == 'HWSTATUS':
            if msg.Vcc >= 4600 and msg.Vcc <= 5300:
                fg = 'green'
            else:
                fg = 'red'
            self.console.set_status('Vcc', 'Vcc %.2f' % (msg.Vcc * 0.001), fg=fg)
        elif type == 'POWER_STATUS':
            if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_CHANGED:
                fg = 'red'
            else:
                fg = 'green'
            status = 'PWR:'
            if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_USB_CONNECTED:
                status += 'U'
            if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_BRICK_VALID:
                status += 'B'
            if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_SERVO_VALID:
                status += 'S'
            if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_PERIPH_OVERCURRENT:
                status += 'O1'
            if msg.flags & mavutil.mavlink.MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT:
                status += 'O2'
            self.console.set_status('PWR', status, fg=fg)
            self.console.set_status('Srv', 'Srv %.2f' % (msg.Vservo*0.001), fg='green')
        elif type in ['RADIO', 'RADIO_STATUS']:
            if msg.rssi < msg.noise+10 or msg.remrssi < msg.remnoise+10:
                fg = 'red'
            else:
                fg = 'black'
            self.console.set_status('Radio', 'Radio %u/%u %u/%u' % (msg.rssi, msg.noise, msg.remrssi, msg.remnoise), fg=fg)
        elif type == 'HEARTBEAT':
            self.console.set_status('Mode', '%s' % master.flightmode, fg='blue')
            if self.master.motors_armed():
                arm_colour = 'green'
            else:
                arm_colour = 'red'
            self.console.set_status('ARM', 'ARM', fg=arm_colour)
            if self.max_link_num != len(self.mpstate.mav_master):
                for i in range(self.max_link_num):
                    self.console.set_status('Link%u'%(i+1), '', row=1)
                self.max_link_num = len(self.mpstate.mav_master)
            for m in self.mpstate.mav_master:
                linkdelay = (self.mpstate.status.highest_msec - m.highest_msec)*1.0e-3
                linkline = "Link %u " % (m.linknum+1)
                if m.linkerror:
                    linkline += "down"
                    fg = 'red'
                else:
                    packets_rcvd_percentage = 100
                    if (m.mav_loss != 0): #avoid divide-by-zero
                        packets_rcvd_percentage = (1.0 - (float(m.mav_loss) / float(m.mav_count))) * 100.0

                    linkline += "OK (%u pkts, %.2fs delay, %u lost) %u%%" % (m.mav_count, linkdelay, m.mav_loss, packets_rcvd_percentage)
                    if linkdelay > 1:
                        fg = 'orange'
                    else:
                        fg = 'dark green'
                self.console.set_status('Link%u'%m.linknum, linkline, row=1, fg=fg)
        elif type in ['WAYPOINT_CURRENT', 'MISSION_CURRENT']:
            self.console.set_status('WP', 'WP %u' % msg.seq)
            lat = master.field('GLOBAL_POSITION_INT', 'lat', 0) * 1.0e-7
            lng = master.field('GLOBAL_POSITION_INT', 'lon', 0) * 1.0e-7
            if lat != 0 and lng != 0:
                airspeed = master.field('VFR_HUD', 'airspeed', 30)
                if abs(airspeed - self.speed) > 5:
                    self.speed = airspeed
                else:
                    self.speed = 0.98*self.speed + 0.02*airspeed
                self.speed = max(1, self.speed)
                time_remaining = int(self.estimated_time_remaining(lat, lng, msg.seq, self.speed))
                self.console.set_status('ETR', 'ETR %u:%02u' % (time_remaining/60, time_remaining%60))

        elif type == 'NAV_CONTROLLER_OUTPUT':
            self.console.set_status('WPDist', 'Distance %u' % msg.wp_dist)
            self.console.set_status('WPBearing', 'Bearing %u' % msg.target_bearing)
            if msg.alt_error > 0:
                alt_error_sign = "L"
            else:
                alt_error_sign = "H"
            if msg.aspd_error > 0:
                aspd_error_sign = "L"
            else:
                aspd_error_sign = "H"
            self.console.set_status('AltError', 'AltError %d%s' % (msg.alt_error, alt_error_sign))
            self.console.set_status('AspdError', 'AspdError %.1f%s' % (msg.aspd_error*0.01, aspd_error_sign))
762266
public void handleMessageReceived(Packet packet) {
        if (log.isTraceEnabled()) {
            log.trace("handleMessageReceived - {}", sessionId);
        }
        // set the packet expiration time if maxHandlingTimeout is not disabled (set to 0)
        if (maxHandlingTimeout > 0) {
            packet.setExpirationTime(System.currentTimeMillis() + maxHandlingTimeout);
        }
        if (executor != null) {
            final byte dataType = packet.getHeader().getDataType();
            // route these types outside the executor
            switch (dataType) {
                case Constants.TYPE_PING:
                case Constants.TYPE_ABORT:
                case Constants.TYPE_BYTES_READ:
                case Constants.TYPE_CHUNK_SIZE:
                case Constants.TYPE_CLIENT_BANDWIDTH:
                case Constants.TYPE_SERVER_BANDWIDTH:
                    // pass message to the handler
                    try {
                        handler.messageReceived(this, packet);
                    } catch (Exception e) {
                        log.error("Error processing received message {}", sessionId, e);
                    }
                    break;
                default:
                    final String messageType = getMessageType(packet);
                    try {
                        // increment the packet number
                        final long packetNumber = packetSequence.incrementAndGet();
                        if (executorQueueSizeToDropAudioPackets > 0 && currentQueueSize.get() >= executorQueueSizeToDropAudioPackets) {
                            if (packet.getHeader().getDataType() == Constants.TYPE_AUDIO_DATA) {
                                // if there's a backlog of messages in the queue. Flash might have sent a burst of messages after a network congestion. Throw away packets that we are able to discard.
                                log.info("Queue threshold reached. Discarding packet: session=[{}], msgType=[{}], packetNum=[{}]", sessionId, messageType, packetNumber);
                                return;
                            }
                        }
                        int streamId = packet.getHeader().getStreamId().intValue();
                        if (log.isTraceEnabled()) {
                            log.trace("Handling message for streamId: {}, channelId: {} Channels: {}", streamId, packet.getHeader().getChannelId(), channels);
                        }
                        // create a task to setProcessing the message
                        ReceivedMessageTask task = new ReceivedMessageTask(sessionId, packet, handler, this);
                        task.setPacketNumber(packetNumber);
                        // create a task queue
                        ReceivedMessageTaskQueue newStreamTasks = new ReceivedMessageTaskQueue(streamId, this);
                        // put the queue in the task by stream map
                        ReceivedMessageTaskQueue currentStreamTasks = tasksByStreams.putIfAbsent(streamId, newStreamTasks);
                        if (currentStreamTasks != null) {
                            // add the task to the existing queue
                            currentStreamTasks.addTask(task);
                        } else {
                            // add the task to the newly created and just added queue
                            newStreamTasks.addTask(task);
                        }
                    } catch (Exception e) {
                        log.error("Incoming message handling failed on session=[" + sessionId + "], messageType=[" + messageType + "]", e);
                        if (log.isDebugEnabled()) {
                            log.debug("Execution rejected on {} - {}", sessionId, RTMP.states[getStateCode()]);
                            log.debug("Lock permits - decode: {} encode: {}", decoderLock.availablePermits(), encoderLock.availablePermits());
                        }
                    }
            }
        } else {
            log.debug("Executor is null on {} state: {}", sessionId, RTMP.states[getStateCode()]);
            // pass message to the handler
            try {
                handler.messageReceived(this, packet);
            } catch (Exception e) {
                log.error("Error processing received message {} state: {}", sessionId, RTMP.states[getStateCode()], e);
            }
        }
    }
1172728
def _handle_ping(self, packet, protocol):
        
        if 'payload' in packet:
            is_valid_node = True
            node_ids = list(packet['payload'].values())
            for node_id in node_ids:
                if self._repository.get_node(node_id) is None:
                    is_valid_node = False
                    break
            if is_valid_node:
                self._pong(packet, protocol)
        else:
            self._pong(packet, protocol)
81639
protected void processReceivedMessage(DatagramPacket packet, byte[] buffer) {

		String received = new String(buffer).trim();
		String tokens[] = received.split("\\|");
		if (tokens.length != 3) {
			logger.warn("Malformed UDP message received (" + received + ")!");
			return;
		}

		// Check prefix and nodeID
		if (!namespace.equals(tokens[0]) || nodeID.equals(tokens[1])) {
			return;
		}
		if (tokens[1].isEmpty()) {
			logger.warn("Empty nodeID received!");
			return;
		}

		// Get source hostname or IP
		String host;
		try {
			InetAddress address = packet.getAddress();
			if (useHostname) {
				host = address.getHostName();
				if (host == null || host.isEmpty() || host.contains("localhost")) {
					host = address.getHostAddress();
				}
			} else {
				host = address.getHostAddress();
				if (host == null || host.isEmpty() || host.startsWith("127.")) {
					host = address.getHostName();
				}
			}
			if (host == null || host.isEmpty()) {
				logger.warn("Unable to detect sender's hostname!");
				return;
			}
		} catch (Exception cause) {
			logger.warn("Unable to detect sender's IP address or hostname!", cause);
			return;
		}
		int port = 0;
		try {
			port = Integer.parseInt(tokens[2]);
			if (port < 1) {
				logger.warn("Invalid port number (" + port + ")!");
				return;
			}
		} catch (Exception cause) {
			logger.warn("Invalid port number format (" + tokens[2] + ")!");
			return;
		}

		// Notify TCP Transporter
		transporter.udpPacketReceived(tokens[1], host.toLowerCase(), port);
	}
789714
def mavlink_packet(self, m):
        ''''''
        state = self
        mtype = m.get_type()
        if mtype == 'GLOBAL_POSITION_INT':
            state.lat, state.lon = m.lat*scale_latlon, m.lon*scale_latlon
            state.hdg = m.hdg*scale_hdg
            agl = state.elevation_model.GetElevation(state.lat, state.lon)
            if agl is not None:
                state.height = m.relative_alt*scale_relative_alt + state.home_height - agl
        elif mtype == 'ATTITUDE':
            state.roll, state.pitch, state.yaw = math.degrees(m.roll), math.degrees(m.pitch), math.degrees(m.yaw)
        elif mtype in ['GPS_RAW', 'GPS_RAW_INT']:
            if self.module('wp').wploader.count() > 0:
                home = self.module('wp').wploader.wp(0).x, self.module('wp').wploader.wp(0).y
            else:
                home = [self.master.field('HOME', c)*scale_latlon for c in ['lat', 'lon']]
            old = state.home_height # TODO TMP
            agl = state.elevation_model.GetElevation(*home)
            if agl is None:
                return
            state.home_height = agl

            # TODO TMP
            if state.home_height != old:
                # tridge said to get home pos from wploader,
                # but this is not the same as from master() below...!!
                # using master() gives the right coordinates
                # (i.e. matches GLOBAL_POSITION_INT coords, and $IMHOME in sim_arduplane.sh)
                # and wploader is a bit off
                print('home height changed from',old,'to',state.home_height)
        elif mtype == 'SERVO_OUTPUT_RAW':
            for (axis, attr) in [('ROLL', 'mount_roll'), ('TILT', 'mount_pitch'), ('PAN', 'mount_yaw')]:
                channel = int(self.get_mav_param('MNT_RC_IN_{0}'.format(axis), 0))
                if self.get_mav_param('MNT_STAB_{0}'.format(axis), 0) and channel:
                    # enabled stabilisation on this axis
                    # TODO just guessing that RC_IN_ROLL gives the servo number, but no idea if this is really the case
                    servo = 'servo{0}_raw'.format(channel)
                    centidegrees = self.scale_rc(getattr(m, servo),
                                            self.get_mav_param('MNT_ANGMIN_{0}'.format(axis[:3])),
                                            self.get_mav_param('MNT_ANGMAX_{0}'.format(axis[:3])),
                                            param='RC{0}'.format(channel))
                    setattr(state, attr, centidegrees*0.01)
            #state.mount_roll = min(max(-state.roll,-45),45)#TODO TMP
            #state.mount_yaw = min(max(-state.yaw,-45),45)#TODO TMP
            #state.mount_pitch = min(max(-state.pitch,-45),45)#TODO TMP
        else:
            return
        if self.mpstate.map: # if the map module is loaded, redraw polygon
            # get rid of the old polygon
            self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('CameraView'))

            # camera view polygon determined by projecting corner pixels of the image onto the ground
            pixel_positions = [cuav_util.pixel_position(px[0],px[1], state.height, state.pitch+state.mount_pitch, state.roll+state.mount_roll, state.yaw+state.mount_yaw, state.camera_params) for px in [(0,0), (state.camera_params.xresolution,0), (state.camera_params.xresolution,state.camera_params.yresolution), (0,state.camera_params.yresolution)]]
            if any(pixel_position is None for pixel_position in pixel_positions):
                # at least one of the pixels is not on the ground
                # so it doesn't make sense to try to draw the polygon
                return
            gps_positions = [mp_util.gps_newpos(state.lat, state.lon, math.degrees(math.atan2(*pixel_position)), math.hypot(*pixel_position)) for pixel_position in pixel_positions]

            # draw new polygon
            self.mpstate.map.add_object(mp_slipmap.SlipPolygon('cameraview', gps_positions+[gps_positions[0]], # append first element to close polygon
                                                          layer='CameraView', linewidth=2, colour=state.col))
1050994
def mavlink_packet(self, m):
        ''''''
        if self.gcs_location is None and self.module('wp').wploader.count() > 0:
            home = self.module('wp').get_home()
            self.gcs_location = (home.x, home.y)
            print("Antenna home set")
        if self.gcs_location is None:
            return
        if m.get_type() == 'GPS_RAW' and self.gcs_location is not None:
            (gcs_lat, gcs_lon) = self.gcs_location
            bearing = cuav_util.gps_bearing(gcs_lat, gcs_lon, m.lat, m.lon)
        elif m.get_type() == 'GPS_RAW_INT' and self.gcs_location is not None:
            (gcs_lat, gcs_lon) = self.gcs_location
            bearing = cuav_util.gps_bearing(gcs_lat, gcs_lon, m.lat / 1.0e7, m.lon / 1.0e7)
        else:
            return
        self.console.set_status('Antenna', 'Antenna %.0f' % bearing, row=0)
        if abs(bearing - self.last_bearing) > 5 and (time.time() - self.last_announce) > 15:
            self.last_bearing = bearing
            self.last_announce = time.time()
            self.say("Antenna %u" % int(bearing + 0.5))
25089
private void handleIncomingPacket(DatagramPacket packet) {
    SsdpResponse response = new ResponseParser().parse(packet);
    if (response == null) {
      // Unknown to protocol
      return;
    }
    if (response.getType().equals(SsdpResponse.Type.DISCOVERY_RESPONSE)) {
      handleDiscoveryResponse(response);
    } else if (response.getType().equals(SsdpResponse.Type.PRESENCE_ANNOUNCEMENT)) {
      handlePresenceAnnouncement(response);
    }
  }
718464
def master_msg_handling(self, m, master):
        ''''''
        if self.settings.target_system != 0 and m.get_srcSystem() != self.settings.target_system:
            # don't process messages not from our target
            if m.get_type() == "BAD_DATA":
                if self.mpstate.settings.shownoise and mavutil.all_printable(m.data):
                    self.mpstate.console.write(str(m.data), bg='red')
            return

        if self.settings.target_system != 0 and master.target_system != self.settings.target_system:
            # keep the pymavlink level target system aligned with the MAVProxy setting
            master.target_system = self.settings.target_system

        if self.settings.target_component != 0 and master.target_component != self.settings.target_component:
            # keep the pymavlink level target component aligned with the MAVProxy setting
            print("change target_component %u" % self.settings.target_component)
            master.target_component = self.settings.target_component
            
        mtype = m.get_type()

        if mtype == 'HEARTBEAT' and m.type != mavutil.mavlink.MAV_TYPE_GCS:
            if self.settings.target_system == 0 and self.settings.target_system != m.get_srcSystem():
                self.settings.target_system = m.get_srcSystem()
                self.say("online system %u" % self.settings.target_system,'message')
                for mav in self.mpstate.mav_master:
                    mav.target_system = self.settings.target_system

            if self.status.heartbeat_error:
                self.status.heartbeat_error = False
                self.say("heartbeat OK")
            if master.linkerror:
                master.linkerror = False
                self.say("link %s OK" % (self.link_label(master)))
            self.status.last_heartbeat = time.time()
            master.last_heartbeat = self.status.last_heartbeat

            armed = self.master.motors_armed()
            if armed != self.status.armed:
                self.status.armed = armed
                if armed:
                    self.say("ARMED")
                else:
                    self.say("DISARMED")

            if master.flightmode != self.status.flightmode:
                self.status.flightmode = master.flightmode
                if self.mpstate.functions.input_handler is None:
                    self.set_prompt(self.status.flightmode + "> ")

            if master.flightmode != self.status.last_mode_announced and time.time() > self.status.last_mode_announce + 2:
                    self.status.last_mode_announce = time.time()
                    self.status.last_mode_announced = master.flightmode
                    self.say("Mode " + self.status.flightmode)

            if m.type == mavutil.mavlink.MAV_TYPE_FIXED_WING:
                self.mpstate.vehicle_type = 'plane'
                self.mpstate.vehicle_name = 'ArduPlane'
            elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
                            mavutil.mavlink.MAV_TYPE_SURFACE_BOAT]:
                self.mpstate.vehicle_type = 'rover'
                self.mpstate.vehicle_name = 'APMrover2'
            elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]:
                self.mpstate.vehicle_type = 'sub'
                self.mpstate.vehicle_name = 'ArduSub'
            elif m.type in [mavutil.mavlink.MAV_TYPE_QUADROTOR,
                            mavutil.mavlink.MAV_TYPE_COAXIAL,
                            mavutil.mavlink.MAV_TYPE_HEXAROTOR,
                            mavutil.mavlink.MAV_TYPE_OCTOROTOR,
                            mavutil.mavlink.MAV_TYPE_TRICOPTER,
                            mavutil.mavlink.MAV_TYPE_HELICOPTER,
                            mavutil.mavlink.MAV_TYPE_DODECAROTOR]:
                self.mpstate.vehicle_type = 'copter'
                self.mpstate.vehicle_name = 'ArduCopter'
            elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
                self.mpstate.vehicle_type = 'antenna'
                self.mpstate.vehicle_name = 'AntennaTracker'

        elif mtype == 'STATUSTEXT':
            if m.text != self.status.last_apm_msg or time.time() > self.status.last_apm_msg_time+2:
                (fg, bg) = self.colors_for_severity(m.severity)
                self.mpstate.console.writeln("APM: %s" % mp_util.null_term(m.text), bg=bg, fg=fg)
                self.status.last_apm_msg = m.text
                self.status.last_apm_msg_time = time.time()

        elif mtype == "VFR_HUD":
            have_gps_lock = False
            if 'GPS_RAW' in self.status.msgs and self.status.msgs['GPS_RAW'].fix_type == 2:
                have_gps_lock = True
            elif 'GPS_RAW_INT' in self.status.msgs and self.status.msgs['GPS_RAW_INT'].fix_type == 3:
                have_gps_lock = True
            if have_gps_lock and not self.status.have_gps_lock and m.alt != 0:
                self.say("GPS lock at %u meters" % m.alt, priority='notification')
                self.status.have_gps_lock = True

        elif mtype == "GPS_RAW":
            if self.status.have_gps_lock:
                if m.fix_type != 2 and not self.status.lost_gps_lock and (time.time() - self.status.last_gps_lock) > 3:
                    self.say("GPS fix lost")
                    self.status.lost_gps_lock = True
                if m.fix_type == 2 and self.status.lost_gps_lock:
                    self.say("GPS OK")
                    self.status.lost_gps_lock = False
                if m.fix_type == 2:
                    self.status.last_gps_lock = time.time()

        elif mtype == "GPS_RAW_INT":
            if self.status.have_gps_lock:
                if m.fix_type < 3 and not self.status.lost_gps_lock and (time.time() - self.status.last_gps_lock) > 3:
                    self.say("GPS fix lost")
                    self.status.lost_gps_lock = True
                if m.fix_type >= 3 and self.status.lost_gps_lock:
                    self.say("GPS OK")
                    self.status.lost_gps_lock = False
                if m.fix_type >= 3:
                    self.status.last_gps_lock = time.time()

        elif mtype == "NAV_CONTROLLER_OUTPUT" and self.status.flightmode == "AUTO" and self.mpstate.settings.distreadout:
            rounded_dist = int(m.wp_dist/self.mpstate.settings.distreadout)*self.mpstate.settings.distreadout
            if math.fabs(rounded_dist - self.status.last_distance_announce) >= self.mpstate.settings.distreadout:
                if rounded_dist != 0:
                    self.say("%u" % rounded_dist, priority="progress")
            self.status.last_distance_announce = rounded_dist

        elif mtype == "GLOBAL_POSITION_INT":
            self.report_altitude(m.relative_alt*0.001)

        elif mtype == "COMPASSMOT_STATUS":
            print(m)

        elif mtype == "SIMSTATE":
            self.mpstate.is_sitl = True

        elif mtype == "ATTITUDE":
            att_time = m.time_boot_ms * 0.001
            self.mpstate.attitude_time_s = max(self.mpstate.attitude_time_s, att_time)
            if self.mpstate.attitude_time_s - att_time > 120:
                # cope with wrap
                self.mpstate.attitude_time_s = att_time

        elif mtype in [ "COMMAND_ACK", "MISSION_ACK" ]:
            self.mpstate.console.writeln("Got MAVLink msg: %s" % m)

            if mtype == "COMMAND_ACK" and m.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION:
                if m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED:
                    self.say("Calibrated")
                elif m.result == mavutil.mavlink.MAV_RESULT_FAILED:
                    self.say("Calibration failed")
                elif m.result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED:
                    self.say("Calibration unsupported")
                elif m.result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED:
                    self.say("Calibration temporarily rejected")
                else:
                    self.say("Calibration response (%u)" % m.result)
        else:
            #self.mpstate.console.writeln("Got MAVLink msg: %s" % m)
            pass

        if self.status.watch is not None:
            for msg_type in self.status.watch:
                if fnmatch.fnmatch(mtype.upper(), msg_type.upper()):
                    self.mpstate.console.writeln('< '+ str(m))
                    break
20846
def mavlink_packet(self, m):
        ''''''
        if self.accelcal_count != -1:
            if m.get_type() == 'STATUSTEXT':
                # handle accelcal packet
                text = str(m.text)
                if text.startswith('Place '):
                    self.accelcal_wait_enter = True
                    self.empty_input_count = self.mpstate.empty_input_count
        if m.get_type() == 'MAG_CAL_PROGRESS':
            while m.compass_id >= len(self.magcal_progess):
                self.magcal_progess.append("")
            self.magcal_progess[m.compass_id] = "%u%%" % m.completion_pct
            self.console.set_status('Progress', 'Calibration Progress: ' + " ".join(self.magcal_progess), row=4)
        if m.get_type() == 'MAG_CAL_REPORT':
            if m.cal_status == mavutil.mavlink.MAG_CAL_SUCCESS:
                result = "SUCCESS"
            else:
                result = "FAILED"
            self.magcal_progess[m.compass_id] = result
            self.console.set_status('Progress', 'Calibration Progress: ' + " ".join(self.magcal_progess), row=4)
            print("Calibration of compass %u %s: fitness %.3f" % (m.compass_id, result, m.fitness))
925417
def packet_handle(self):
        """"""
        cmd = self.in_packet.command & 0xF0
        
        if cmd == NC.CMD_CONNACK:
            return self.handle_connack()
        elif cmd == NC.CMD_PINGRESP:
            return self.handle_pingresp()
        elif cmd == NC.CMD_PUBLISH:
            return self.handle_publish()
        elif cmd == NC.CMD_PUBACK:
            return self.handle_puback()
        elif cmd == NC.CMD_PUBREC:
            return self.handle_pubrec()
        elif cmd == NC.CMD_PUBREL:
            return self.handle_pubrel()
        elif cmd == NC.CMD_PUBCOMP:
            return self.handle_pubcomp()
        elif cmd == NC.CMD_SUBSCRIBE:
            sys.exit(-1)
        elif cmd == NC.CMD_SUBACK:
            return self.handle_suback()
        elif cmd == NC.CMD_UNSUBSCRIBE:
            print "Received UNSUBSCRIBE"
            sys.exit(-1)
        elif cmd == NC.CMD_UNSUBACK:
            return self.handle_unsuback()
        else:
            self.logger.warning("Unknown protocol. Cmd = %d", cmd)
            return NC.ERR_PROTOCOL
408089
def _incoming(self, packet):
        
        if len(packet.data) < 1:
            logger.warning('Localization packet received with incorrect' +
                           'length (length is {})'.format(len(packet.data)))
            return

        pk_type = struct.unpack('<B', packet.data[:1])[0]
        data = packet.data[1:]

        # Decoding the known packet types
        # TODO: more generic decoding scheme?
        decoded_data = None
        if pk_type == self.RANGE_STREAM_REPORT:
            if len(data) % 5 != 0:
                logger.error('Wrong range stream report data lenght')
                return
            decoded_data = {}
            raw_data = data
            for i in range(int(len(data) / 5)):
                anchor_id, distance = struct.unpack('<Bf', raw_data[:5])
                decoded_data[anchor_id] = distance
                raw_data = raw_data[5:]

        pk = LocalizationPacket(pk_type, data, decoded_data)
        self.receivedLocationPacket.call(pk)
725805
def mavlink_packet(self, m):
        ''''''

        if not self.mpstate.map:
            # don't draw if no map
            return

        if m.get_type() != 'GIMBAL_REPORT':
            return

        needed = ['ATTITUDE', 'GLOBAL_POSITION_INT']
        for n in needed:
            if not n in self.master.messages:
                return

        # clear the camera icon
        self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('GimbalView'))

        gpi = self.master.messages['GLOBAL_POSITION_INT']
        att = self.master.messages['ATTITUDE']
        vehicle_dcm = Matrix3()
        vehicle_dcm.from_euler(att.roll, att.pitch, att.yaw)

        rotmat_copter_gimbal = Matrix3()
        rotmat_copter_gimbal.from_euler312(m.joint_roll, m.joint_el, m.joint_az)
        gimbal_dcm = vehicle_dcm * rotmat_copter_gimbal

        lat = gpi.lat * 1.0e-7
        lon = gpi.lon * 1.0e-7
        alt = gpi.relative_alt * 1.0e-3

        # ground plane
        ground_plane = Plane()

        # the position of the camera in the air, remembering its a right
        # hand coordinate system, so +ve z is down
        camera_point = Vector3(0, 0, -alt)

        # get view point of camera when not rotated
        view_point = Vector3(1, 0, 0)

        # rotate view_point to form current view vector
        rot_point = gimbal_dcm * view_point

        # a line from the camera to the ground
        line = Line(camera_point, rot_point)

        # find the intersection with the ground
        pt = line.plane_intersection(ground_plane, forward_only=True)
        if pt is None:
            # its pointing up into the sky
            return None

        (view_lat, view_lon) = mp_util.gps_offset(lat, lon, pt.y, pt.x)

        icon = self.mpstate.map.icon('camera-small-red.png')
        self.mpstate.map.add_object(mp_slipmap.SlipIcon('gimbalview',
                                                        (view_lat,view_lon),
                                                        icon, layer='GimbalView', rotation=0, follow=False))
952387
def mavlink_packet(self, m):
        ''''''
        if m.get_type() == "ADSB_VEHICLE":
            id = 'ADSB-' + str(m.ICAO_address)
            if id not in self.threat_vehicles.keys():  # check to see if the vehicle is in the dict
                # if not then add it
                self.threat_vehicles[id] = ADSBVehicle(id=id, state=m.to_dict())
                for mp in self.module_matching('map*'):
                    from MAVProxy.modules.lib import mp_menu
                    from MAVProxy.modules.mavproxy_map import mp_slipmap
                    self.threat_vehicles[id].menu_item = mp_menu.MPMenuItem(name=id, returnkey=None)
                    if m.emitter_type >= 100 and m.emitter_type-100 in obc_icons:
                        icon = mp.map.icon(obc_icons[m.emitter_type-100])
                        threat_radius = get_threat_radius(m.emitter_type-100)
                    else:
                        icon = mp.map.icon(self.threat_vehicles[id].icon)
                        threat_radius = 0
                    popup = mp_menu.MPMenuSubMenu('ADSB', items=[self.threat_vehicles[id].menu_item])
                    # draw the vehicle on the map
                    mp.map.add_object(mp_slipmap.SlipIcon(id, (m.lat * 1e-7, m.lon * 1e-7),
                                                                    icon, layer=3, rotation=m.heading*0.01, follow=False,
                                                                    trail=mp_slipmap.SlipTrail(colour=(0, 255, 255)),
                                                                    popup_menu=popup))
                    if threat_radius > 0:
                        mp.map.add_object(mp_slipmap.SlipCircle(id+":circle", 3,
                                                    (m.lat * 1e-7, m.lon * 1e-7),
                                                    threat_radius, (0, 255, 255), linewidth=1))
            else:  # the vehicle is in the dict
                # update the dict entry
                self.threat_vehicles[id].update(m.to_dict(), self.get_time())
                for mp in self.module_matching('map*'):
                    # update the map
                    ground_alt = mp.ElevationMap.GetElevation(m.lat*1e-7, m.lon*1e-7)
                    alt_amsl = m.altitude * 0.001
                    if alt_amsl > 0:
                        alt = int(alt_amsl - ground_alt)
                        label = str(alt) + "m"
                    else:
                        label = None
                    mp.map.set_position(id, (m.lat * 1e-7, m.lon * 1e-7), rotation=m.heading*0.01, label=label, colour=(0,250,250))
                    mp.map.set_position(id+":circle", (m.lat * 1e-7, m.lon * 1e-7))