Python IvyMessagesInterface.shutdown方法代码示例

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class Ivy2RedisServer():
    def __init__(self, redishost, redisport, verbose=False):
        self.verbose = verbose
        self.interface = IvyMessagesInterface("Ivy2Redis")
        self.interface.subscribe(self.message_recv)
        self.r = redis.StrictRedis(host=redishost, port=redisport, db=0)
        self.keep_running = True
        print("Connected to redis server %s on port %i" % (redishost, redisport))

    def message_recv(self, ac_id, msg):
        # if ac_id is not 0 (i.e. telemetry from an aircraft) include it in the key
        # don't add it to the key for ground messages
        if ac_id:
            key = "{0}.{1}.{2}".format(msg.msg_class, msg.name, ac_id)
        else:
            key = "{0}.{1}".format(msg.msg_class, msg.name)
        if self.verbose:
            print("received message, key=%s, msg=%s" % (key, msg.to_json(payload_only=True)))
            sys.stdout.flush()
        self.r.publish(key, msg.to_json(payload_only=True))
        self.r.set(key, msg.to_json(payload_only=True))

    def run(self):
        while self.keep_running:
            time.sleep(0.1)

    def stop(self):
        self.keep_running = False
        self.interface.shutdown()

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class IvyRequester(object):
    def __init__(self, interface=None):
        self._interface = interface
        if interface is None:
            self._interface = IvyMessagesInterface("ivy requester")
        self.ac_list = []

    def __del__(self):
        self.shutdown()

    def shutdown(self):
        if self._interface is not None:
            print("Shutting down ivy interface...")
            self._interface.shutdown()
            self._interface = None

    def get_aircrafts(self):

        def aircrafts_cb(ac_id, msg):
            self.ac_list = [int(a) for a in msg['ac_list'].split(',') if a]
            print("aircrafts: {}".format(self.ac_list))

        self._interface.subscribe(aircrafts_cb, "(.*AIRCRAFTS .*)")
        sender = 'get_aircrafts'
        request_id = '42_1' # fake request id, should be PID_index
        self._interface.send("{} {} AIRCRAFTS_REQ".format(sender, request_id))
        # hack: sleep briefly to wait for answer
        sleep(0.1)
        return self.ac_list

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class RadioWatchFrame(wx.Frame):
    def message_recv(self, ac_id, msg):
        if msg.name == "ROTORCRAFT_STATUS":
            self.rc_status = int(msg['rc_status'])
        if self.rc_status != 0 and not self.alertChannel.get_busy():
            self.warn_timer = wx.CallLater(5, self.rclink_alert)
            # else:
            # self.notification.close()

    def gui_update(self):
        self.rc_statusText.SetLabel(["OK", "LOST", "REALLY LOST"][self.rc_status])
        self.update_timer.Restart(UPDATE_INTERVAL)

    def rclink_alert(self):
        self.alertChannel.queue(self.alertSound)
        self.notification.show()
        time.sleep(5)

    def setFont(self, control):
        font = control.GetFont()
        size = font.GetPointSize()
        font.SetPointSize(size * 1.4)
        control.SetFont(font)

    def __init__(self):
        wx.Frame.__init__(self, id=-1, parent=None, name=u'RCWatchFrame',
                          size=wx.Size(WIDTH, HEIGHT), title=u'RC Status')
        self.Bind(wx.EVT_CLOSE, self.OnClose)

        self.rc_statusText = wx.StaticText(self, -1, "UNKWN")

        pygame.mixer.init()
        self.alertSound = pygame.mixer.Sound("crossing.wav")
        self.alertChannel = pygame.mixer.Channel(False)

        self.setFont(self.rc_statusText)

        self.notification = pynotify.Notification("RC Link Warning!",
                                                  "RC Link status not OK!",
                                                  "dialog-warning")

        self.rc_status = -1

        pynotify.init("RC Status")

        sizer = wx.BoxSizer(wx.VERTICAL)
        sizer.Add(self.rc_statusText, 1, wx.EXPAND)
        self.SetSizer(sizer)
        sizer.Layout()
        self.interface = IvyMessagesInterface("radiowatchframe")
        self.interface.subscribe(self.message_recv)
        self.update_timer = wx.CallLater(UPDATE_INTERVAL, self.gui_update)

    def OnClose(self, event):
        self.interface.shutdown()
        self.Destroy()

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class PprzlinkDatastream(AbstractDatastream):
    def initialize(self):
        name = 'pprzlink'
        self._interface = IvyMessagesInterface("pprzlink_morse")

    @classproperty
    def _type_url(cls):
        return "http://docs.paparazziuav.org/latest/paparazzi_messages.html#" + cls._type_name

    def finalize(self):
        if self._interface is not None:
            self._interface.shutdown()
            self._interface = None

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class WaypointMover(object):
    def __init__(self, verbose=False):
        self.verbose = verbose
        self._interface = IvyMessagesInterface("WaypointMover")

    def shutdown(self):
        print("Shutting down ivy interface...")
        self._interface.shutdown()

    def __del__(self):
        self.shutdown()

    def move_waypoint(self, ac_id, wp_id, lat, lon, alt):
        msg = PprzMessage("ground", "MOVE_WAYPOINT")
        msg['ac_id'] = ac_id
        msg['wp_id'] = wp_id
        msg['lat'] = lat
        msg['long'] = lon
        msg['alt'] = alt
        print("Sending message: %s" % msg)
        self._interface.send(msg)

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class CommandSender(IvyMessagesInterface):
    def __init__(self, verbose=False, callback = None):
        self.verbose = verbose
        self.callback = callback
        self._interface = IvyMessagesInterface("Mission Commander", start_ivy=False)
        self._interface.subscribe(self.message_recv)
        self._interface.start()

    def message_recv(self, ac_id, msg):
	
        if (self.verbose and self.callback != None):
            self.callback(ac_id, msg)

    def shutdown(self):
        print("Shutting down ivy interface...")
        self._interface.shutdown()

    def __del__(self):
        self.shutdown()

    def add_mission_command_dict(self, ac_id, insert, msg_id, msgs):

        msg = PprzMessage("datalink", msg_id)

        msg['ac_id'] = ac_id
        msg['insert'] = insert
        msg['duration'] = msgs.get('duration')
        msg['index'] = msgs.get('index')

        if msg_id == 'MISSION_GOTO_WP_LLA':
            msg['wp_lat'] = msgs.get('wp_lat')
            msg['wp_lon'] = msgs.get('wp_lon')
            msg['wp_alt'] = msgs.get('wp_alt')

        elif msg_id == 'MISSION_CIRCLE_LLA':
            msg['center_lat'] = msgs.get('center_lat')
            msg['center_lon'] = msgs.get('center_lon')
            msg['center_alt'] = msgs.get('center_alt')
            msg['radius'] = msgs.get('radius')

        elif msg_id == 'MISSION_SEGMENT_LLA':
            msg['segment_lat_1'] = msgs.get('segment_lat_1')
            msg['segment_lon_1'] = msgs.get('segment_lon_1')
            msg['segment_lat_2'] = msgs.get('segment_lat_2')
            msg['segment_lon_2'] = msgs.get('segment_lon_2')

        elif msg_id == 'MISSION_PATH_LLA':
            msg['point_lat_1'] = msgs.get('point_lat_1')
            msg['point_lon_1'] = msgs.get('point_lon_1')
            msg['point_lat_2'] = msgs.get('point_lat_2')
            msg['point_lon_2'] = msgs.get('point_lon_2')
            msg['point_lat_3'] = msgs.get('point_lat_3')
            msg['point_lon_3'] = msgs.get('point_lon_3')
            msg['point_lat_4'] = msgs.get('point_lat_4')
            msg['point_lon_4'] = msgs.get('point_lon_4')
            msg['point_lat_5'] = msgs.get('point_lat_5')
            msg['point_lon_5'] = msgs.get('point_lon_5')
            msg['path_alt'] = msgs.get('path_alt')
            msg['nb'] = msgs.get('nb')

        elif msg_id == 'MISSION_SURVEY_LLA':
            msg['survey_lat_1'] = msgs.get('survey_lat_1')
            msg['survey_lon_1'] = msgs.get('survey_lon_1')
            msg['survey_lat_2'] = msgs.get('survey_lat_2')
            msg['survey_lon_2'] = msgs.get('survey_lon_2')
            msg['survey_alt'] = msgs.get('survey_alt')


