process packets from MAVLink slaves, forwarding to the master
13109
def process_mavlink(slave):
    ''''''
    try:
        buf = slave.recv()
    except socket.error:
        return
    try:
        global mavversion
        if slave.first_byte and mavversion is None:
            slave.auto_mavlink_version(buf)
        msgs = slave.mav.parse_buffer(buf)
    except mavutil.mavlink.MAVError as e:
        mpstate.console.error("Bad MAVLink slave message from %s: %s" % (slave.address, e.message))
        return
    if msgs is None:
        return
    if mpstate.settings.mavfwd and not mpstate.status.setup_mode:
        for m in msgs:
            mpstate.master().write(m.get_msgbuf())
            if mpstate.status.watch:
                for msg_type in mpstate.status.watch:
                    if fnmatch.fnmatch(m.get_type().upper(), msg_type.upper()):
                        mpstate.console.writeln('> '+ str(m))
                        break
    mpstate.status.counters['Slave'] += 1
173558
def ping_master(master):
    '''
    
    '''
    if master is None or master == '':
        return False

    opts = copy.deepcopy(__opts__)
    opts['master'] = master
    if 'master_ip' in opts:  # avoid 'master ip changed' warning
        del opts['master_ip']
    opts.update(salt.minion.prep_ip_port(opts))
    try:
        opts.update(salt.minion.resolve_dns(opts, fallback=False))
    except Exception:
        return False

    timeout = opts.get('auth_timeout', 60)
    load = {'cmd': 'ping'}

    result = False
    channel = salt.transport.client.ReqChannel.factory(opts, crypt='clear')
    try:
        payload = channel.send(load, tries=0, timeout=timeout)
        result = True
    except Exception as e:
        pass

    if result:
        event = salt.utils.event.get_event('minion', opts=__opts__, listen=False)
        event.fire_event({'master': master}, salt.minion.master_event(type='failback'))

    return result
301433
def mavlink_packet(self, m):
        ''''''
        type = m.get_type()
        if type in ['COMMAND_ACK']:
            if m.command == mavutil.mavlink.MAV_CMD_DO_GO_AROUND:
                if (m.result == 0 and self.abort_ack_received == False):
                    self.say("Landing Abort Command Successfully Sent.")
                    self.abort_ack_received = True
                elif (m.result != 0 and self.abort_ack_received == False):
                    self.say("Landing Abort Command Unsuccessful.")

            elif m.command == mavutil.mavlink.MAV_CMD_DO_RALLY_LAND:
                if (m.result == 0):
                    self.say("Landing.")
105726
def cmd_join(self, connection, sender, target, payload):
        
        if payload:
            connection.join(payload)
        else:
            raise ValueError("No channel given")
1182882
def respond(self, packet, peer, flags=0):
        
        self.sock.sendto(packet, flags, peer)
790764
def mavlink_packet(self, m):
        ''''''
        if m.get_type() == 'VFR_HUD' and ('GPS_RAW' in self.status.msgs or 'GPS_RAW_INT' in self.status.msgs):
            self.check_heading(m)
            if self.settings.speedreporting:
                if m.airspeed != 0:
                    speed = m.airspeed
                else:
                    speed = m.groundspeed
                self.report_change('speed', speed, maxdiff=2, deltat=2)
        if self.status.watch == "sensors" and time.time() > self.sensors_state.last_watch + 1:
            self.sensors_state.last_watch = time.time()
            self.cmd_sensors([])
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
1052962
def run(self):
        """"""
        # self.log("ready to receive.")
        # we use a runloop instead of synchronous recv so stopping the node mid-recv is possible
        read_ready = None

        while self.keep_listening:
            try:
                read_ready, w, x = select.select([self.recv_socket], [], [], 0.01)
            except Exception:
                # catch timeouts
                pass

            if read_ready:
                packet, addr = read_ready[0].recvfrom(4096)
                if addr[1] == self.port:
                    # for each address listening to this link
                    for mac_addr, recv_queue in self.inq.items():
                        recv_queue.put(packet)  # put packet in node's recv queue
                else:
                    pass
193120
def connect_master(self, failed=False):
        '''
        
        '''
        master, self.pub_channel = yield self.eval_master(self.opts, self.timeout, self.safe, failed)
        yield self._post_master_init(master)
