handle mavlink packets
332109
def mavlink_packet(self, m):
        ''''''
        mtype = m.get_type()
        if mtype not in self.counts:
            self.counts[mtype] = 0
        self.counts[mtype] += 1
581774
def _recv(self):
                ''''''
                from . import mavutil
                start_time = time.time()
                while time.time() < start_time + self.timeout:
                        m = self.mav.recv_match(condition='SERIAL_CONTROL.count!=0',
                                                type='SERIAL_CONTROL', blocking=False, timeout=0)
                        if m is not None and m.count != 0:
                                break
                        self.mav.mav.serial_control_send(self.port,
                                                         mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
                                                         mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND,
                                                         0,
                                                         0,
                                                         0, [0]*70)
                        m = self.mav.recv_match(condition='SERIAL_CONTROL.count!=0',
                                                type='SERIAL_CONTROL', blocking=True, timeout=0.01)
                        if m is not None and m.count != 0:
                                break
                if m is not None:
                        if self._debug > 2:
                                print(m)
                        data = m.data[:m.count]
                        self.buf += ''.join(str(chr(x)) for x in data)
411463
def data16_send(self, type, len, data, force_mavlink1=False):
                '''
                

                '''
                return self.send(self.data16_encode(type, len, data), force_mavlink1=force_mavlink1)
953169
def mavlink_packet(self, m):
        ''''''
        if m.get_type() in ['GLOBAL_POSITION_INT', 'SCALED_PRESSURE']:
            connection = self.find_connection()
            if not connection:
                return
            if m.get_srcSystem() != connection.target_system:
                connection.mav.send(m)
1333411
def mavlink_packet(self, m):
        ''''''
        #do nothing if not caibrating
        if (self.calibrating == False):
            return

        if m.get_type() == 'RC_CHANNELS_RAW':
            for i in range(1,self.num_channels+1):
                v = getattr(m, 'chan%u_raw' % i)

                if self.get_cal_min(i) > v:
                    self.set_cal_min(i,v)
                    self.console.writeln("Calibrating: RC%u_MIN=%u" % (i, v))
                if self.get_cal_max(i) < v:
                    self.set_cal_max(i,v)
                    self.console.writeln("Calibrating: RC%u_MAX=%u" % (i, v))
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))
697222
def master_callback(self, m, master):
        ''''''

        # see if it is handled by a specialised sysid connection
        sysid = m.get_srcSystem()
        mtype = m.get_type()
        if sysid in self.mpstate.sysid_outputs:
            self.mpstate.sysid_outputs[sysid].write(m.get_msgbuf())
            if mtype == "GLOBAL_POSITION_INT":
                for modname in 'map', 'asterix', 'NMEA', 'NMEA2':
                    mod = self.module(modname)
                    if mod is not None:
                        mod.set_secondary_vehicle_position(m)
            return

        if getattr(m, '_timestamp', None) is None:
            master.post_message(m)
        self.status.counters['MasterIn'][master.linknum] += 1

        if mtype == 'GLOBAL_POSITION_INT':
            # send GLOBAL_POSITION_INT to 2nd GCS for 2nd vehicle display
            for sysid in self.mpstate.sysid_outputs:
                self.mpstate.sysid_outputs[sysid].write(m.get_msgbuf())

            if self.mpstate.settings.fwdpos:
                for link in self.mpstate.mav_master:
                    if link != master:
                        link.write(m.get_msgbuf())

        # and log them
        if mtype not in dataPackets and self.mpstate.logqueue:
            # put link number in bottom 2 bits, so we can analyse packet
            # delay in saved logs
            usec = self.get_usec()
            usec = (usec & ~3) | master.linknum
            self.mpstate.logqueue.put(bytearray(struct.pack('>Q', usec) + m.get_msgbuf()))

        # keep the last message of each type around
        self.status.msgs[mtype] = m
        if mtype not in self.status.msg_count:
            self.status.msg_count[mtype] = 0
        self.status.msg_count[mtype] += 1

        if m.get_srcComponent() == mavutil.mavlink.MAV_COMP_ID_GIMBAL and mtype == 'HEARTBEAT':
            # silence gimbal heartbeat packets for now
            return

        if getattr(m, 'time_boot_ms', None) is not None and self.settings.target_system == m.get_srcSystem():
            # update link_delayed attribute
            self.handle_msec_timestamp(m, master)

        if mtype in activityPackets:
            if master.linkerror:
                master.linkerror = False
                self.say("link %s OK" % (self.link_label(master)))
            self.status.last_message = time.time()
            master.last_message = self.status.last_message

        if master.link_delayed and self.mpstate.settings.checkdelay:
            # don't process delayed packets that cause double reporting
            if mtype in delayedPackets:
                return

        self.master_msg_handling(m, master)

        # don't pass along bad data
        if mtype != 'BAD_DATA':
            # pass messages along to listeners, except for REQUEST_DATA_STREAM, which
            # would lead a conflict in stream rate setting between mavproxy and the other
            # GCS
            if self.mpstate.settings.mavfwd_rate or mtype != 'REQUEST_DATA_STREAM':
                if mtype not in self.no_fwd_types:
                    for r in self.mpstate.mav_outputs:
                        r.write(m.get_msgbuf())

            sysid = m.get_srcSystem()
            target_sysid = self.target_system

            # pass to modules
            for (mod,pm) in self.mpstate.modules:
                if not hasattr(mod, 'mavlink_packet'):
                    continue
                if not mod.multi_vehicle and sysid != target_sysid:
                    # only pass packets not from our target to modules that
                    # have marked themselves as being multi-vehicle capable
                    continue
                try:
                    mod.mavlink_packet(m)
                except Exception as msg:
                    if self.mpstate.settings.moddebug == 1:
                        print(msg)
                    elif self.mpstate.settings.moddebug > 1:
                        exc_type, exc_value, exc_traceback = sys.exc_info()
                        traceback.print_exception(exc_type, exc_value, exc_traceback,
                                                  limit=2, file=sys.stdout)