#        print("Sending message: %s" % msg)
        self._interface.send(msg)

    def add_shape(self, status, obstacle_id, obmsg):
        msg = PprzMessage("ground", "SHAPE")
        msg['id'] = int(obstacle_id)
        msg['fillcolor'] = "red" if obstacle_id > 19 else "orange"
        msg['linecolor'] = "red" if obstacle_id > 19 else "orange"
        msg['status'] = 0 if status=="create" else 1
        msg['shape'] = 0
        msg['latarr'] = [int(obmsg.get("latitude")*10000000.),int(obmsg.get("latitude")*10000000.)]
        msg['lonarr'] = [int(obmsg.get("longitude")*10000000.),int(obmsg.get("longitude")*10000000.)]
        msg['radius'] = int(obmsg.get("sphere_radius") if "sphere_radius" in obmsg else obmsg.get("cylinder_radius"))
        msg['text'] = "NULL"
        msg['opacity'] = 2
        self._interface.send(msg)

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class MagCalibrator(object):
    def __init__(self, plot_results=True, verbose=False):
        self._interface = IvyMessagesInterface("calib_mag")
        self.plotter = MagPlot()
        self.data = []
        self.flt_meas = []
        self.p0 = np.array([0, 0, 0, 0, 0, 0])
        self.optimization_done = False
        self.plot_results = plot_results

    def start_collect(self):
        self._interface.subscribe(self.message_recv, "(.*IMU_MAG_RAW.*)")

    def stop_collect(self):
        self._interface.unsubscribe_all()

    def message_recv(self, ac_id, msg):
        self.data.append(np.array([int(v) for v in msg.fieldvalues]))
        if self.plot_results:
            self.plotter.add_data((map(int, msg.fieldvalues)))

    def shutdown(self):
        if self._interface is not None:
            print("Shutting down ivy interface...")
            self._interface.shutdown()
            self._interface = None

    def __del__(self):
        self.shutdown()

    def calc_min_max_guess(self):
        if len(self.data) > 3:
            # filter out noisy measurements?
            self.flt_meas = np.array(self.data)
            self.p0 = calibration_utils.get_min_max_guess(self.flt_meas, 1.0)

    def print_min_max_guess(self):
        self.calc_min_max_guess()
        if self.data:
            print("Current guess from %d measurements: neutral [%d, %d, %d], scale [%.3f, %.3f, %.3f]" % (len(self.flt_meas),
                  int(round(self.p0[0])), int(round(self.p0[1])), int(round(self.p0[2])),
                  self.p0[3]*2**11, self.p0[4]*2**11, self.p0[5]*2**11))

    def calibrate(self):
        self.calc_min_max_guess()

        if len(self.flt_meas) < 10:
            logging.warning("Not enough measurements")
            return

        cp0, np0 = calibration_utils.scale_measurements(self.flt_meas, self.p0)
        logging.info("initial guess : avg "+str(np0.mean())+" std "+str(np0.std()))
        calibration_utils.print_xml(self.p0, "MAG", 11)

        def err_func(p, meas, y):
            cp, np = calibration_utils.scale_measurements(meas, p)
            err = y * scipy.ones(len(meas)) - np
            return err

        p1, cov, info, msg, success = optimize.leastsq(err_func, self.p0[:], args=(self.flt_meas, 1.0), full_output=1)
        self.optimization_done = success in [1, 2, 3, 4]
        if not self.optimization_done:
            logging.warning("Optimization error: ", msg)

        cp1, np1 = calibration_utils.scale_measurements(self.flt_meas, p1)

        if self.optimization_done:
            logging.info("optimized guess : avg " + str(np1.mean()) + " std " + str(np1.std()))
            calibration_utils.print_xml(p1, "MAG", 11)
        else:
            logging.info("last iteration of failed optimized guess : avg " + str(np1.mean()) + " std " + str(np1.std()))

        if self.plot_results:
            calibration_utils.plot_results("MAG", np.array(self.data), range(len(self.data)),
                                           self.flt_meas, cp0, np0, cp1, np1, 1.0, blocking=False)
            calibration_utils.plot_mag_3d(self.flt_meas, cp1, p1)

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class DistanceCounterFrame(wx.Frame):

    def message_recv(self, ac_id, msg):
        if msg.name == "INS":

            self.msg_count = self.msg_count + 1

            newx = float(msg.get_field(0)) / 256.0
            newy = float(msg.get_field(1)) / 256.0

            moved = ((newx - self.ins_msg_x) ** 2 + (newy - self.ins_msg_y) ** 2)
            if self.init == 0:
                self.init = 1
            else:
                self.distance = self.distance + math.sqrt(moved)

            self.ins_msg_x = newx
            self.ins_msg_y = newy
            self.ins_msg_z = msg.get_field(2)


            # graphical update
            wx.CallAfter(self.update)

    def update(self):
        self.Refresh()

    def OnSize(self, event):
        self.w = event.GetSize()[0]
        self.h = event.GetSize()[1]
        self.Refresh()

    def OnPaint(self, e):
        # Paint Area
        dc = wx.PaintDC(self)
        brush = wx.Brush("white")
        dc.SetBackground(brush)
        dc.Clear()

        # Background
        dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT))
        font = wx.Font(11, wx.DEFAULT, wx.NORMAL, wx.NORMAL)
        dc.SetFont(font)
        dc.DrawText("INS Packets:" + str(self.msg_count),2,2)
        dc.DrawText("Data: " + str(self.ins_msg_x) + ", " + str(self.ins_msg_y) + ", " + str(self.ins_msg_z) + ".",2,22)
        dc.DrawText("Distance: " + str(round(float(self.distance)/1.0,2)) + " m",2,22+20)



    def __init__(self, _settings):

        # Command line arguments
        self.settings = _settings

        # Statistics
        self.data = { 'packets': 0, 'bytes': 0}

        self.w = WIDTH
        self.h = WIDTH

        # Frame
        wx.Frame.__init__(self, id=-1, parent=None, name=u'Distance Counter',
                          size=wx.Size(self.w, self.h), title=u'Distance Counter')

        self.Bind(wx.EVT_PAINT, self.OnPaint)
        self.Bind(wx.EVT_SIZE, self.OnSize)
        self.Bind(wx.EVT_CLOSE, self.OnClose)

        # IVY
        self.interface = IvyMessagesInterface("DistanceCounter")
        self.interface.subscribe(self.message_recv)

        self.msg_count = 0
        self.distance = 0
        self.ins_msg_x = 0
        self.ins_msg_y = 0
        self.ins_msg_z = 0
        self.init = 0

    def OnClose(self, event):
        self.interface.shutdown()
        self.Destroy()

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class initTable:
    def __init__(self, config, verbose=False):
        self.verbose = verbose
        self.config = config
        self.ids = np.array(self.config['ids'])
        self.B = np.array(self.config['topology'])
        self.Zdesired = np.array(self.config['desired_intervehicle_angles'])
        self.list_ids = np.ndarray.tolist(self.ids)

        # Start IVY interface
        self._interface = IvyMessagesInterface("Init DCF tables")

    def __del__(self):
        self.stop()

    def stop(self):
        # Stop IVY interface
        if self._interface is not None:
            self._interface.shutdown()

    def init_dcftables(self):
        time.sleep(2)

        for count, column in enumerate(self.B.T):
            index = np.nonzero(column)
            i = index[0]

            # nei_id = 0, special msg to clean the table onboard
            msg_clean_a = PprzMessage("datalink", "DCF_REG_TABLE")
            msg_clean_a['ac_id'] = int(self.list_ids[i[0]])
            msg_clean_a['nei_id'] = 0
            msg_clean_b = PprzMessage("datalink", "DCF_REG_TABLE")
            msg_clean_b['ac_id'] = int(self.list_ids[i[1]])
            msg_clean_b['nei_id'] = 0

            self._interface.send(msg_clean_a)
            self._interface.send(msg_clean_b)

            if self.verbose:
                print(msg_clean_a)
                print(msg_clean_b)

        for count, column in enumerate(self.B.T):
            index = np.nonzero(column)
            i = index[0]

            msga = PprzMessage("datalink", "DCF_REG_TABLE")
            msga['ac_id'] = int(self.list_ids[i[0]])
            msga['nei_id'] = int(self.list_ids[i[1]])
            if len(self.list_ids) == 2:
                msga['desired_sigma'] = (column[index])[0]*int(self.Zdesired)
            else:
                msga['desired_sigma'] = (column[index])[0]*int(self.Zdesired[count])
            self._interface.send(msga)

            msgb = PprzMessage("datalink", "DCF_REG_TABLE")
            msgb['ac_id'] = int(self.list_ids[i[1]])
            msgb['nei_id'] = int(self.list_ids[i[0]])
            if len(self.list_ids) == 2:
                msgb['desired_sigma'] = (column[index])[1]*int(self.Zdesired)
            else:
                msgb['desired_sigma'] = (column[index])[1]*int(self.Zdesired[count])
            self._interface.send(msgb)

            if self.verbose:
                print(msga)
                print(msgb)

        time.sleep(2)

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]