2022
def fire_master(data, tag, preload=None, timeout=60):
    '''
    
    '''
    if (__opts__.get('local', None) or __opts__.get('file_client', None) == 'local') and not __opts__.get('use_master_when_local', False):
        #  We can't send an event if we're in masterless mode
        log.warning('Local mode detected. Event with tag %s will NOT be sent.', tag)
        return False

    if preload or __opts__.get('__cli') == 'salt-call':
        # If preload is specified, we must send a raw event (this is
        # slower because it has to independently authenticate)
        if 'master_uri' not in __opts__:
            __opts__['master_uri'] = 'tcp://{ip}:{port}'.format(
                    ip=salt.utils.zeromq.ip_bracket(__opts__['interface']),
                    port=__opts__.get('ret_port', '4506')  # TODO, no fallback
                    )
        masters = list()
        ret = None
        if 'master_uri_list' in __opts__:
            for master_uri in __opts__['master_uri_list']:
                masters.append(master_uri)
        else:
            masters.append(__opts__['master_uri'])
        auth = salt.crypt.SAuth(__opts__)
        load = {'id': __opts__['id'],
                'tag': tag,
                'data': data,
                'tok': auth.gen_token(b'salt'),
                'cmd': '_minion_event'}

        if isinstance(preload, dict):
            load.update(preload)

        for master in masters:
            channel = salt.transport.client.ReqChannel.factory(__opts__, master_uri=master)
            try:
                channel.send(load, timeout=timeout)
                # channel.send was successful.
                # Ensure ret is True.
                ret = True
            except Exception:
                # only set a False ret if it hasn't been sent atleast once
                if ret is None:
                    ret = False
            finally:
                channel.close()
        return ret
    else:
        # Usually, we can send the event via the minion, which is faster
        # because it is already authenticated
        try:
            me = salt.utils.event.MinionEvent(__opts__, listen=False, keep_loop=True)
            return me.fire_event({'data': data, 'tag': tag, 'events': None, 'pretag': None}, 'fire_master')
        except Exception:
            exc_type, exc_value, exc_traceback = sys.exc_info()
            lines = traceback.format_exception(exc_type, exc_value, exc_traceback)
            log.debug(lines)
            return False
1221920
def handle_packet(self, packet):
        """"""
        if self.packet_callback:
            # forward to callback
            self.packet_callback(packet)
        else:
            print('packet', packet)
600264
def master_call(self, **kwargs):
        '''
        
        '''
        load = kwargs
        load['cmd'] = 'wheel'
        interface = self.opts['interface']
        if interface == '0.0.0.0':
            interface = '127.0.0.1'
        master_uri = 'tcp://{}:{}'.format(
            salt.utils.zeromq.ip_bracket(interface),
            six.text_type(self.opts['ret_port'])
        )
        channel = salt.transport.client.ReqChannel.factory(self.opts,
                                                           crypt='clear',
                                                           master_uri=master_uri,
                                                           usage='master_call')
        try:
            ret = channel.send(load)
        finally:
            channel.close()
        if isinstance(ret, collections.Mapping):
            if 'error' in ret:
                salt.utils.error.raise_error(**ret['error'])
        return ret
1367704
def slave_for(self, service):
        """"""
        # TODO: make it coroutine and connect minsize connections
        if service not in self._slaves:
            self._slaves[service] = ManagedPool(
                self, service, is_master=False,
                db=self._redis_db,
                password=self._redis_password,
                encoding=self._redis_encoding,
                minsize=self._redis_minsize,
                maxsize=self._redis_maxsize,
                ssl=self._redis_ssl,
                parser=self._parser_class,
                loop=self._loop)
        return self._slaves[service]
445411
def master_callback(self, m, master):
        ''''''

        # see if it is handled by a specialised sysid connection
        sysid = m.get_srcSystem()
        if sysid in self.mpstate.sysid_outputs:
            self.mpstate.sysid_outputs[sysid].write(m.get_msgbuf())
            return

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

        mtype = m.get_type()

        # 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(str(struct.pack('>Q', usec) + m.get_msgbuf()))

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

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

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

        if mtype in activityPackets:
            if master.linkerror:
                master.linkerror = False
                self.say("link %u OK" % (master.linknum+1))
            self.status.last_message = time.time()
            master.last_message = self.status.last_message

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

        if mtype == 'HEARTBEAT' and m.get_srcSystem() != 255:
            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')

            if self.status.heartbeat_error:
                self.status.heartbeat_error = False
                self.say("heartbeat OK")
            if master.linkerror:
                master.linkerror = False
                self.say("link %u OK" % (master.linknum+1))

            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 and time.time() > self.status.last_mode_announce + 2:
                self.status.flightmode = master.flightmode
                self.status.last_mode_announce = time.time()
                if self.mpstate.functions.input_handler is None:
                    self.mpstate.rl.set_prompt(self.status.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,
                            mavutil.mavlink.MAV_TYPE_SUBMARINE]:
                self.mpstate.vehicle_type = 'rover'
                self.mpstate.vehicle_name = 'APMrover2'
            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]:
                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:
                self.mpstate.console.writeln("APM: %s" % m.text, bg='red')
                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 == "BAD_DATA":
            if self.mpstate.settings.shownoise and mavutil.all_printable(m.data):
                self.mpstate.console.write(str(m.data), bg='red')
        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")
        else:
            #self.mpstate.console.writeln("Got MAVLink msg: %s" % m)
            pass

        if self.status.watch is not None:
            if fnmatch.fnmatch(m.get_type().upper(), self.status.watch.upper()):
                self.mpstate.console.writeln('< '+str(m))

        # 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 not mtype in self.no_fwd_types:
                    for r in self.mpstate.mav_outputs:
                        r.write(m.get_msgbuf())

            # pass to modules
            for (mod,pm) in self.mpstate.modules:
                if not hasattr(mod, 'mavlink_packet'):
                    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)
752282
def send(self, mavmsg, force_mavlink1=False):
                ''''''
                buf = mavmsg.pack(self, force_mavlink1=force_mavlink1)
                self.file.write(buf)
                self.seq = (self.seq + 1) % 256
                self.total_packets_sent += 1
                self.total_bytes_sent += len(buf)
                if self.send_callback:
                    self.send_callback(mavmsg, *self.send_callback_args, **self.send_callback_kwargs)
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)
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))