891281
def data32_send(self, type, len, data, force_mavlink1=False):
                '''
                

                '''
                return self.send(self.data32_encode(type, len, data), force_mavlink1=force_mavlink1)
192632
def handle_mavlink_packet(self, master, m):
        ''''''
        if m.get_type() == 'PARAM_VALUE':
            param_id = "%.16s" % m.param_id
            # Note: the xml specifies param_index is a uint16, so -1 in that field will show as 65535
            # We accept both -1 and 65535 as 'unknown index' to future proof us against someday having that
            # xml fixed.
            if m.param_index != -1 and m.param_index != 65535 and m.param_index not in self.mav_param_set:
                added_new_parameter = True
                self.mav_param_set.add(m.param_index)
            else:
                added_new_parameter = False
            if m.param_count != -1:
                self.mav_param_count = m.param_count
            self.mav_param[str(param_id)] = m.param_value
            if self.fetch_one > 0:
                self.fetch_one -= 1
                print("%s = %f" % (param_id, m.param_value))
            if added_new_parameter and len(self.mav_param_set) == m.param_count:
                print("Received %u parameters" % m.param_count)
                if self.logdir != None:
                    self.mav_param.save(os.path.join(self.logdir, self.parm_file), '*', verbose=True)
1089223
def packet_in_handler(self, req_pkt, req_igmp, msg):
        """"""
        dpid = msg.datapath.id
        ofproto = msg.datapath.ofproto
        if ofproto.OFP_VERSION == ofproto_v1_0.OFP_VERSION:
            in_port = msg.in_port
        else:
            in_port = msg.match['in_port']

        log = "SW=%s PORT=%d IGMP received. " % (
            dpid_to_str(dpid), in_port)
        self.logger.debug(str(req_igmp))
        if igmp.IGMP_TYPE_QUERY == req_igmp.msgtype:
            self.logger.info(log + "[QUERY]")
            (req_ipv4, ) = req_pkt.get_protocols(ipv4.ipv4)
            (req_eth, ) = req_pkt.get_protocols(ethernet.ethernet)
            self._do_query(req_igmp, req_ipv4, req_eth, in_port, msg)
        elif (igmp.IGMP_TYPE_REPORT_V1 == req_igmp.msgtype or
              igmp.IGMP_TYPE_REPORT_V2 == req_igmp.msgtype):
            self.logger.info(log + "[REPORT]")
            self._do_report(req_igmp, in_port, msg)
        elif igmp.IGMP_TYPE_LEAVE == req_igmp.msgtype:
            self.logger.info(log + "[LEAVE]")
            self._do_leave(req_igmp, in_port, msg)
        elif igmp.IGMP_TYPE_REPORT_V3 == req_igmp.msgtype:
            self.logger.info(log + "V3 is not supported yet.")
            self._do_flood(in_port, msg)
        else:
            self.logger.info(log + "[unknown type:%d]",
                             req_igmp.msgtype)
            self._do_flood(in_port, msg)
540848
def handle_px4_param_value(self, m):
        ''''''
        if m.param_type == mavutil.mavlink.MAV_PARAM_TYPE_REAL32:
            # already right type
            return m.param_value
        is_px4_params = False
        if m.get_srcComponent() in [mavutil.mavlink.MAV_COMP_ID_UDP_BRIDGE]:
            # ESP8266 uses PX4 style parameters
            is_px4_params = True
        sysid = m.get_srcSystem()
        if self.autopilot_type_by_sysid.get(sysid,-1) in [mavutil.mavlink.MAV_AUTOPILOT_PX4]:
            is_px4_params = True
        if not is_px4_params:
            return m.param_value
        # try to extract px4 param value
        value = m.param_value
        try:
            v = struct.pack(">f", value)
        except Exception:
            return value
        if m.param_type == mavutil.mavlink.MAV_PARAM_TYPE_UINT8:
            value, = struct.unpack(">B", v[3:])
        elif m.param_type == mavutil.mavlink.MAV_PARAM_TYPE_INT8:
            value, = struct.unpack(">b", v[3:])
        elif m.param_type == mavutil.mavlink.MAV_PARAM_TYPE_UINT16:
            value, = struct.unpack(">H", v[2:])
        elif m.param_type == mavutil.mavlink.MAV_PARAM_TYPE_INT16:
            value, = struct.unpack(">h", v[2:])
        elif m.param_type == mavutil.mavlink.MAV_PARAM_TYPE_UINT32:
            value, = struct.unpack(">I", v[0:])
        elif m.param_type == mavutil.mavlink.MAV_PARAM_TYPE_INT32:
            value, = struct.unpack(">i", v[0:])
        # can't pack other types

        # remember type for param set
        self.param_types[m.param_id.upper()] = m.param_type
        return value
