handle an incoming mavlink packet
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)
46332
def mavlink_packet(self, m):
        ''''''
        if m.get_type() == 'SERIAL_CONTROL':
            data = m.data[:m.count]
            if m.count > 0:
                s = ''.join(str(chr(x)) for x in data)
                if self.mpstate.system == 'Windows':
                    # strip nsh ansi codes
                    s = s.replace("\033[K","")
                sys.stdout.write(s)
                self.last_packet = time.time()
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)
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))
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
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
100038
def mavlink_packet(self, msg):
        ''''''

        # check for any closed graphs
        for i in range(len(self.graphs) - 1, -1, -1):
            if not self.graphs[i].is_alive():
                self.graphs[i].close()
                self.graphs.pop(i)

        # add data to the rest
        for g in self.graphs:
            g.add_mavlink_packet(msg)
1142275
def _payload(self):
        """"""
        return self.tcp or self.udp or self.icmpv4 or self.icmpv6
707166
def mavlink_packet(self, m):
        ''''''
        now = time.time()
        if m.get_type() == 'REMOTE_LOG_DATA_BLOCK':
            if self.stopped:
                # send a stop packet every second until the other end gets the idea:
                if now - self.time_last_stop_packet_sent > 1:
                    if self.log_settings.verbose:
                        print("DFLogger: Sending stop packet")
                    self.master.mav.remote_log_block_status_send(mavutil.mavlink.MAV_REMOTE_LOG_DATA_BLOCK_STOP,1)
                return

#            if random.random() < 0.1: # drop 1 packet in 10
#                return


            if not self.new_log_started:
                if self.log_settings.verbose:
                    print("DFLogger: Received data packet - starting new log")
                self.start_new_log()
                self.new_log_started = True
            if self.new_log_started == True:
                size = m.block_size
                data = ''.join(str(chr(x)) for x in m.data[:size])
                ofs = size*(m.block_cnt)
                self.logfile.seek(ofs)
                self.logfile.write(data)

                if m.block_cnt in self.missing_blocks:
                    if self.log_settings.verbose:
                        print("DFLogger: Received missing block: %d" % (m.block_cnt,))
                    del self.missing_blocks[m.block_cnt]
                    self.missing_found += 1
                    self.blocks_to_ack_and_nack.append([self.master,m.block_cnt,1,now,None])
                    self.acking_blocks[m.block_cnt] = 1
#                    print("DFLogger: missing blocks: %s" % (str(self.missing_blocks),))
                else:
                    # ACK the block we just got:
                    if m.block_cnt in self.acking_blocks:
                        # already acking this one; we probably sent
                        # multiple nacks and received this one
                        # multiple times
                        pass
                    else:
                        self.blocks_to_ack_and_nack.append([self.master,m.block_cnt,1,now,None])
                        self.acking_blocks[m.block_cnt] = 1
                        # NACK any blocks we haven't seen and should have:
                        if(m.block_cnt - self.block_cnt > 1):
                            for block in range(self.block_cnt+1, m.block_cnt):
                                if block not in self.missing_blocks and \
                                   block not in self.acking_blocks:
                                    self.missing_blocks[block] = 1
                                    if self.log_settings.verbose:
                                        print ("DFLogger: setting %d for nacking" % (block,))
                                    self.blocks_to_ack_and_nack.append([self.master,block,0,now,None])
                        #print "\nmissed blocks: ",self.missing_blocks
                    if self.block_cnt < m.block_cnt:
                        self.block_cnt = m.block_cnt
                self.download += size
        elif not self.new_log_started and not self.stopped:
            # send a start packet every second until the other end gets the idea:
            if now - self.time_last_start_packet_sent > 1:
                if self.log_settings.verbose:
                    print("DFLogger: Sending start packet")
                self.master.mav.remote_log_block_status_send(mavutil.mavlink.MAV_REMOTE_LOG_DATA_BLOCK_START,1)
                self.time_last_start_packet_sent = now