#.........这里部分代码省略.........
                rc.V[1] = float(msg['speed'][0])
                rc.V[2] = -float(msg['speed'][2])
                rc.timeout = 0
                rc.initialized = True
        if self.use_ground_ref:
            self._interface.subscribe(ground_ref_cb, PprzMessage("ground", "GROUND_REF"))

        # bind to JOYSTICK message
        def joystick_cb(ac_id, msg):
            self.joystick.trans = float(msg['axis1']) * self.sens['t1'] / 127.
            self.joystick.trans2 = float(msg['axis2']) * self.sens['t2'] / 127.
            self.joystick.rot = float(msg['axis3']) * self.sens['r1'] / 127.
            self.altitude = self.sens['alt_min'] + float(msg['axis4']) * (self.sens['alt_max'] - self.sens['alt_min']) / 127.
            if msg['button1'] == '1' and not self.joystick.button1:
                self.scale = min(self.scale + self.sens['scale'], self.sens['scale_max'])
            if msg['button2'] == '1' and not self.joystick.button2:
                self.scale = max(self.scale - self.sens['scale'], self.sens['scale_min'])
            if msg['button4'] == '1' and not self.joystick.button4:
                self.enabled = False
            if msg['button3'] == '1' and not self.joystick.button3:
                self.enabled = True
            self.joystick.button1 = (msg['button1'] == '1')
            self.joystick.button2 = (msg['button2'] == '1')
            self.joystick.button3 = (msg['button3'] == '1')
            self.joystick.button4 = (msg['button4'] == '1')
        self._interface.subscribe(joystick_cb, PprzMessage("ground", "JOYSTICK"))

    def __del__(self):
        self.stop()

    def stop(self):
        # Stop IVY interface
        if self._interface is not None:
            self._interface.shutdown()

    def formation(self):
        '''
        formation control algorithm
        '''
        ready = True
        for rc in self.rotorcrafts:
            if not rc.initialized:
                if self.verbose:
                    print("Waiting for state of rotorcraft ", rc.id)
                    sys.stdout.flush()
                ready = False
            if rc.timeout > 0.5:
                if self.verbose:
                    print("The state msg of rotorcraft ", rc.id, " stopped")
                    sys.stdout.flush()
                ready = False
            if rc.initialized and 'geo_fence' in self.config.keys():
                geo_fence = self.config['geo_fence']
                if not self.ignore_geo_fence:
                    if (rc.X[0] < geo_fence['x_min'] or rc.X[0] > geo_fence['x_max']
                            or rc.X[1] < geo_fence['y_min'] or rc.X[1] > geo_fence['y_max']
                            or rc.X[2] < geo_fence['z_min'] or rc.X[2] > geo_fence['z_max']):
                        if self.verbose:
                            print("The rotorcraft", rc.id, " is out of the fence (", rc.X, ", ", geo_fence, ")")
                            sys.stdout.flush()
                        ready = False

        if not ready:
            return

        dim = 2 * len(self.rotorcrafts)

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class SVInfoFrame(wx.Frame):
    def message_recv(self, ac_id, msg):
        if msg.name == "SVINFO":
            chn = int(msg['chn'])
            self.sv[chn] = SvChannel(chn, msg)
            wx.CallAfter(self.update)

    def update(self):
        self.Refresh()

    def OnSize(self, event):
        self.w = event.GetSize()[0]
        self.h = event.GetSize()[1]
        self.Refresh()

    def OnPaint(self, e):
        tdx = -5
        tdy = -7
        th = 15

        w = self.w
        h = self.w
        if h < self.w + 50:
            h = self.w + 50
        bar = self.h - w - th - th

        dc = wx.PaintDC(self)
        brush = wx.Brush("white")
        dc.SetBackground(brush)
        dc.Clear()

        # Background
        dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
        dc.DrawCircle(w / 2, w / 2, w / 2 - 1)
        dc.DrawCircle(w / 2, w / 2, w / 4 - 1)
        dc.DrawCircle(w / 2, w / 2, 1)
        font = wx.Font(11, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD)
        dc.SetFont(font)

        dc.DrawText("N", w / 2 + tdx, 2)
        dc.DrawText("S", w / 2 + tdx, w - 17)
        dc.DrawText("E", w - 15, w / 2 + tdy)
        dc.DrawText("W", 2, w / 2 + tdy)

        # SV
        for chn in self.sv:
            sv = self.sv[chn]

            c = QIColour(sv.QI)
            used = sv.Flags & 1
            s = 7 + used * 5

            el = float(sv.Elev) / 90.0 * float(w) / 2.0
            az = float(sv.Azim) * math.pi / 180.0

            y = float(w) / 2.0 - math.cos(az) * el
            x = float(w) / 2.0 + math.sin(az) * el

            dc.SetBrush(wx.Brush(c, wx.SOLID))
            dc.DrawCircle(int(x), int(y), s)

            font = wx.Font(8, wx.ROMAN, wx.NORMAL, wx.NORMAL)
            dc.SetFont(font)
            dc.DrawText(str(sv.SVID), x + tdx, y + tdy)

            bh = float(bar - th - th) * float(sv.CNO) / 55.0
            dc.DrawRectangle(w / CHANNEL * chn + 5 * (1 - used), self.h - th - bh, w / CHANNEL - 2 - 10 * (1 - used), bh)
            dc.DrawText(str(chn), w / CHANNEL * chn, self.h - th)
            dc.DrawText(str(sv.CNO), w / CHANNEL * chn, self.h - bar)

    def __init__(self):

        self.w = WIDTH
        self.h = WIDTH + BARH

        wx.Frame.__init__(self, id=-1, parent=None, name=u'SVInfoFrame',
                          size=wx.Size(self.w, self.h), title=u'SV Info')
        self.Bind(wx.EVT_PAINT, self.OnPaint)
        self.Bind(wx.EVT_SIZE, self.OnSize)
        self.Bind(wx.EVT_CLOSE, self.OnClose)

        ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/svinfo/svinfo.ico", wx.BITMAP_TYPE_ICO)
        self.SetIcon(ico)

        self.sv = {}

        self.interface = IvyMessagesInterface("svinfoframe")
        self.interface.subscribe(self.message_recv)

    def OnClose(self, event):
        self.interface.shutdown()
        self.Destroy()

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class MessagePicker(wx.Frame):
    def __init__(self, parent, callback, ivy_interface=None):
        wx.Frame.__init__(self, parent, name="MessagePicker", title=u'Message Picker', size=wx.Size(320,640))

        self.aircrafts = {}
        self.callback = callback

        self.tree = wx.TreeCtrl(self)
        self.root = self.tree.AddRoot("Telemetry")
        self.tree.Bind(wx.EVT_LEFT_DCLICK, self.OnDoubleClick)
        self.tree.Bind(wx.EVT_CHAR, self.OnKeyChar)
        self.Bind(wx.EVT_CLOSE, self.OnClose)
        if ivy_interface is None:
            self.message_interface = IvyMessagesInterface("MessagePicker")
        else:
            self.message_interface = ivy_interface
        self.message_interface.subscribe(self.msg_recv)

    def OnClose(self, event):
        # if we have a parent (like the plotpanel) only hide instead of shutdown
        if self.GetParent() is not None:
            self.Hide()
        else:
            self.message_interface.shutdown()
            self.Destroy()

    def msg_recv(self, ac_id, msg):
        if msg.msg_class != "telemetry":
            return

        self.tree.Expand(self.root)
        if ac_id not in self.aircrafts:
            ac_node = self.tree.AppendItem(self.root, str(ac_id))
            self.aircrafts[ac_id] = Aircraft(ac_id)
            self.aircrafts[ac_id].messages_book = ac_node

        aircraft = self.aircrafts[ac_id]
        ac_node = aircraft.messages_book

        if msg.name not in aircraft.messages:
            msg_node = self.tree.AppendItem(ac_node, str(msg.name))
            self.tree.SortChildren(ac_node)
            aircraft.messages[msg.name] = Message("telemetry", msg.name)
            for field in aircraft.messages[msg.name].fieldnames:
                item = self.tree.AppendItem(msg_node, field)

    def OnKeyChar(self, event):
        if event.GetKeyCode() != 13:
            return False
        node = self.tree.GetSelection()
        field_name = self.tree.GetItemText(node)

        parent = self.tree.GetItemParent(node)
        message_name = self.tree.GetItemText(parent)

        grandparent = self.tree.GetItemParent(parent)
        ac_id = self.tree.GetItemText(grandparent)

        if node == self.root or parent == self.root or grandparent == self.root:
            # if not leaf, double click = expand
            if self.tree.IsExpanded(node):
                self.tree.Collapse(node)
            else:
                self.tree.Expand(node)
            return

        self.callback(int(ac_id), message_name, field_name)

    def OnDoubleClick(self, event):
        node = self.tree.GetSelection()
        field_name = self.tree.GetItemText(node)

        parent = self.tree.GetItemParent(node)
        message_name = self.tree.GetItemText(parent)

        grandparent = self.tree.GetItemParent(parent)
        ac_id = self.tree.GetItemText(grandparent)

        if node == self.root or parent == self.root or grandparent == self.root:
            # if not leaf, double click = expand
            if self.tree.IsExpanded(node):
                self.tree.Collapse(node)
            else:
                self.tree.Expand(node)
            return

        self.callback(int(ac_id), message_name, field_name)

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class Guidance(object):
    def __init__(self, ac_id, verbose=False):
        self.ac_id = ac_id
        self.verbose = verbose
        self._interface = None
        self.ap_mode = None
        try:
            settings = PaparazziACSettings(self.ac_id)
        except Exception as e:
            print(e)
            return
        try:
            self.ap_mode = settings.name_lookup['mode'].index
        except Exception as e:
            print(e)
            print("ap_mode setting not found, mode change not possible.")
        self._interface = IvyMessagesInterface("gb2ivy")

    def shutdown(self):
        if self._interface is not None:
            print("Shutting down ivy interface...")
            self._interface.shutdown()
            self._interface = None

    def __del__(self):
        self.shutdown()

    def bind_flight_param(self, send_cb):
        def bat_cb(ac_id, msg):
            bat = float(msg['bat'])
            # should not be more that 18 characters
            send_cb('bat: '+str(bat)+' V')
        self._interface.subscribe(bat_cb, regex=('(^ground ENGINE_STATUS '+str(self.ac_id)+' .*)'))

    def set_guided_mode(self):
        """
        change mode to GUIDED.
        """
        if self.ap_mode is not None:
            msg = PprzMessage("ground", "DL_SETTING")
            msg['ac_id'] = self.ac_id
            msg['index'] = self.ap_mode
            msg['value'] = 19  # AP_MODE_GUIDED
            print("Setting mode to GUIDED: %s" % msg)
            self._interface.send(msg)

    def set_nav_mode(self):
        """
        change mode to NAV.
        """
        if self.ap_mode is not None:
            msg = PprzMessage("ground", "DL_SETTING")
            msg['ac_id'] = self.ac_id
            msg['index'] = self.ap_mode
            msg['value'] = 13  # AP_MODE_NAV
            print("Setting mode to NAV: %s" % msg)
            self._interface.send(msg)

    def move_at_body_vel(self, forward=0.0, right=0.0, down=0.0, yaw=0.0):
        """
        move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
        """
        msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
        msg['ac_id'] = self.ac_id
        msg['flags'] = 0xE2
        msg['x'] = forward
        msg['y'] = right
        msg['z'] = down
        msg['yaw'] = yaw
        print("move at vel body: %s" % msg)
        self._interface.send_raw_datalink(msg)

    def command_callback(self, command):
        """
        convert incoming command into velocity
        """
        right = 0.0
        forward = 0.0
        down = 0.0
        yaw = 0.0
        command = int(command)
        if command & J_RIGHT:
            right += 2.0
        if command & J_LEFT:
            right -= 2.0
        if command & J_UP:
            forward += 2.0
        if command & J_DOWN:
            forward -= 2.0
        if command & J_A:
            down -= 1.0
        if command & J_B:
            down += 1.0
        if command & J_START:
            yaw += radians(20)
        if command & J_SELECT:
            yaw -= radians(20)
        self.move_at_body_vel(forward, right, down, yaw)

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class AtcFrame(wx.Frame):

    def message_recv(self, ac_id, msg):
        if msg.name == "INS_REF":
            self.qfe = round(float(msg['baro_qfe'])  / 100.0,1)
            wx.CallAfter(self.update)
        elif msg.name =="ROTORCRAFT_FP_MIN":
            self.gspeed = round(float(msg['gspeed']) / 100.0 * 3.6 / 1.852,1)
            self.alt = round(float(msg['up']) * 0.0039063 * 3.28084 ,1)
            wx.CallAfter(self.update)
        elif msg.name =="ROTORCRAFT_FP":
            self.alt = round(float(msg['up']) * 0.0039063 * 3.28084 ,1)
            wx.CallAfter(self.update)
        elif msg.name =="AIR_DATA":
            self.airspeed = round(float(msg['airspeed']) * 3.6 / 1.852,1)
            self.qnh = round(float(msg['qnh']),1)
            self.amsl = round(float(msg['amsl_baro']) * 3.28084,1)
            wx.CallAfter(self.update)

    def update(self):
        self.Refresh()

    def OnPaint(self, e):
        tdx = 10
        tdy = 80

        w = self.w
        h = self.w


        dc = wx.PaintDC(self)
        #brush = wx.Brush("white")
        #dc.SetBackground(brush)
        #dc.Clear()

        # Background
        dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT))
        #dc.DrawCircle(w/2,w/2,w/2-1)
        font = wx.Font(40, wx.ROMAN, wx.BOLD, wx.NORMAL)
        dc.SetFont(font)
        dc.DrawText("Airspeed: " + str(self.airspeed) + " kt",tdx,tdx)
        dc.DrawText("Ground Speed: " + str(self.gspeed) + " kt",tdx,tdx+tdy*1)

        dc.DrawText("AMSL: " + str(self.amsl) + " ft",tdx,tdx+tdy*2)
        dc.DrawText("QNH: " + str(self.qnh) + " ",tdx,tdx+tdy*3)

        dc.DrawText("ALT: " + str(self.alt) + " ",tdx,tdx+tdy*4)
        dc.DrawText("QFE: " + str(self.qfe) + " ",tdx,tdx+tdy*5)

        #dc.DrawText("HMSL: " + str(self.hmsl) + " ft",tdx,tdx+tdy*6)

        #c = wx.Colour(0,0,0)
        #dc.SetBrush(wx.Brush(c, wx.SOLID))
        #dc.DrawCircle(int(w/2),int(w/2),10)



    def __init__(self):

        self.w = 900
        self.h = 700

        self.airspeed = 0;

        self.amsl = 0;
        self.qnh = 0;

        self.qfe = 0;
        self.alt = 0;

        #self.hmsl = 0;
        self.gspeed = 0;

        wx.Frame.__init__(self, id=-1, parent=None, name=u'ATC Center',
                          size=wx.Size(self.w, self.h), title=u'ATC Center')


        self.Bind(wx.EVT_PAINT, self.OnPaint)
        self.Bind(wx.EVT_CLOSE, self.OnClose)

        ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/atc/atc.ico", wx.BITMAP_TYPE_ICO)
        self.SetIcon(ico)

        self.interface = IvyMessagesInterface("ATC-Center")
        self.interface.subscribe(self.message_recv)

    def OnClose(self, event):
        self.interface.shutdown()
        self.Destroy()