65016
def handle(self, pkt, raddress, rport):
        """"""
        if not self.context.tidport:
            self.context.tidport = rport
            log.info("Set remote port for session to %s" % rport)

        # Now check the packet type and dispatch it properly.
        if isinstance(pkt, TftpPacketOACK):
            log.info("Received OACK from server")
            try:
                self.handleOACK(pkt)
            except TftpException as err:
                log.error("Failed to negotiate options: %s" % str(err))
                self.sendError(TftpErrors.FailedNegotiation)
                raise
            else:
                log.debug("Sending ACK to OACK")

                self.sendACK(blocknumber=0)

                log.debug("Changing state to TftpStateExpectDAT")
                return TftpStateExpectDAT(self.context)

        elif isinstance(pkt, TftpPacketDAT):
            # If there are any options set, then the server didn't honour any
            # of them.
            log.info("Received DAT from server")
            if self.context.options:
                log.info("Server ignored options, falling back to defaults")
                self.context.options = { 'blksize': DEF_BLKSIZE }
            return self.handleDat(pkt)

        # Every other packet type is a problem.
        elif isinstance(pkt, TftpPacketACK):
            # Umm, we ACK, the server doesn't.
            self.sendError(TftpErrors.IllegalTftpOp)
            raise TftpException("Received ACK from server while in download")

        elif isinstance(pkt, TftpPacketWRQ):
            self.sendError(TftpErrors.IllegalTftpOp)
            raise TftpException("Received WRQ from server while in download")

        elif isinstance(pkt, TftpPacketERR):
            self.sendError(TftpErrors.IllegalTftpOp)
            log.debug("Received ERR packet: %s", pkt)
            if pkt.errorcode == TftpErrors.FileNotFound:
                raise TftpFileNotFoundError("File not found")
            else:
                raise TftpException("Received ERR from server: {}".format(pkt))

        else:
            self.sendError(TftpErrors.IllegalTftpOp)
            raise TftpException("Received unknown packet type from server: %s" % pkt)

        # By default, no state change.
        return self
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))
1330922
def mavlink_packet(self, m):
        """"""
        mtype = m.get_type()
        if mtype == 'GPS_RAW':
            (self.lat, self.lon) = (m.lat, m.lon)
        elif mtype == 'GPS_RAW_INT':
            (self.lat, self.lon) = (m.lat / 1.0e7, m.lon / 1.0e7)
        elif mtype == "VFR_HUD":
            self.heading = m.heading
            self.alt = m.alt
            self.airspeed = m.airspeed
            self.groundspeed = m.groundspeed
503364
def _handle_datapath(self, inport, packet):
        '''
        
        '''
        inport = self._switchyard_net.port_by_name(inport)
        portnum = inport.ifnum
        log_info("Processing packet: {}->{}".format(portnum, packet))

        actions = None
        for tnum,t in enumerate(self._tables):
            actions = t.match_packet(portnum, packet)

            # FIXME: this is all wrong/incomplete

            # if match: Update counters Execute instructions:
            #           update action set
            #           update packet/match set fields
            #            update metadata
            # if no match and table miss entry exists, do the above
            # otherwise, drop the packet
        # actions = self._table.match_packet(portnum, packet)
        if actions is None:
            self._send_packet_in(portnum, packet)
        else:
            self._datapath_action(portnum, packet, actions=actions)
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))
343548
def handle_stream(self, stream, address):
        '''
        
        '''
        log.trace('Req client %s connected', address)
        self.clients.append((stream, address))
        unpacker = msgpack.Unpacker()
        try:
            while True:
                wire_bytes = yield stream.read_bytes(4096, partial=True)
                unpacker.feed(wire_bytes)
                for framed_msg in unpacker:
                    if six.PY3:
                        framed_msg = salt.transport.frame.decode_embedded_strs(
                            framed_msg
                        )
                    header = framed_msg['head']
                    self.io_loop.spawn_callback(self.message_handler, stream, header, framed_msg['body'])

        except StreamClosedError:
            log.trace('req client disconnected %s', address)
            self.clients.remove((stream, address))
        except Exception as e:
            log.trace('other master-side exception: %s', e)
            self.clients.remove((stream, address))
            stream.close()