218182
def handle_event(self, packet):
        """"""
        if packet.get('command'):
            task = self.send_command_ack(packet['id'], packet['command'])
            self.loop.create_task(task)
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([])
1281348
def mavlink_packet(self, m):
        ''''''
        from MAVProxy.modules.mavproxy_map import mp_slipmap
        mtype = m.get_type()
        sysid = m.get_srcSystem()

        if mtype == "HEARTBEAT":
            vname = 'plane'
            if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
                vname = 'plane'
            elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER]:
                vname = 'rover'
            elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]:
                vname = 'sub'
            elif m.type in [mavutil.mavlink.MAV_TYPE_SURFACE_BOAT]:
                vname = 'boat'
            elif m.type in [mavutil.mavlink.MAV_TYPE_QUADROTOR,
                            mavutil.mavlink.MAV_TYPE_HEXAROTOR,
                            mavutil.mavlink.MAV_TYPE_OCTOROTOR,
                            mavutil.mavlink.MAV_TYPE_TRICOPTER]:
                vname = 'copter'
            elif m.type in [mavutil.mavlink.MAV_TYPE_COAXIAL]:
                vname = 'singlecopter'
            elif m.type in [mavutil.mavlink.MAV_TYPE_HELICOPTER]:
                vname = 'heli'
            elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
                vname = 'antenna'
            self.vehicle_type_by_sysid[sysid] = vname

        if not sysid in self.vehicle_type_by_sysid:
            self.vehicle_type_by_sysid[sysid] = 'plane'
        self.vehicle_type_name = self.vehicle_type_by_sysid[sysid]

        # this is the beginnings of allowing support for multiple vehicles
        # in the air at the same time
        vehicle = 'Vehicle%u' % m.get_srcSystem()

        if mtype == "SIMSTATE" and self.map_settings.showsimpos:
            self.create_vehicle_icon('Sim' + vehicle, 'green')
            self.map.set_position('Sim' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))

        elif mtype == "AHRS2" and self.map_settings.showahrs2pos:
            self.create_vehicle_icon('AHRS2' + vehicle, 'blue')
            self.map.set_position('AHRS2' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))

        elif mtype == "AHRS3" and self.map_settings.showahrs3pos:
            self.create_vehicle_icon('AHRS3' + vehicle, 'orange')
            self.map.set_position('AHRS3' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))

        elif mtype == "GPS_RAW_INT" and self.map_settings.showgpspos:
            (lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
            if lat != 0 or lon != 0:
                if m.vel > 300 or 'ATTITUDE' not in self.master.messages:
                    cog = m.cog*0.01
                else:
                    cog = math.degrees(self.master.messages['ATTITUDE'].yaw)
                self.create_vehicle_icon('GPS' + vehicle, 'blue')
                self.map.set_position('GPS' + vehicle, (lat, lon), rotation=cog)

        elif mtype == "GPS2_RAW" and self.map_settings.showgps2pos:
            (lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
            if lat != 0 or lon != 0:
                self.create_vehicle_icon('GPS2' + vehicle, 'green')
                self.map.set_position('GPS2' + vehicle, (lat, lon), rotation=m.cog*0.01)

        elif mtype == 'GLOBAL_POSITION_INT':
            (lat, lon, heading) = (m.lat*1.0e-7, m.lon*1.0e-7, m.hdg*0.01)
            self.lat_lon[m.get_srcSystem()] = (lat,lon)
            if abs(lat) > 1.0e-3 or abs(lon) > 1.0e-3:
                self.have_global_position = True
                self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
                if len(self.vehicle_type_by_sysid) > 1:
                    label = str(sysid)
                else:
                    label = None
                self.map.set_position('Pos' + vehicle, (lat, lon), rotation=heading, label=label, colour=(255,255,255))
                self.map.set_follow_object('Pos' + vehicle, self.is_primary_vehicle(m))

        elif mtype == 'LOCAL_POSITION_NED' and not self.have_global_position:
            (lat, lon) = mp_util.gps_offset(0, 0, m.x, m.y)
            self.lat_lon[m.get_srcSystem()] = (lat,lon)
            heading = math.degrees(math.atan2(m.vy, m.vx))
            self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
            self.map.set_position('Pos' + vehicle, (lat, lon), rotation=heading)
            self.map.set_follow_object('Pos' + vehicle, self.is_primary_vehicle(m))

        elif mtype == 'HOME_POSITION':
            (lat, lon) = (m.latitude*1.0e-7, m.longitude*1.0e-7)
            icon = self.map.icon('home.png')
            self.map.add_object(mp_slipmap.SlipIcon('HOME_POSITION',
                                                            (lat,lon),
                                                            icon, layer=3, rotation=0, follow=False))

        elif mtype == "NAV_CONTROLLER_OUTPUT":
            tlayer = 'Trajectory%u' % m.get_srcSystem()
            if (self.master.flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER", "QLAND", "FOLLOW" ] and
                m.get_srcSystem() in self.lat_lon):
                (lat,lon) = self.lat_lon[m.get_srcSystem()]
                trajectory = [ (lat, lon),
                                mp_util.gps_newpos(lat, lon, m.target_bearing, m.wp_dist) ]
                self.map.add_object(mp_slipmap.SlipPolygon('trajectory',
                                                           trajectory, layer=tlayer,
                                                               linewidth=2, colour=(255,0,180)))
            else:
                self.map.add_object(mp_slipmap.SlipClearLayer(tlayer))

        elif mtype == "POSITION_TARGET_GLOBAL_INT":
            # FIXME: base this off SYS_STATUS.MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL?
            if not m.get_srcSystem() in self.lat_lon:
                return
            tlayer = 'PostionTarget%u' % m.get_srcSystem()
            (lat,lon) = self.lat_lon[m.get_srcSystem()]
            if (self.master.flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER", "QLAND", "FOLLOW" ]):
                lat_float = m.lat_int*1e-7
                lon_float = m.lon_int*1e-7
                vec = [ (lat_float, lon_float),
                        (lat, lon) ]
                self.map.add_object(mp_slipmap.SlipPolygon('position_target',
                                                           vec,
                                                           layer=tlayer,
                                                           linewidth=2,
                                                           colour=(0,255,0)))
            else:
                self.map.add_object(mp_slipmap.SlipClearLayer(tlayer))

        if not self.is_primary_vehicle(m):
            # the rest should only be done for the primary vehicle
            return

        # if the waypoints have changed, redisplay
        last_wp_change = self.module('wp').wploader.last_change
        if self.wp_change_time != last_wp_change and abs(time.time() - last_wp_change) > 1:
            self.wp_change_time = last_wp_change
            self.display_waypoints()

            #this may have affected the landing lines from the rally points:
            self.rally_change_time = time.time()

        # if the fence has changed, redisplay
        if self.fence_change_time != self.module('fence').fenceloader.last_change:
            self.display_fence()

        # if the rallypoints have changed, redisplay
        if self.rally_change_time != self.module('rally').rallyloader.last_change:
            self.rally_change_time = self.module('rally').rallyloader.last_change
            icon = self.map.icon('rallypoint.png')
            self.map.add_object(mp_slipmap.SlipClearLayer('RallyPoints'))
            for i in range(self.module('rally').rallyloader.rally_count()):
                rp = self.module('rally').rallyloader.rally_point(i)
                popup = MPMenuSubMenu('Popup',
                                      items=[MPMenuItem('Rally Remove', returnkey='popupRallyRemove'),
                                             MPMenuItem('Rally Move', returnkey='popupRallyMove')])
                self.map.add_object(mp_slipmap.SlipIcon('Rally %u' % (i+1), (rp.lat*1.0e-7, rp.lng*1.0e-7), icon,
                                                                layer='RallyPoints', rotation=0, follow=False,
                                                                popup_menu=popup))

                loiter_rad = self.get_mav_param('WP_LOITER_RAD')

                if self.map_settings.rallycircle:
                    self.map.add_object(mp_slipmap.SlipCircle('Rally Circ %u' % (i+1), 'RallyPoints', (rp.lat*1.0e-7, rp.lng*1.0e-7),
                                                                      loiter_rad, (255,255,0), 2, arrow = self.map_settings.showdirection))

                #draw a line between rally point and nearest landing point
                nearest_land_wp = None
                nearest_distance = 10000000.0
                for j in range(self.module('wp').wploader.count()):
                    w = self.module('wp').wploader.wp(j)
                    if (w.command == 21): #if landing waypoint
                        #get distance between rally point and this waypoint
                        dis = mp_util.gps_distance(w.x, w.y, rp.lat*1.0e-7, rp.lng*1.0e-7)
                        if (dis < nearest_distance):
                            nearest_land_wp = w
                            nearest_distance = dis

                if nearest_land_wp is not None:
                    points = []
                    #tangential approach?
                    if self.get_mav_param('LAND_BREAK_PATH') == 0:
                        theta = math.degrees(math.atan(loiter_rad / nearest_distance))
                        tan_dis = math.sqrt(nearest_distance * nearest_distance - (loiter_rad * loiter_rad))

                        ral_bearing = mp_util.gps_bearing(nearest_land_wp.x, nearest_land_wp.y,rp.lat*1.0e-7, rp.lng*1.0e-7)

                        points.append(mp_util.gps_newpos(nearest_land_wp.x,nearest_land_wp.y, ral_bearing + theta, tan_dis))

                    else: #not tangential approach
                        points.append((rp.lat*1.0e-7, rp.lng*1.0e-7))

                    points.append((nearest_land_wp.x, nearest_land_wp.y))
                    self.map.add_object(mp_slipmap.SlipPolygon('Rally Land %u' % (i+1), points, 'RallyPoints', (255,255,0), 2))

        # check for any events from the map
        self.map.check_events()
108676
def handle_mavlink_packet(self, master, m):
        ''''''
        if m.get_type() == 'PARAM_VALUE':
            value = self.handle_px4_param_value(m)
            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 self.fetch_set is not None:
                self.fetch_set.discard(m.param_index)
            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)] = value
            if param_id in self.fetch_one and self.fetch_one[param_id] > 0:
                self.fetch_one[param_id] -= 1
                if isinstance(value, float):
                    print("%s = %.7f" % (param_id, value))
                else:
                    print("%s = %s" % (param_id, str(value)))
            if added_new_parameter and len(self.mav_param_set) == m.param_count:
                print("Received %u parameters" % m.param_count)
                if self.logdir is not None:
                    self.mav_param.save(os.path.join(self.logdir, self.parm_file), '*', verbose=True)
                self.fetch_set = None
            if self.fetch_set is not None and len(self.fetch_set) == 0:
                self.fetch_check(master, force=True)
        elif m.get_type() == 'HEARTBEAT':
            if m.get_srcComponent() == 1:
                # remember autopilot types so we can handle PX4 parameters
                self.autopilot_type_by_sysid[m.get_srcSystem()] = m.autopilot
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)
441132
def mavlink_packet(self, m):
        ''''''
        mtype = m.get_type()
        if mtype in ['WAYPOINT_COUNT','MISSION_COUNT']:
            if self.wp_op is None:
                self.console.error("No waypoint load started")
            else:
                self.wploader.clear()
                self.wploader.expected_count = m.count
                self.console.writeln("Requesting %u waypoints t=%s now=%s" % (m.count,
                                                                                 time.asctime(time.localtime(m._timestamp)),
                                                                                 time.asctime()))
                self.master.waypoint_request_send(0)

        elif mtype in ['WAYPOINT', 'MISSION_ITEM'] and self.wp_op != None:
            if m.seq > self.wploader.count():
                self.console.writeln("Unexpected waypoint number %u - expected %u" % (m.seq, self.wploader.count()))
            elif m.seq < self.wploader.count():
                # a duplicate
                pass
            else:
                self.wploader.add(m)
            if m.seq+1 < self.wploader.expected_count:
                self.master.waypoint_request_send(m.seq+1)
            else:
                if self.wp_op == 'list':
                    for i in range(self.wploader.count()):
                        w = self.wploader.wp(i)
                        print("%u %u %.10f %.10f %f p1=%.1f p2=%.1f p3=%.1f p4=%.1f cur=%u auto=%u" % (
                            w.command, w.frame, w.x, w.y, w.z,
                            w.param1, w.param2, w.param3, w.param4,
                            w.current, w.autocontinue))
                    if self.logdir != None:
                        waytxt = os.path.join(self.logdir, 'way.txt')
                        self.save_waypoints(waytxt)
                        print("Saved waypoints to %s" % waytxt)
                elif self.wp_op == "save":
                    self.save_waypoints(self.wp_save_filename)
                self.wp_op = None

        elif mtype in ["WAYPOINT_REQUEST", "MISSION_REQUEST"]:
            self.process_waypoint_request(m, self.master)

        elif mtype in ["WAYPOINT_CURRENT", "MISSION_CURRENT"]:
            if m.seq != self.last_waypoint:
                self.last_waypoint = m.seq
                if self.settings.wpupdates:
                    self.say("waypoint %u" % m.seq,priority='message')
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)
946692
private void handleApplicationCommandRequest(SerialMessage incomingMessage) {
		logger.trace("Handle Message Application Command Request");
		int nodeId = incomingMessage.getMessagePayloadByte(1);
		logger.debug("Application Command Request from Node " + nodeId);
		ZWaveNode node = getNode(nodeId);
		
		if (node == null) {
			logger.warn("Node {} not initialized yet, ignoring message.", nodeId);
			return;
		}
		
		node.resetResendCount();
		
		int commandClassCode = incomingMessage.getMessagePayloadByte(3);
		ZWaveCommandClass.CommandClass commandClass = ZWaveCommandClass.CommandClass.getCommandClass(commandClassCode);

		if (commandClass == null) {
			logger.error(String.format("Unsupported command class 0x%02x", commandClassCode));
			return;
		}
		
		logger.debug(String.format("Incoming command class %s (0x%02x)", commandClass.getLabel(), commandClass.getKey()));
		ZWaveCommandClass zwaveCommandClass =  node.getCommandClass(commandClass);
		
		// We got an unsupported command class, return.
		if (zwaveCommandClass == null) {
			logger.error(String.format("Unsupported command class %s (0x%02x)", commandClass.getLabel(), commandClassCode));
			return;
		}
		
		logger.trace("Found Command Class {}, passing to handleApplicationCommandRequest", zwaveCommandClass.getCommandClass().getLabel());
		zwaveCommandClass.handleApplicationCommandRequest(incomingMessage, 4, 1);

		if (incomingMessage.getMessageClass() == this.lastSentMessage.getExpectedReply() && nodeId == this.lastSentMessage.getMessageNode() && !incomingMessage.isTransActionCanceled()) {
				notifyEventListeners(new ZWaveEvent(ZWaveEventType.TRANSACTION_COMPLETED_EVENT, this.lastSentMessage.getMessageNode(), 1, this.lastSentMessage));
				transactionCompleted.release();
				logger.trace("Released. Transaction completed permit count -> {}", transactionCompleted.availablePermits());
		}
	}