# 需要导入模块: from pprzlink.ivy import IvyMessagesInterface [as 别名]
# 或者: from pprzlink.ivy.IvyMessagesInterface import shutdown [as 别名]
class PayloadForwarderFrame(wx.Frame):

    def message_recv(self, ac_id, msg):
        if msg.name == "PAYLOAD":
            # convert text to binary
            pld = msg.get_field(0).split(",")
            b = []
            for p in pld:
                b.append(int(p))

            # forward over UDP
            self.data['packets'] = self.data['packets'] + 1
            self.data['bytes'] = self.data['bytes'] + len(b)
            self.sock.sendto(bytearray(b), (self.settings.ip, self.settings.port))

            # send to decoder
            self.decoder.add_payload(b)

            # graphical update
            wx.CallAfter(self.update)

    def update(self):
        self.Refresh()

    def OnSize(self, event):
        self.w = event.GetSize()[0]
        self.h = event.GetSize()[1]
        self.Refresh()

    def OnPaint(self, e):
        # Paint Area
        dc = wx.PaintDC(self)
        brush = wx.Brush("white")
        dc.SetBackground(brush)
        dc.Clear()

        # Background
        dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT))
        font = wx.Font(11, wx.ROMAN, wx.BOLD, wx.NORMAL)
        dc.SetFont(font)
        dc.DrawText("UDP: " + self.settings.ip + ":" + str(self.settings.port),2,2)
        dc.DrawText("Data: " + str(self.data['packets']) + " packets, " + str(round(float(self.data['bytes'])/1024.0,2)) + "kb)",2,22)
        dc.DrawText("Decoder: " + self.settings.decoder ,2,42)

        # Payload visual representation
        self.decoder.draw(dc, 2, 62)


    def __init__(self, _settings):

        # Command line arguments
        self.settings = _settings

        # Statistics
        self.data = { 'packets': 0, 'bytes': 0}

        # Decoder
        if (self.settings.decoder is 'jpeg100'):
            self.decoder = jpeg100_decoder.ThumbNailFromPayload()
        else:
            self.decoder = MinimalDecoder()

        self.w = WIDTH
        self.h = WIDTH

        # Socket
        self.sock = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP

        # Frame
        wx.Frame.__init__(self, id=-1, parent=None, name=u'Payload Forwarding',
                          size=wx.Size(self.w, self.h), title=u'Payload Forwarding')
        ico = wx.Icon(os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)))) + "/payload.ico", wx.BITMAP_TYPE_ICO)
        self.SetIcon(ico)

        self.Bind(wx.EVT_PAINT, self.OnPaint)
        self.Bind(wx.EVT_SIZE, self.OnSize)
        self.Bind(wx.EVT_CLOSE, self.OnClose)

        # IVY
        self.interface = IvyMessagesInterface("PayloadForwarder")
        self.interface.subscribe(self.message_recv)

    def OnClose(self, event):
        self.interface.shutdown()
        self.Destroy()