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()
def main(): # parse arguments import argparse as ap argp = ap.ArgumentParser(description="Environment variables provider " "for Paparazzi simulation from MesoNH data") argp.add_argument("-t", "--time-step", required=True, type=int, help="Duration of a time step between MesoNH Files.") argp.add_argument("-f", "--files", required=True, nargs='+', help="MesoNH netCDF files, in temporal ordering") argp.add_argument("-x", "--origin-x", required=False, type=float, default=0., help="Origin translation x.") argp.add_argument("-y", "--origin-y", required=False, type=float, default=0., help="Origin translation y.") argp.add_argument("-z", "--origin-z", required=False, type=float, default=0., help="Origin translation z.") arguments = argp.parse_args() print(arguments) # register signal handler for ctrl+c to stop the program signal.signal(signal.SIGINT, signal_handler) # origin for translation from paparazzi to mesoNH frame global origin origin = np.array( [0, arguments.origin_z, arguments.origin_x, arguments.origin_y]) # build atmosphere simulation source global atm atm = MesoNHAtmosphere(arguments.files, arguments.time_step, tinit=0) # init ivy and register callback for WORLD_ENV_REQ and NPS_SPEED_POS global ivy ivy = IvyMessagesInterface("MesoNH") ivy.subscribe(worldenv_cb, '(.* WORLD_ENV_REQ .*)') # wait for ivy to stop from ivy.std_api import IvyMainLoop signal.pause()
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
class Cx10ds2ivy: def __init__(self, verbose=False): self.verbose = verbose self.step = 0.1 # period in seconds self.button_trim = False self._cx10 = CX10DS(verbose) # Start IVY interface self._interface = IvyMessagesInterface("Cx10ds2ivy") # bind to JOYSTICK message def joystick_cb(ac_id, msg): aileron = int(msg['axis1']) elevator = int(msg['axis2']) rudder = int(msg['axis3']) if int(msg['button1']) == 255: throttle = int(msg['axis4']) # direct throttle reading else: throttle = 128 direction = int(msg['button1'])-1 throttle_incr = self._cx10.valid_range(int(msg['axis4']), 0, 127) throttle = 128 + direction * throttle_incr # up mode = int(msg['button2']) if msg['button4'] == '1' and not self.button_trim: self._cx10.set_trim() self.button_trim = (msg['button4'] == '1') self._cx10.set_cmd(aileron,elevator,rudder,throttle,mode) if self.verbose: print("Throttle {0}".format(throttle)) print("Rudder {0}".format(rudder)) print("elevator {0}".format(elevator)) print("aileron {0}".format(aileron)) print("Mode {0}".format(mode)) 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() # main loop def run(self): try: while True: # TODO: make better frequency managing self._cx10.send() sleep(self.step) except KeyboardInterrupt: if self.verbose: print("Exiting..") self.stop()
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()
def main(): # parse arguments import argparse as ap argp = ap.ArgumentParser(description="Environment variables provider " "for Paparazzi simulation from MesoNH data") argp.add_argument("-t", "--time-step", required=True, type=int, help="Duration of a time step between MesoNH Files.") argp.add_argument("-f", "--files", required=True, nargs='+', help="MesoNH netCDF files, in temporal ordering") argp.add_argument("-x", "--origin-x", required=False, type=float, default=0., help="Origin translation x.") argp.add_argument("-y", "--origin-y", required=False, type=float, default=0., help="Origin translation y.") argp.add_argument("-z", "--origin-z", required=False, type=float, default=0., help="Origin translation z.") arguments = argp.parse_args() print(arguments) # register signal handler for ctrl+c to stop the program signal.signal(signal.SIGINT, signal_handler) # origin for translation from paparazzi to mesoNH frame global origin origin = np.array([0, arguments.origin_z, arguments.origin_x, arguments.origin_y]) # build atmosphere simulation source global atm atm = MesoNHAtmosphere(arguments.files, arguments.time_step, tinit=0) # init ivy and register callback for WORLD_ENV_REQ and NPS_SPEED_POS global ivy ivy = IvyMessagesInterface("MesoNH"); ivy.subscribe(worldenv_cb,'(.* WORLD_ENV_REQ .*)') # wait for ivy to stop from ivy.std_api import IvyMainLoop signal.pause()
class RosIvyMessagesInterface(RosMessagesInterface): def __init__(self, agent_name=None, start_ivy=True, verbose=False, ivy_bus=IVY_BUS): self.interface = IvyMessagesInterface(agent_name, start_ivy, verbose, ivy_bus) RosMessagesInterface.__init__(self) # only subscribe to telemetry messages eg. starting with AC_ID self.interface.subscribe(callback=self.to_ros, regex_or_msg='(^[0-9]+ .*)') def from_ros(self, ros_msg): pprz_msg = self.converter.ros2pprz(ros_msg) self.interface.send(pprz_msg) def to_ros(self, sender_id, pprz_msg): ros_msg = self.converter.pprz2ros(sender_id, pprz_msg) self.pub.publish(ros_msg)
class uEyeIvy(uEyePprzlink): def __init__(self, verbose=False): from pprzlink.ivy import IvyMessagesInterface # init Ivy interface self.pprzivy = IvyMessagesInterface("pyueye") # init cam related part uEyePprzlink.__init__(self, verbose) # bind to message self.pprzivy.subscribe(self.process_msg, PprzMessage("telemetry", "DC_SHOT")) def __exit__(self): if self.pprzivy is not None: self.stop() def stop(self): self.pprzivy.shutdown() self.pprzivy = None uEyePprzlink.stop(self) def run(self): try: while True: if self.new_msg and self.cam is not None: ret = self.cam.freeze_video(True) if ret == ueye.IS_SUCCESS: self.verbose_print("Freeze done") img = ImageData(self.cam.handle(), self.buff) self.process_image(img, 0) self.verbose_print("Process done") else: self.verbose_print('Freeze fail with {%d}' % ret) self.new_msg = False else: time.sleep(0.1) except (KeyboardInterrupt, SystemExit): pass
class GVFFrame(wx.Frame): def __init__(self, ac_id): wx.Frame.__init__(self, id=-1, parent=None, \ name=u'GVF', size=wx.Size(WIDTH, HEIGHT), \ style=wx.DEFAULT_FRAME_STYLE, title=u'Guidance Vector Field') # Vehicle variables self.ac_id = ac_id self.course = 0 self.yaw = 0 self.XY = np.array([0, 0]) # Desired trajectory self.timer_traj = 0 # We do not update the traj every time we receive a msg self.timer_traj_lim = 7 # (7+1) * 0.25secs self.s = 0 self.ke = 0 self.map_gvf = map2d(np.array([0, 0]), 150000) self.traj = None # Frame self.canvas = FigureCanvas(self, -1, self.map_gvf.fig) self.Bind(wx.EVT_CLOSE, self.OnClose) self.redraw_timer = wx.Timer(self) self.Bind(wx.EVT_TIMER, self.OnRedrawTimer, self.redraw_timer) self.redraw_timer.Start(100) # Ivy self.interface = IvyMessagesInterface("GVF") self.interface.subscribe(self.message_recv) settings = PaparazziACSettings(ac_id) def message_recv(self, ac_id, msg): if int(ac_id) == self.ac_id: if msg.name == 'GPS': self.course = int(msg.get_field(3))*np.pi/1800 if msg.name == 'NAVIGATION': self.XY[0] = float(msg.get_field(2)) self.XY[1] = float(msg.get_field(3)) if msg.name == 'ATTITUDE': self.yaw = float(msg.get_field(1)) if msg.name == 'GVF': self.gvf_error = float(msg.get_field(0)) # Straight line if int(msg.get_field(1)) == 0 \ and self.timer_traj == self.timer_traj_lim: self.s = int(msg.get_field(2)) self.ke = float(msg.get_field(3)) param = [float(x) for x in msg.get_field(4).split(',')] a = param[0] b = param[1] c = param[2] self.traj = traj_line(np.array([-100,100]), a, b, c) self.traj.vector_field(self.traj.XYoff, self.map_gvf.area, \ self.s, self.ke) # Ellipse if int(msg.get_field(1)) == 1 \ and self.timer_traj == self.timer_traj_lim: self.s = int(msg.get_field(2)) self.ke = float(msg.get_field(3)) param = [float(x) for x in msg.get_field(4).split(',')] ex = param[0] ey = param[1] ea = param[2] eb = param[3] ealpha = param[4] self.traj = traj_ellipse(np.array([ex, ey]), ealpha, ea, eb) self.traj.vector_field(self.traj.XYoff, \ self.map_gvf.area, self.s, self.ke) # Sin if int(msg.get_field(1)) == 2 \ and self.timer_traj == self.timer_traj_lim: self.s = int(msg.get_field(2)) self.ke = float(msg.get_field(3)) param = [float(x) for x in msg.get_field(4).split(',')] a = param[0] b = param[1] alpha = param[2] w = param[3] off = param[4] A = param[5] self.traj = traj_sin(np.array([-100, 100]), a, b, alpha, \ w, off, A) self.traj.vector_field(self.traj.XYoff, \ self.map_gvf.area, self.s, self.ke) self.timer_traj = self.timer_traj + 1 if self.timer_traj > self.timer_traj_lim: self.timer_traj = 0 def draw_gvf(self, XY, yaw, course): if self.traj is not None: self.map_gvf.draw(XY, yaw, course, self.traj) def OnClose(self, event): self.interface.shutdown() self.Destroy() def OnRedrawTimer(self, event): self.draw_gvf(self.XY, self.yaw, self.course) self.canvas.draw()
class MessagesFrame(wx.Frame): def message_recv(self, ac_id, msg): """Handle incoming messages Callback function for IvyMessagesInterface :param ac_id: aircraft id :type ac_id: int :param msg: message :type msg: PprzMessage """ # only show messages of the requested class if msg.msg_class != self.msg_class: return if ac_id in self.aircrafts and msg.name in self.aircrafts[ac_id].messages: if time.time() - self.aircrafts[ac_id].messages[msg.name].last_seen < 0.2: return wx.CallAfter(self.gui_update, ac_id, msg) def find_page(self, book, name): if book.GetPageCount() < 1: return 0 start = 0 end = book.GetPageCount() while start < end: if book.GetPageText(start) >= name: return start start += 1 return start def update_leds(self): wx.CallAfter(self.update_leds_real) def update_leds_real(self): for ac_id in self.aircrafts: aircraft = self.aircrafts[ac_id] for msg_str in aircraft.messages: message = aircraft.messages[msg_str] if message.last_seen + 0.2 < time.time(): aircraft.messages_book.SetPageImage(message.index, 0) self.timer = threading.Timer(0.1, self.update_leds) self.timer.start() def setup_image_list(self, notebook): imageList = wx.ImageList(24, 24) image = wx.Image(PPRZ_HOME + "/data/pictures/gray_led24.png") bitmap = wx.BitmapFromImage(image) imageList.Add(bitmap) image = wx.Image(PPRZ_HOME + "/data/pictures/green_led24.png") bitmap = wx.BitmapFromImage(image) imageList.Add(bitmap) notebook.AssignImageList(imageList) def add_new_aircraft(self, ac_id): self.aircrafts[ac_id] = Aircraft(ac_id) ac_panel = wx.Panel(self.notebook, -1) self.notebook.AddPage(ac_panel, str(ac_id)) messages_book = wx.Notebook(ac_panel, style=wx.NB_LEFT) self.setup_image_list(messages_book) sizer = wx.BoxSizer(wx.VERTICAL) sizer.Add(messages_book, 1, wx.EXPAND) ac_panel.SetSizer(sizer) sizer.Layout() self.aircrafts[ac_id].messages_book = messages_book def add_new_message(self, aircraft, msg_class, name): messages_book = aircraft.messages_book aircraft.messages[name] = Message(msg_class, name) field_panel = wx.Panel(messages_book) grid_sizer = wx.FlexGridSizer(len(aircraft.messages[name].fieldnames), 2) index = self.find_page(messages_book, name) messages_book.InsertPage(index, field_panel, name, imageId=1) aircraft.messages[name].index = index # update indexes of pages which are to be moved for message_name in aircraft.messages: aircraft.messages[message_name].index = self.find_page(messages_book, message_name) for field_name in aircraft.messages[name].fieldnames: name_text = wx.StaticText(field_panel, -1, field_name) size = name_text.GetSize() size.x = LABEL_WIDTH name_text.SetMinSize(size) grid_sizer.Add(name_text, 1, wx.ALL, BORDER) value_control = wx.StaticText(field_panel, -1, "42", style=wx.EXPAND) size = value_control.GetSize() size.x = LABEL_WIDTH value_control.SetMinSize(size) grid_sizer.Add(value_control, 1, wx.ALL | wx.EXPAND, BORDER) if wx.MAJOR_VERSION > 2: if grid_sizer.IsColGrowable(1): grid_sizer.AddGrowableCol(1) else: grid_sizer.AddGrowableCol(1) aircraft.messages[name].field_controls.append(value_control) field_panel.SetAutoLayout(True) field_panel.SetSizer(grid_sizer) field_panel.Layout() def gui_update(self, ac_id, msg): if ac_id not in self.aircrafts: self.add_new_aircraft(ac_id) aircraft = self.aircrafts[ac_id] if msg.name not in aircraft.messages: self.add_new_message(aircraft, msg.msg_class, msg.name) aircraft.messages_book.SetPageImage(aircraft.messages[msg.name].index, 1) self.aircrafts[ac_id].messages[msg.name].last_seen = time.time() for index in range(0, len(msg.fieldvalues)): aircraft.messages[msg.name].field_controls[index].SetLabel(str(msg.get_field(index))) def __init__(self, msg_class="telemetry"): wx.Frame.__init__(self, id=-1, parent=None, name=u'MessagesFrame', size=wx.Size(WIDTH, HEIGHT), style=wx.DEFAULT_FRAME_STYLE, title=u'Messages') self.Bind(wx.EVT_CLOSE, self.OnClose) self.notebook = wx.Notebook(self) self.aircrafts = {} sizer = wx.BoxSizer(wx.HORIZONTAL) sizer.Add(self.notebook, 1, wx.EXPAND) self.SetSizer(sizer) sizer.Layout() self.timer = threading.Timer(0.1, self.update_leds) self.timer.start() self.msg_class = msg_class self.interface = IvyMessagesInterface("Paparazzi Messages Viewer") self.interface.subscribe(self.message_recv) def OnClose(self, event): self.timer.cancel() self.interface.shutdown() self.Destroy()
class FormationControl: def __init__(self, config, freq=10., use_ground_ref=False, ignore_geo_fence=False, verbose=False): self.config = config self.step = 1. / freq self.sens = self.config['sensitivity'] self.use_ground_ref = use_ground_ref self.enabled = True # run control by default self.ignore_geo_fence = ignore_geo_fence self.verbose = verbose self.ids = self.config['ids'] self.rotorcrafts = [FC_Rotorcraft(i) for i in self.ids] self.joystick = FC_Joystick() self.altitude = 2.0 # starts from 1 m high self.scale = 1.0 self.B = np.array(self.config['topology']) self.d = np.array(self.config['desired_distances']) self.t1 = np.array(self.config['motion']['t1']) self.t2 = np.array(self.config['motion']['t2']) self.r1 = np.array(self.config['motion']['r1']) self.r2 = np.array(self.config['motion']['r2']) self.k = np.array(self.config['gains']) if self.B.size == 2: self.B.shape = (2, 1) # Check formation settings if len(self.ids) != np.size(self.B, 0): print( "The number of rotorcrafts in the topology and ids do not match" ) return if np.size(self.d) != np.size(self.B, 1): print( "The number of links in the topology and desired distances do not match" ) return #if np.size(self.d) != np.size(self.m,1): # print("The number of (columns) motion parameters and relative vectors do not match") # return #if np.size(self.m, 0) != 8: # print("The number of (rows) motion parameters must be eight") # return if self.config['3D'] == True: print("3D formation is not supported yet") return # Start IVY interface self._interface = IvyMessagesInterface("Formation Control Rotorcrafts") # bind to INS message def ins_cb(ac_id, msg): if ac_id in self.ids and msg.name == "INS": rc = self.rotorcrafts[self.ids.index(ac_id)] i2p = 1. / 2**8 # integer to position i2v = 1. / 2**19 # integer to velocity rc.X[0] = float(msg['ins_x']) * i2p rc.X[1] = float(msg['ins_y']) * i2p rc.X[2] = float(msg['ins_z']) * i2p rc.V[0] = float(msg['ins_xd']) * i2v rc.V[1] = float(msg['ins_yd']) * i2v rc.V[2] = float(msg['ins_zd']) * i2v rc.timeout = 0 rc.initialized = True if not self.use_ground_ref: self._interface.subscribe(ins_cb, PprzMessage("telemetry", "INS")) # bind to GROUND_REF message def ground_ref_cb(ground_id, msg): ac_id = int(msg['ac_id']) if ac_id in self.ids: rc = self.rotorcrafts[self.ids.index(ac_id)] # X and V in NED rc.X[0] = float(msg['pos'][1]) rc.X[1] = float(msg['pos'][0]) rc.X[2] = -float(msg['pos'][2]) rc.V[0] = float(msg['speed'][1]) 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 list(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) X = np.zeros(dim) V = np.zeros(dim) U = np.zeros(dim) i = 0 for rc in self.rotorcrafts: X[i] = rc.X[0] X[i + 1] = rc.X[1] V[i] = rc.V[0] V[i + 1] = rc.V[1] i = i + 2 Bb = la.kron(self.B, np.eye(2)) Z = Bb.T.dot(X) Dz = rf.make_Dz(Z, 2) Dzt = rf.make_Dzt(Z, 2, 1) Dztstar = rf.make_Dztstar(self.d * self.scale, 2, 1) Zh = rf.make_Zh(Z, 2) E = rf.make_E(Z, self.d * self.scale, 2, 1) # Shape and motion control jmu_t1 = self.joystick.trans * self.t1[0, :] jtilde_mu_t1 = self.joystick.trans * self.t1[1, :] jmu_r1 = self.joystick.rot * self.r1[0, :] jtilde_mu_r1 = self.joystick.rot * self.r1[1, :] jmu_t2 = self.joystick.trans2 * self.t2[0, :] jtilde_mu_t2 = self.joystick.trans2 * self.t2[1, :] jmu_r2 = self.joystick.rot2 * self.r2[0, :] jtilde_mu_r2 = self.joystick.rot2 * self.r2[1, :] Avt1 = rf.make_Av(self.B, jmu_t1, jtilde_mu_t1) Avt1b = la.kron(Avt1, np.eye(2)) Avr1 = rf.make_Av(self.B, jmu_r1, jtilde_mu_r1) Avr1b = la.kron(Avr1, np.eye(2)) Avt2 = rf.make_Av(self.B, jmu_t2, jtilde_mu_t2) Avt2b = la.kron(Avt2, np.eye(2)) Avr2 = rf.make_Av(self.B, jmu_r2, jtilde_mu_r2) Avr2b = la.kron(Avr2, np.eye(2)) Avb = Avt1b + Avr1b + Avt2b + Avr2b Avr = Avr1 + Avr2 if self.B.size == 2: Zhr = np.array([-Zh[1], Zh[0]]) U = -self.k[1] * V - self.k[0] * Bb.dot(Dz).dot(Dzt).dot( E) + self.k[1] * (Avt1b.dot(Zh) + Avt2b.dot(Zhr)) + np.sign( jmu_r1[0]) * la.kron( Avr1.dot(Dztstar).dot(self.B.T).dot(Avr1), np.eye(2)).dot(Zhr) else: U = -self.k[1] * V - self.k[0] * Bb.dot(Dz).dot(Dzt).dot( E) + self.k[1] * Avb.dot(Zh) + la.kron( Avr.dot(Dztstar).dot(self.B.T).dot(Avr), np.eye(2)).dot(Zh) if self.verbose: #print "Positions: " + str(X).replace('[','').replace(']','') #print "Velocities: " + str(V).replace('[','').replace(']','') #print "Acceleration command: " + str(U).replace('[','').replace(']','') print("Error distances: " + str(E).replace('[', '').replace(']', '')) sys.stdout.flush() i = 0 for ac in self.rotorcrafts: msg = PprzMessage("datalink", "DESIRED_SETPOINT") msg['ac_id'] = ac.id msg['flag'] = 0 # full 3D not supported yet msg['ux'] = U[i] msg['uy'] = U[i + 1] msg['uz'] = self.altitude self._interface.send(msg) i = i + 2 def run(self): try: # The main loop while True: # TODO: make better frequency managing sleep(self.step) for rc in self.rotorcrafts: rc.timeout = rc.timeout + self.step # Run the formation algorithm if self.enabled: self.formation() except KeyboardInterrupt: self.stop()
class EnergyMonFrame(wx.Frame): def message_recv(self, ac_id, msg): if msg.name == "ENERGY": self.bat = EnergyMessage(msg) self.cell.fill_from_energy_msg(self.bat) wx.CallAfter(self.update) elif msg.name == "TEMP_ADC": self.temp = TempMessage(msg) self.cell.fill_from_temp_msg(self.temp) 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 StatusBox(self, dc, nr, txt, percent, color): if percent < 0: percent = 0 if percent > 1: percent = 1 boxw = self.stat tdx = int(boxw * 10.0 / 300.0) tdy = int(boxw * 6.0 / 300.0) boxh = int(boxw * 40.0 / 300.0) boxw = self.stat - 2 * tdx spacing = boxh + 10 dc.SetPen(wx.Pen(wx.Colour(0, 0, 0))) dc.SetBrush(wx.Brush(wx.Colour(220, 220, 220))) dc.DrawRectangle(tdx, int(nr * spacing + tdx), int(boxw), boxh) if color < 0.2: dc.SetBrush(wx.Brush(wx.Colour(250, 0, 0))) elif color < 0.6: dc.SetBrush(wx.Brush(wx.Colour(250, 180, 0))) else: dc.SetBrush(wx.Brush(wx.Colour(0, 250, 0))) # dc.DrawLine(200,50,350,50) dc.DrawRectangle(tdx, int(nr * spacing + tdx), int(boxw * percent), boxh) dc.DrawText(txt, 18, int(nr * spacing + tdy + tdx)) def plot_x(self, x): return int(self.stat + self.tdx + x * (self.w - self.stat - 2 * self.tdx)) def plot_y(self, y): return int(self.tdx + (1 - y) * (self.h - self.tdx - self.tdx)) def plot(self, dc, i1, i2): dc.DrawLine(self.plot_x(i1[1] / 3500), self.plot_y((i1[0] - 2.5) / (4.2 - 2.5)), self.plot_x(i2[1] / 3500), self.plot_y((i2[0] - 2.5) / (4.2 - 2.5))) def DischargePlot(self, dc): self.tdx = int(self.stat * 10.0 / 300.0) dc.SetPen(wx.Pen(wx.Colour(0, 0, 0), 1)) dc.SetBrush(wx.Brush(wx.Colour(250, 250, 250))) dc.DrawRectangle(self.plot_x(0.0), self.plot_y(1.0), self.w - self.stat - 2 * self.tdx, self.h - 2 * self.tdx) for i in range(0, 5): dc.DrawLine(self.plot_x(0.0), self.plot_y(i / 5.0), self.plot_x(1.0), self.plot_y(i / 5.0)) for i in range(0, 7): dc.DrawLine(self.plot_x(i / 7.0), self.plot_y(0), self.plot_x(i / 7.0), self.plot_y(1)) dc.SetPen(wx.Pen(wx.Colour(255, 180, 0), 4)) dc.DrawLine(self.plot_x(self.cell.model / 3500), self.plot_y(0), self.plot_x(self.cell.model / 3500), self.plot_y(1)) dc.DrawLine(self.plot_x(0.0), self.plot_y(self.cell.get_volt_perc()), self.plot_x(1.0), self.plot_y(self.cell.get_volt_perc())) thickness = 3 dc.SetPen(wx.Pen(wx.Colour(0, 0, 0), thickness)) li = bat.batmodel[0, [0, 1]] for i in bat.batmodel[:, [0, 1]]: self.plot(dc, li, i) li = i dc.SetPen(wx.Pen(wx.Colour(0, 0, 255), thickness)) li = bat.batmodel[0, [0, 2]] for i in bat.batmodel[:, [0, 2]]: self.plot(dc, li, i) li = i dc.SetPen(wx.Pen(wx.Colour(0, 255, 0), thickness)) li = bat.batmodel[0, [0, 3]] for i in bat.batmodel[:, [0, 3]]: self.plot(dc, li, i) li = i dc.SetPen(wx.Pen(wx.Colour(255, 255, 0), thickness)) li = bat.batmodel[0, [0, 4]] for i in bat.batmodel[:, [0, 4]]: self.plot(dc, li, i) li = i dc.SetPen(wx.Pen(wx.Colour(255, 0, 0), thickness)) li = bat.batmodel[0, [0, 5]] for i in bat.batmodel[:, [0, 5]]: self.plot(dc, li, i) li = i def OnPaint(self, e): # Automatic Scaling w = self.w h = self.h self.stat = int(w / 4) if self.stat < 100: self.stat = 100 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)) fontscale = int(w * 11.0 / 800.0) if fontscale < 6: fontscale = 6 font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD) dc.SetFont(font) self.StatusBox(dc, 0, self.cell.get_volt(), self.cell.get_volt_perc(), self.cell.get_volt_color()) self.StatusBox(dc, 1, self.cell.get_current(), self.cell.get_current_perc(), self.cell.get_current_color()) self.StatusBox(dc, 2, self.cell.get_energy(), self.cell.get_energy_perc(), self.cell.get_energy_color()) self.StatusBox(dc, 3, self.cell.get_mah_from_volt(), self.cell.get_energy_perc(), self.cell.get_energy_color()) self.StatusBox(dc, 4, self.cell.get_temp(), self.cell.get_temp_perc(), self.cell.get_temp_color()) self.DischargePlot(dc) def __init__(self): self.w = WIDTH self.h = WIDTH + BARH wx.Frame.__init__(self, id=-1, parent=None, name=u'EnergyMonFrame', size=wx.Size(self.w, self.h), title=u'Energy Monitoring') 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/energy_mon/energy_mon.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.bat = {} self.temp = {} self.cell = BatteryCell() self.interface = IvyMessagesInterface("energymonframe") self.interface.subscribe(self.message_recv) def OnClose(self, event): self.interface.shutdown() self.Destroy()
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)
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)
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 elif self.running: 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) if msg.name == "ROTORCRAFT_STATUS": self.msg_count_time = self.msg_count_time + 1 time_new = float(msg['cpu_time']) if time_new > self.time_old and self.time_old != 0 and self.running: self.time_elapsed += time_new - self.time_old self.time_old = time_new # 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) dc.DrawText("Time elapsed: " + str(self.time_elapsed) + "s",2,22+20+20) if self.running: dc.DrawText("Counter running", 150, 22+20) else: dc.DrawText("Counter paused", 150, 22+20) def onStartStop(self, event): self.running = not self.running self.Refresh() def onReset(self, event): self.time_old = 0 self.time_elapsed = 0 self.distance = 0 self.init = 0 self.Refresh() return 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') start_stop_button = wx.Button(self, wx.ID_ANY, 'Start/Pause', (150, 58),size=(90, 25)) start_stop_button.Bind(wx.EVT_BUTTON, self.onStartStop) reset_button = wx.Button(self, wx.ID_ANY, 'Reset', (245, 58), size=(50, 25)) reset_button.Bind(wx.EVT_BUTTON, self.onReset) 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.msg_count_time = 0 self.distance = 0 self.time_old = 0 self.time_elapsed = 0 self.ins_msg_x = 0 self.ins_msg_y = 0 self.ins_msg_z = 0 self.init = 0 self.running = True def OnClose(self, event): self.interface.shutdown() self.Destroy()
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()
class FormationControl: def __init__(self, config, freq=10., use_ground_ref=False, ignore_geo_fence=False, verbose=False): self.config = config self.step = 1. / freq self.sens = self.config['sensitivity'] self.use_ground_ref = use_ground_ref self.enabled = True # run control by default self.ignore_geo_fence = ignore_geo_fence self.verbose = verbose self.ids = self.config['ids'] self.rotorcrafts = [FC_Rotorcraft(i) for i in self.ids] self.joystick = FC_Joystick() self.altitude = 2.0 # starts from 1 m high self.scale = 1.0 self.B = np.array(self.config['topology']) self.d = np.array(self.config['desired_distances']) self.t1 = np.array(self.config['motion']['t1']) self.t2 = np.array(self.config['motion']['t2']) self.r1 = np.array(self.config['motion']['r1']) self.r2 = np.array(self.config['motion']['r2']) self.k = np.array(self.config['gains']) if self.B.size == 2: self.B.shape = (2,1) # Check formation settings if len(self.ids) != np.size(self.B, 0): print("The number of rotorcrafts in the topology and ids do not match") return if np.size(self.d) != np.size(self.B, 1): print("The number of links in the topology and desired distances do not match") return #if np.size(self.d) != np.size(self.m,1): # print("The number of (columns) motion parameters and relative vectors do not match") # return #if np.size(self.m, 0) != 8: # print("The number of (rows) motion parameters must be eight") # return if self.config['3D'] == True: print("3D formation is not supported yet") return # Start IVY interface self._interface = IvyMessagesInterface("Formation Control Rotorcrafts") # bind to INS message def ins_cb(ac_id, msg): if ac_id in self.ids and msg.name == "INS": rc = self.rotorcrafts[self.ids.index(ac_id)] i2p = 1. / 2**8 # integer to position i2v = 1. / 2**19 # integer to velocity rc.X[0] = float(msg['ins_x']) * i2p rc.X[1] = float(msg['ins_y']) * i2p rc.X[2] = float(msg['ins_z']) * i2p rc.V[0] = float(msg['ins_xd']) * i2v rc.V[1] = float(msg['ins_yd']) * i2v rc.V[2] = float(msg['ins_zd']) * i2v rc.timeout = 0 rc.initialized = True if not self.use_ground_ref: self._interface.subscribe(ins_cb, PprzMessage("telemetry", "INS")) # bind to GROUND_REF message def ground_ref_cb(ground_id, msg): ac_id = int(msg['ac_id']) if ac_id in self.ids: rc = self.rotorcrafts[self.ids.index(ac_id)] # X and V in NED rc.X[0] = float(msg['pos'][1]) rc.X[1] = float(msg['pos'][0]) rc.X[2] = -float(msg['pos'][2]) rc.V[0] = float(msg['speed'][1]) 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) X = np.zeros(dim) V = np.zeros(dim) U = np.zeros(dim) i = 0 for rc in self.rotorcrafts: X[i] = rc.X[0] X[i+1] = rc.X[1] V[i] = rc.V[0] V[i+1] = rc.V[1] i = i + 2 Bb = la.kron(self.B, np.eye(2)) Z = Bb.T.dot(X) Dz = rf.make_Dz(Z, 2) Dzt = rf.make_Dzt(Z, 2, 1) Dztstar = rf.make_Dztstar(self.d * self.scale, 2, 1) Zh = rf.make_Zh(Z, 2) E = rf.make_E(Z, self.d * self.scale, 2, 1) # Shape and motion control jmu_t1 = self.joystick.trans * self.t1[0,:] jtilde_mu_t1 = self.joystick.trans * self.t1[1,:] jmu_r1 = self.joystick.rot * self.r1[0,:] jtilde_mu_r1 = self.joystick.rot * self.r1[1,:] jmu_t2 = self.joystick.trans2 * self.t2[0,:] jtilde_mu_t2 = self.joystick.trans2 * self.t2[1,:] jmu_r2 = self.joystick.rot2 * self.r2[0,:] jtilde_mu_r2 = self.joystick.rot2 * self.r2[1,:] Avt1 = rf.make_Av(self.B, jmu_t1, jtilde_mu_t1) Avt1b = la.kron(Avt1, np.eye(2)) Avr1 = rf.make_Av(self.B, jmu_r1, jtilde_mu_r1) Avr1b = la.kron(Avr1, np.eye(2)) Avt2 = rf.make_Av(self.B, jmu_t2, jtilde_mu_t2) Avt2b = la.kron(Avt2, np.eye(2)) Avr2 = rf.make_Av(self.B, jmu_r2, jtilde_mu_r2) Avr2b = la.kron(Avr2, np.eye(2)) Avb = Avt1b + Avr1b + Avt2b + Avr2b Avr = Avr1 + Avr2 if self.B.size == 2: Zhr = np.array([-Zh[1],Zh[0]]) U = -self.k[1]*V - self.k[0]*Bb.dot(Dz).dot(Dzt).dot(E) + self.k[1]*(Avt1b.dot(Zh) + Avt2b.dot(Zhr)) + np.sign(jmu_r1[0])*la.kron(Avr1.dot(Dztstar).dot(self.B.T).dot(Avr1), np.eye(2)).dot(Zhr) else: U = -self.k[1]*V - self.k[0]*Bb.dot(Dz).dot(Dzt).dot(E) + self.k[1]*Avb.dot(Zh) + la.kron(Avr.dot(Dztstar).dot(self.B.T).dot(Avr), np.eye(2)).dot(Zh) if self.verbose: #print "Positions: " + str(X).replace('[','').replace(']','') #print "Velocities: " + str(V).replace('[','').replace(']','') #print "Acceleration command: " + str(U).replace('[','').replace(']','') print "Error distances: " + str(E).replace('[','').replace(']','') sys.stdout.flush() i = 0 for ac in self.rotorcrafts: msg = PprzMessage("datalink", "DESIRED_SETPOINT") msg['ac_id'] = ac.id msg['flag'] = 0 # full 3D not supported yet msg['ux'] = U[i] msg['uy'] = U[i+1] msg['uz'] = self.altitude self._interface.send(msg) i = i+2 def run(self): try: # The main loop while True: # TODO: make better frequency managing sleep(self.step) for rc in self.rotorcrafts: rc.timeout = rc.timeout + self.step # Run the formation algorithm if self.enabled: self.formation() except KeyboardInterrupt: self.stop()
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()
class OpenSkyTraffic(object): def __init__(self, period=10., margin=1., timeout=60., verbose=False): self.period = period self.margin = margin self.timeout = timeout self.last_receive = time() self.verbose = verbose self._interface = IvyMessagesInterface("OpenSkyTraffic") self._opensky = OpenSkyApi() self.ac_list = {} self.intruder_list = {} self.running = False if self.verbose: print("Starting opensky interface...") # subscribe to FLIGHT_PARAM ground message self._interface.subscribe(self.update_ac, PprzMessage("ground", "FLIGHT_PARAM")) def stop(self): if self.verbose: print("Shutting down opensky interface...") self.running = False if self._interface is not None: self._interface.shutdown() def __del__(self): self.stop() def update_ac(self, ac_id, msg): # update A/C info self.last_receive = time() self.ac_list[ac_id] = (self.last_receive, float(msg['lat']), float(msg['long'])) def filter_ac(self): # purge timeout A/C timeout = time() - self.timeout for ac, (t, lat, lon) in self.ac_list.items(): if t < timeout: del self.ac_list[ac] def compute_bbox_list(self): bbox = [] # for now assume that all UAVs are close enough, so merge all boxes into one # future improvement could handle groups of UAVs far appart lat_min, lat_max, lon_min, lon_max = None, None, None, None for ac, (t, lat, lon) in self.ac_list.items(): if lat_min is None: lat_min = lat lat_max = lat lon_min = lon lon_max = lon else: lat_min = min(lat, lat_min) lat_max = max(lat, lat_max) lon_min = min(lon, lon_min) lon_max = max(lon, lon_max) if lat_min is not None: bbox.append((lat_min-self.margin, lat_max+self.margin, lon_min-self.margin, lon_max+self.margin)) return bbox def get_intruders(self): self.filter_ac() bbox = self.compute_bbox_list() # request surounding aircraft to opensky-network self.intruder_list = {} for bb in bbox: states = self._opensky.get_states(bbox=bb) if states is not None: for s in states.states: self.intruder_list[s.callsign] = s def send_intruder_msgs(self): self.get_intruders() def val_or_default(val, default): if val is None: return default else: return val for i in self.intruder_list: intruder = self.intruder_list[i] if intruder.callsign is not None and len(intruder.callsign) > 0: msg = PprzMessage("ground", "INTRUDER") msg['id'] = intruder.icao24 msg['name'] = intruder.callsign.replace(" ", "") msg['lat'] = int(intruder.latitude * 1e7) msg['lon'] = int(intruder.longitude * 1e7) msg['alt'] = int(val_or_default(intruder.geo_altitude, 0.) * 1000.) msg['course'] = val_or_default(intruder.heading, 0.) msg['speed'] = val_or_default(intruder.velocity, 0.) msg['climb'] = val_or_default(intruder.vertical_rate, 0.) msg['itow'] = intruder.time_position if self.verbose: print(msg) self._interface.send(msg) def run(self): try: self.running = True t = 0. while self.running: t = t + 0.1 # increment timer until next update time is reach if t > self.period: self.send_intruder_msgs() t = 0. sleep(0.1) # wait a short time except KeyboardInterrupt: self.stop() except Exception as e: print("Failing with: "+str(e)) self.stop()
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()
class DroneControler(object): def __init__(self, ac_id, plateform_id, tag_id): # Start a ivy messages listener named PIR DRONE self._interface = IvyMessagesInterface("PIR", ivy_bus="192.168.1:2010") self.drone = FC_Rotorcraft(ac_id, tag_id, self._interface) self.plateform = Platform(plateform_id) # bind to GROUND_REF message def ground_ref_cb(ground_id, msg): ac_id = int(msg['ac_id']) if ac_id == self.drone.id: # X and V in NED self.drone.X_opti[0] = float(msg['pos'][1]) self.drone.X_opti[1] = float(msg['pos'][0]) self.drone.X_opti[2] = -float(msg['pos'][2]) self.drone.V_opti[0] = float(msg['speed'][1]) self.drone.V_opti[1] = float(msg['speed'][0]) self.drone.V_opti[2] = -float(msg['speed'][2]) self.drone.quaternions[0] = float(msg['quat'][0]) self.drone.quaternions[1] = float(msg['quat'][1]) self.drone.quaternions[2] = float(msg['quat'][2]) self.drone.quaternions[3] = float(msg['quat'][3]) self.drone.timeLastUpdate = time.time() self.drone.initialized = True if ac_id == self.plateform.id: # X and V in NED self.plateform.X[0] = float(msg['pos'][1]) self.plateform.X[1] = float(msg['pos'][0]) self.plateform.X[2] = -float(msg['pos'][2]) self.plateform.V[0] = float(msg['speed'][1]) self.plateform.V[1] = float(msg['speed'][0]) self.plateform.V[2] = -float(msg['speed'][2]) self.plateform.quaternions[0] = float(msg['quat'][0]) self.plateform.quaternions[1] = float(msg['quat'][1]) self.plateform.quaternions[2] = float(msg['quat'][2]) self.plateform.quaternions[3] = float(msg['quat'][3]) self.plateform.timeLastUpdate = time.time() self.plateform.initialized = True self._interface.subscribe(ground_ref_cb, PprzMessage("ground", "GROUND_REF")) # Recuperation du x et y via l'UWB def get_xy_uwb(uwb_id, msg): # X and V in NED #msg est un tableau contenant : d1corr, d2corr, d3corr, d1vrai, d2vrai, d3vrai, x, y, z, vx, vy, vz x = float(msg['values'][6]) y = float(msg['values'][7]) z = self.drone.X_opti[3] psi = cr.getEulerAngles( self.drone.quaternions)[2] - cr.getEulerAngles( self.plateform.quaternions)[2] #self.drone.V_uwb[0] = float(msg['values'][9]) #self.drone.V_uwb[1] = float(msg['values'][10]) self.drone.X_uwb = cr.uwb2pseudoBody(x, y, z) self._interface.subscribe(get_xy_uwb, PprzMessage("telemetry", "PAYLOAD_FLOAT")) def stop(self): # Stop IVY interface print("Stopping Ivy interface") if self._interface is not None: self._interface.shutdown() def envoi_cmd(self, phi, theta, psi, throttle): msg = PprzMessage("datalink", "JOYSTICK_RAW") msg['ac_id'] = self.drone.id msg['roll'] = phi msg['pitch'] = theta msg['yaw'] = psi msg['throttle'] = throttle self._interface.send_raw_datalink(msg) def envoi_cmd_guidedSetPointNED(self): msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg['ac_id'] = self.drone.id msg['flags'] = 0b100011 # x and y (position) as offset coordinate in pseudoBody frame phi, theta, psi = cr.getEulerAngles(self.drone.quaternions) Xtag_pseudoBody = cr.cam2pseudoBody(self.drone.cam.tag.X, phi, theta) msg['x'] = Xtag_pseudoBody[0] / 2.3 msg['y'] = Xtag_pseudoBody[1] / 2.3 msg['z'] = -1.3 # COmmande en NED so the z axis is oriented facing down msg['yaw'] = 0 print(msg) self._interface.send_raw_datalink(msg) def gen_commande(self, x, y, z, psi): """Entree : coordonnes x,y,z du drone dans le repere pseudo drone Sortie : Commande phi, theta, psi,V_z""" pidx = pid.PID(Px, Ix, Dx) pidy = pid.PID(Py, Iy, Dy) phi = pidx.update(x) theta = pidy.update(y) psi = getEulerAngles(self.drone.quaternions)[2] - getEulerAngles( self.plateform.quaternions)[2] V_z = pidz.update(z) return phi, theta, psi, V_z def update(self): [self.drone.U[0], self.drone.U[1], self.drone.U[2], self.drone.U[3]] = self.gen_commande(self.drone.X[0], self.drone.X[1], self.drone.X[2]) self.envoi_cmd(phi, theta, psi, throttle) def run(self): while (True): self.update()
class PprzConnect(object): """ Main class to handle the initialization process with the server in order to retrieve the configuration of the known aircraft and update for the new ones """ def __init__(self, notify=None, ivy=None, verbose=False): """ Init function Create an ivy interface if not provided and request for all aircraft :param notify: callback function called on new aircraft, takes a PprzConfig as parameter :param ivy: ivy interface to contact the server, if None a new one will be created :param verbose: display debug information """ self.verbose = verbose self._notify = notify self._req_idx = 0 self._conf_list_by_name = {} self._conf_list_by_id = {} if ivy is None: self._ivy = IvyMessagesInterface("PprzConnect") else: self._ivy = ivy sleep(0.1) self.get_aircrafts() def __del__(self): self.shutdown() def shutdown(self): """ Shutdown function Should be called before leaving if the ivy interface is not closed elsewhere """ if self._ivy is not None: if self.verbose: print("Shutting down ivy interface...") self._ivy.shutdown() self._ivy = None def conf_by_name(self, ac_name=None): """ Get a conf by its name :param ac_name: aircraft name, if None the complete dict is returned :type ac_name: str """ if ac_name is not None: return self._conf_list_by_name[ac_name] else: return self._conf_list_by_name def conf_by_id(self, ac_id=None): """ Get a conf by its ID :param ac_id: aircraft id, if None the complete dict is returned :type ac_id: str """ if ac_id is not None: return self._conf_list_by_id[ac_id] else: return self._conf_list_by_id @property def ivy(self): """ Getter function for the ivy interface """ return self._ivy def _get_req_id(self): req_id = '{}_{}'.format(getpid(), self._req_idx) self._req_idx += 1 return req_id def _message_req(self, msg_name, cb, params=None): bind_id = None def _cb(sender, msg): if bind_id is not None: self._ivy.unsubscribe(bind_id) cb(sender, msg) req_id = self._get_req_id() req_regex = '^{} ([^ ]* +{}( .*|$))'.format(req_id, msg_name) bind_id = self._ivy.subscribe(_cb, req_regex) req_msg = PprzMessage('ground', '{}_REQ'.format(msg_name)) if params is not None: req_msg.set_values(params) #FIXME we shouldn't use directly Ivy, but pprzlink python API is not supporting the request id for now IvySendMsg('pprz_connect {} {} {}'.format( req_id, req_msg.name, req_msg.payload_to_ivy_string())) #self._ivy.send(req_msg) def get_aircrafts(self): """ request all aircrafts IDs from a runing server and new aircraft when they appear """ def aircrafts_cb(sender, msg): ac_list = msg['ac_list'] for ac_id in ac_list: self.get_config(ac_id) #ac_list = [int(a) for a in msg['ac_list'].split(',') if a] if self.verbose: print("aircrafts: {}".format(ac_list)) self._message_req("AIRCRAFTS", aircrafts_cb) def new_ac_cb(sender, msg): ac_id = msg['ac_id'] self.get_config(ac_id) if self.verbose: print("new aircraft: {}".format(ac_id)) self._ivy.subscribe(new_ac_cb, PprzMessage('ground', 'NEW_AIRCRAFT')) def get_config(self, ac_id): """ Requsest a config from the server for a given ID :param ac_id: aircraft ID :type ac_id: str """ def conf_cb(sender, msg): conf = PprzConfig(msg['ac_id'], msg['ac_name'], msg['airframe'], msg['flight_plan'], msg['settings'], msg['radio'], msg['default_gui_color']) self._conf_list_by_name[conf.name] = conf self._conf_list_by_id[int(conf.id)] = conf if self._notify is not None: self._notify(conf) # user defined general callback if self.verbose: print(conf) self._message_req("CONFIG", conf_cb, [ac_id])
class FormationControl: def __init__(self, config, freq=10., verbose=False): self.config = config self.step = 1. / freq self.verbose = verbose self.ids = self.config['ids'] self.B = np.array(self.config['topology']) self.Zdesired = np.array(self.config['desired_intervehicle_angles_degrees'])*np.pi/180 self.k = np.array(self.config['gain']) self.radius = np.array(self.config['desired_stationary_radius_meters']) self.aircraft = [Aircraft(i) for i in self.ids] self.sigmas = np.zeros(len(self.aircraft)) for ac in self.aircraft: settings = PaparazziACSettings(ac.id) list_of_indexes = ['ell_a', 'ell_b'] for setting_ in list_of_indexes: try: index = settings.name_lookup[setting_].index if setting_ == 'ell_a': ac.a_index = index if setting_ == 'ell_b': ac.b_index = index except Exception as e: print(e) print(setting_ + " setting not found, \ have you forgotten to check gvf.xml for your settings?") # Start IVY interface self._interface = IvyMessagesInterface("Circular Formation") # bind to NAVIGATION message def nav_cb(ac_id, msg): if ac_id in self.ids and msg.name == "NAVIGATION": ac = self.aircraft[self.ids.index(ac_id)] ac.XY[0] = float(msg.get_field(2)) ac.XY[1] = float(msg.get_field(3)) ac.initialized_nav = True self._interface.subscribe(nav_cb, PprzMessage("telemetry", "NAVIGATION")) def gvf_cb(ac_id, msg): if ac_id in self.ids and msg.name == "GVF": if int(msg.get_field(1)) == 1: ac = self.aircraft[self.ids.index(ac_id)] param = msg.get_field(4) ac.XYc[0] = float(param[0]) ac.XYc[1] = float(param[1]) ac.a = float(param[2]) ac.b = float(param[3]) ac.s = float(msg.get_field(2)) ac.initialized_gvf = True self._interface.subscribe(gvf_cb, PprzMessage("telemetry", "GVF")) def __del__(self): self.stop() def stop(self): # Stop IVY interface if self._interface is not None: self._interface.shutdown() def circular_formation(self): ''' circular formation control algorithm ''' ready = True for ac in self.aircraft: if (not ac.initialized_nav) or (not ac.initialized_gvf): if self.verbose: print("Waiting for state of aircraft ", ac.id) ready = False if not ready: return i = 0 for ac in self.aircraft: ac.sigma = np.arctan2(ac.XY[1]-ac.XYc[1], ac.XY[0]-ac.XYc[0]) self.sigmas[i] = ac.sigma i = i + 1 inter_sigma = self.B.transpose().dot(self.sigmas) error_sigma = inter_sigma - self.Zdesired if np.size(error_sigma) > 1: for i in range(0, np.size(error_sigma)): if error_sigma[i] > np.pi: error_sigma[i] = error_sigma[i] - 2*np.pi elif error_sigma[i] <= -np.pi: error_sigma[i] = error_sigma[i] + 2*np.pi else: if error_sigma > np.pi: error_sigma = error_sigma - 2*np.pi elif error_sigma <= -np.pi: error_sigma = error_sigma + 2*np.pi u = -self.aircraft[0].s*self.k*self.B.dot(error_sigma) if self.verbose: print("Inter-vehicle errors: ", str(error_sigma*180.0/np.pi).replace('[','').replace(']','')) i = 0 for ac in self.aircraft: msga = PprzMessage("ground", "DL_SETTING") msga['ac_id'] = ac.id msga['index'] = ac.a_index msga['value'] = self.radius + u[i] msgb = PprzMessage("ground", "DL_SETTING") msgb['ac_id'] = ac.id msgb['index'] = ac.b_index msgb['value'] = self.radius + u[i] self._interface.send(msga) self._interface.send(msgb) i = i + 1 def run(self): try: # The main loop while True: # TODO: make better frequency managing sleep(self.step) # Run the formation algorithm self.circular_formation() except KeyboardInterrupt: self.stop()
class FormationControl: def __init__(self, config, freq=10., verbose=False): self.config = config self.step = 1. / freq self.verbose = verbose self.ids = self.config['ids'] self.B = np.array(self.config['topology']) self.Zdesired = np.array(self.config['desired_intervehicle_angles']) self.k = np.array(self.config['gain']) self.radius = np.array(self.config['desired_stationary_radius']) self.aircraft = [Aircraft(i) for i in self.ids] self.sigmas = np.zeros(len(self.aircraft)) for ac in self.aircraft: settings = PaparazziACSettings(ac.id) list_of_indexes = ['ell_a', 'ell_b'] for setting_ in list_of_indexes: try: index = settings.name_lookup[setting_].index if setting_ == 'ell_a': ac.a_index = index if setting_ == 'ell_b': ac.b_index = index except Exception as e: print(e) print(setting_ + " setting not found, \ have you forgotten to check gvf.xml for your settings?") # Start IVY interface self._interface = IvyMessagesInterface("Circular Formation") # bind to NAVIGATION message def nav_cb(ac_id, msg): if ac_id in self.ids and msg.name == "NAVIGATION": ac = self.aircraft[self.ids.index(ac_id)] ac.XY[0] = float(msg.get_field(2)) ac.XY[1] = float(msg.get_field(3)) ac.initialized_nav = True self._interface.subscribe(nav_cb, PprzMessage("telemetry", "NAVIGATION")) def gvf_cb(ac_id, msg): if ac_id in self.ids and msg.name == "GVF": if int(msg.get_field(1)) == 1: ac = self.aircraft[self.ids.index(ac_id)] param = msg.get_field(4) ac.XYc[0] = float(param[0]) ac.XYc[1] = float(param[1]) ac.a = float(param[2]) ac.b = float(param[3]) ac.s = float(msg.get_field(2)) ac.initialized_gvf = True self._interface.subscribe(gvf_cb, PprzMessage("telemetry", "GVF")) def __del__(self): self.stop() def stop(self): # Stop IVY interface if self._interface is not None: self._interface.shutdown() def circular_formation(self): ''' circular formation control algorithm ''' ready = True for ac in self.aircraft: if (not ac.initialized_nav) or (not ac.initialized_gvf): if self.verbose: print("Waiting for state of aircraft ", ac.id) ready = False if not ready: return i = 0 for ac in self.aircraft: ac.sigma = np.arctan2(ac.XY[1]-ac.XYc[1], ac.XY[0]-ac.XYc[0]) self.sigmas[i] = ac.sigma i = i + 1 inter_sigma = self.B.transpose().dot(self.sigmas) error_sigma = inter_sigma - self.Zdesired if np.size(error_sigma) > 1: for i in range(0, np.size(error_sigma)): if error_sigma[i] > np.pi: error_sigma[i] = error_sigma[i] - 2*np.pi elif error_sigma[i] <= -np.pi: error_sigma[i] = error_sigma[i] + 2*np.pi else: if error_sigma > np.pi: error_sigma = error_sigma - 2*np.pi elif error_sigma <= -np.pi: error_sigma = error_sigma + 2*np.pi u = -self.aircraft[0].s*self.k*self.B.dot(error_sigma) if self.verbose: print("Inter-vehicle errors: ", str(error_sigma*180.0/np.pi).replace('[','').replace(']','')) i = 0 for ac in self.aircraft: msga = PprzMessage("ground", "DL_SETTING") msga['ac_id'] = ac.id msga['index'] = ac.a_index msga['value'] = self.radius + u[i] msgb = PprzMessage("ground", "DL_SETTING") msgb['ac_id'] = ac.id msgb['index'] = ac.b_index msgb['value'] = self.radius + u[i] self._interface.send(msga) self._interface.send(msgb) i = i + 1 def run(self): try: # The main loop while True: # TODO: make better frequency managing sleep(self.step) # Run the formation algorithm self.circular_formation() except KeyboardInterrupt: self.stop()
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.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD) 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 == '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()
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)
class WindFrame(wx.Frame): def message_recv(self, ac_id, msg): if msg.name == "ROTORCRAFT_FP": self.ground_gs_x[self.count_gs] = float(msg['veast']) * 0.0000019 self.ground_gs_y[self.count_gs] = float(msg['vnorth']) * 0.0000019 self.last_heading = float(msg['psi']) * 0.0139882 self.count_gs = self.count_gs + 1 if self.count_gs > MSG_BUFFER_SIZE: self.count_gs = 0 wx.CallAfter(self.update) elif msg.name == "AIR_DATA": self.airspeed[self.count_as] = float(msg['airspeed']) self.heading[self.count_as] = self.last_heading * math.pi / 180.0 self.count_as = self.count_as + 1 if self.count_as > MSG_BUFFER_SIZE: self.count_as = 0 wx.CallAfter(self.update) def update(self): self.Refresh() def OnSize(self, event): self.w = event.GetSize().x self.h = event.GetSize().y self.cfg.Write("width", str(self.w)) self.cfg.Write("height", str(self.h)) self.Refresh() def OnMove(self, event): self.x = event.GetPosition().x self.y = event.GetPosition().y self.cfg.Write("left", str(self.x)) self.cfg.Write("top", str(self.y)) def OnClickD(self, event): self.click_on = 1 def OnClickU(self, event): self.click_on = 0 def OnClickM(self, event): if self.click_on == 1: m = event.GetPosition().Get() self.click_x = m[0] - self.mid self.click_y = m[1] - self.mid self.Refresh() def OnPaint(self, e): w = float(self.w) h = float(self.h) if w / h > (WIDTH / (WIDTH + BARH)): w = (WIDTH / (WIDTH + BARH)) * h else: h = ((WIDTH + BARH) / (WIDTH)) * w bar = BARH / WIDTH * w tdx = -5.0 / WIDTH * w tdy = -7.0 / WIDTH * w th = 15.0 / WIDTH * w dc = wx.PaintDC(self) brush = wx.Brush("white") dc.SetBackground(brush) dc.Clear() self.mid = w / 2 diameter = w / 2 # Background dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT)) # Speed circles for v in range(0, 40, 5): dc.DrawCircle(self.mid, self.mid, diameter * v / MAX_AIRSPEED) font = wx.Font(11, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD) dc.SetFont(font) dc.DrawText("N", self.mid + tdx, 2) dc.DrawText("S", self.mid + tdx, w - 17) dc.DrawText("E", w - 15, w / 2 + tdy) dc.DrawText("W", 2, w / 2 + tdy) # Ground Speed dc.SetBrush(wx.Brush(wx.Colour(0, 0, 255), wx.SOLID)) for i in range(0, MSG_BUFFER_SIZE): gx = self.ground_gs_x[i] gy = self.ground_gs_y[i] dc.DrawCircle( int(gx * diameter / MAX_AIRSPEED + self.mid + self.click_x), int(gy * diameter / MAX_AIRSPEED + self.mid + self.click_y), 2) # Airspeed in function of heading dc.SetBrush(wx.Brush(wx.Colour(255, 0, 0), wx.SOLID)) for i in range(0, MSG_BUFFER_SIZE): gx = self.airspeed[i] * math.cos(self.heading[i]) gy = self.airspeed[i] * math.sin(self.heading[i]) dc.DrawCircle(int(gx * diameter / MAX_AIRSPEED + self.mid), int(gy * diameter / MAX_AIRSPEED + self.mid), 2) # Result font = wx.Font(8, wx.ROMAN, wx.NORMAL, wx.NORMAL) dc.SetFont(font) dc.DrawText( "#" + str(self.count_gs) + ", " + str(self.count_as) + " | " + str(self.click_x) + "-" + str(self.click_y), 0, h - 14) windspeed = math.sqrt(self.click_x * +self.click_x + +self.click_y * +self.click_y) / diameter * MAX_AIRSPEED windheading = math.atan2(self.click_x, -self.click_y) * 180 / math.pi fontsize = int(16.0 / WIDTH * w) font = wx.Font(fontsize, wx.ROMAN, wx.NORMAL, wx.NORMAL) dc.SetFont(font) dc.DrawText( "Wind = " + str(round(windspeed, 1)) + " m/s from " + str(round(windheading, 0)), 0, w) def __init__(self): # own data self.count_gs = 0 self.ground_gs_x = [0] * MSG_BUFFER_SIZE self.ground_gs_y = [0] * MSG_BUFFER_SIZE self.count_as = 0 self.last_heading = 0 self.airspeed = [0] * MSG_BUFFER_SIZE self.heading = [0] * MSG_BUFFER_SIZE # Click self.click_x = 0 self.click_y = 0 self.click_on = 0 # Window self.w = WIDTH self.h = WIDTH + BARH self.cfg = wx.Config('wind_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) self.mid = self.w / 2 wx.Frame.__init__(self, id=-1, parent=None, name=u'WindFrame', size=wx.Size(self.w, self.h), title=u'Wind Tool') if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x, self.y), wx.SIZE_USE_EXISTING) self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) self.Bind(wx.EVT_CLOSE, self.OnClose) self.Bind(wx.EVT_LEFT_DOWN, self.OnClickD) self.Bind(wx.EVT_MOTION, self.OnClickM) self.Bind(wx.EVT_LEFT_UP, self.OnClickU) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/wind/wind.png", wx.BITMAP_TYPE_PNG) self.SetIcon(ico) self.interface = IvyMessagesInterface("windframe") self.interface.subscribe(self.message_recv) def OnClose(self, event): self.interface.shutdown() self.Destroy()
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()
class EnergyMonFrame(wx.Frame): def message_recv(self, ac_id, msg): if msg.name == "ENERGY": self.bat = EnergyMessage(msg) self.cell.fill_from_energy_msg(self.bat) wx.CallAfter(self.update) elif msg.name == "TEMP_ADC": self.temp = TempMessage(msg) self.cell.fill_from_temp_msg(self.temp) 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 StatusBox(self, dc, nr, txt, percent, color): if percent < 0: percent = 0 if percent > 1: percent = 1 boxw = self.stat tdx = int(boxw * 10.0 / 300.0) tdy = int(boxw * 6.0 / 300.0) boxh = int(boxw * 40.0 / 300.0) boxw = self.stat - 2*tdx spacing = boxh+10 dc.SetPen(wx.Pen(wx.Colour(0,0,0))) dc.SetBrush(wx.Brush(wx.Colour(220,220,220))) dc.DrawRectangle(tdx, int(nr*spacing+tdx), int(boxw), boxh) if color < 0.2: dc.SetBrush(wx.Brush(wx.Colour(250,0,0))) elif color < 0.6: dc.SetBrush(wx.Brush(wx.Colour(250,180,0))) else: dc.SetBrush(wx.Brush(wx.Colour(0,250,0))) # dc.DrawLine(200,50,350,50) dc.DrawRectangle(tdx, int(nr*spacing+tdx), int(boxw * percent), boxh) dc.DrawText(txt,18,int(nr*spacing+tdy+tdx)) def plot_x(self, x): return int(self.stat+self.tdx + x * (self.w-self.stat-2*self.tdx)) def plot_y(self, y): return int(self.tdx + (1-y) * (self.h-self.tdx-self.tdx)) def plot(self, dc, i1, i2): dc.DrawLine(self.plot_x(i1[1]/3500), self.plot_y((i1[0]-2.5)/(4.2-2.5)), self.plot_x(i2[1]/3500), self.plot_y((i2[0]-2.5)/(4.2-2.5))) def DischargePlot(self, dc): self.tdx = int(self.stat * 10.0 / 300.0) dc.SetPen(wx.Pen(wx.Colour(0,0,0),1)) dc.SetBrush(wx.Brush(wx.Colour(250,250,250))) dc.DrawRectangle(self.plot_x(0.0), self.plot_y(1.0), self.w-self.stat-2*self.tdx, self.h-2*self.tdx) for i in range(0,5): dc.DrawLine(self.plot_x(0.0), self.plot_y(i/5.0), self.plot_x(1.0), self.plot_y(i/5.0)) for i in range(0,7): dc.DrawLine(self.plot_x(i/7.0), self.plot_y(0), self.plot_x(i/7.0), self.plot_y(1)) dc.SetPen(wx.Pen(wx.Colour(255,180,0),4)) dc.DrawLine(self.plot_x(self.cell.model/3500), self.plot_y(0), self.plot_x(self.cell.model/3500), self.plot_y(1)) dc.DrawLine(self.plot_x(0.0), self.plot_y(self.cell.get_volt_perc()), self.plot_x(1.0), self.plot_y(self.cell.get_volt_perc())) thickness = 3 dc.SetPen(wx.Pen(wx.Colour(0,0,0),thickness)) li = bat.batmodel[0,[0,1]] for i in bat.batmodel[:,[0,1]]: self.plot(dc,li,i) li=i dc.SetPen(wx.Pen(wx.Colour(0,0,255),thickness)) li = bat.batmodel[0,[0,2]] for i in bat.batmodel[:,[0,2]]: self.plot(dc,li,i) li=i dc.SetPen(wx.Pen(wx.Colour(0,255,0),thickness)) li = bat.batmodel[0,[0,3]] for i in bat.batmodel[:,[0,3]]: self.plot(dc,li,i) li=i dc.SetPen(wx.Pen(wx.Colour(255,255,0),thickness)) li = bat.batmodel[0,[0,4]] for i in bat.batmodel[:,[0,4]]: self.plot(dc,li,i) li=i dc.SetPen(wx.Pen(wx.Colour(255,0,0),thickness)) li = bat.batmodel[0,[0,5]] for i in bat.batmodel[:,[0,5]]: self.plot(dc,li,i) li=i def OnPaint(self, e): # Automatic Scaling w = self.w h = self.h self.stat = int(w/4) if self.stat<100: self.stat=100 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)) fontscale = int(w * 11.0 / 800.0) if fontscale < 6: fontscale = 6 font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD) dc.SetFont(font) self.StatusBox(dc,0, self.cell.get_volt(), self.cell.get_volt_perc(), self.cell.get_volt_color()) self.StatusBox(dc,1, self.cell.get_current(), self.cell.get_current_perc(), self.cell.get_current_color() ) self.StatusBox(dc,2, self.cell.get_energy(), self.cell.get_energy_perc(), self.cell.get_energy_color() ) self.StatusBox(dc,3, self.cell.get_mah_from_volt(), self.cell.get_energy_perc(), self.cell.get_energy_color() ) self.StatusBox(dc,4, self.cell.get_temp(), self.cell.get_temp_perc(), self.cell.get_temp_color()) self.DischargePlot(dc) def __init__(self): self.w = WIDTH self.h = WIDTH + BARH wx.Frame.__init__(self, id=-1, parent=None, name=u'EnergyMonFrame', size=wx.Size(self.w, self.h), title=u'Energy Monitoring') 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/energy_mon/energy_mon.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.bat = {} self.temp = {} self.cell = BatteryCell(); self.interface = IvyMessagesInterface("energymonframe") self.interface.subscribe(self.message_recv) def OnClose(self, event): self.interface.shutdown() self.Destroy()
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 import messages_xml_map try: msgs = messages_xml_map.get_msgs('test') except Exception as e: print(e) dico = messages_xml_map.message_dictionary for msg_type in dico.keys(): for msg in dico[msg_type]: print(msg_type, ":", msg) ac_id = 24 ivyInterface = IvyMessagesInterface() time.sleep(0.5) world = None uavid = None def callback01(ac_id, msg, request_id): print(request_id, msg) def callback02(ac_id, msg): print(msg) ivyInterface.subscribe(callback01, '(.* WORLD_ENV_REQ .*)') ivyInterface.subscribe(callback02, '(.* GPS .*)') signal.signal(signal.SIGINT, lambda frame, sig: ivyInterface.stop())
class PprzConnect(object): """ Main class to handle the initialization process with the server in order to retrieve the configuration of the known aircraft and update for the new ones """ def __init__(self, notify=None, ivy=None, verbose=False): """ Init function Create an ivy interface if not provided and request for all aircraft :param notify: callback function called on new aircraft, takes a PprzConfig as parameter :param ivy: ivy interface to contact the server, if None a new one will be created :param verbose: display debug information """ self.verbose = verbose self._notify = notify self._conf_list_by_name = {} self._conf_list_by_id = {} if ivy is None: self._ivy = IvyMessagesInterface("PprzConnect") else: self._ivy = ivy sleep(0.1) self.get_aircrafts() def __del__(self): self.shutdown() def shutdown(self): """ Shutdown function Should be called before leaving if the ivy interface is not closed elsewhere """ if self._ivy is not None: if self.verbose: print("Shutting down ivy interface...") self._ivy.shutdown() self._ivy = None def conf_by_name(self, ac_name=None): """ Get a conf by its name :param ac_name: aircraft name, if None the complete dict is returned :type ac_name: str """ if ac_name is not None: return self._conf_list_by_name[ac_name] else: return self._conf_list_by_name def conf_by_id(self, ac_id=None): """ Get a conf by its ID :param ac_id: aircraft id, if None the complete dict is returned :type ac_id: str """ if ac_id is not None: return self._conf_list_by_id[ac_id] else: return self._conf_list_by_id @property def ivy(self): """ Getter function for the ivy interface """ return self._ivy def get_aircrafts(self): """ request all aircrafts IDs from a runing server and new aircraft when they appear """ def aircrafts_cb(sender, msg): ac_list = msg['ac_list'] for ac_id in ac_list: self.get_config(ac_id) #ac_list = [int(a) for a in msg['ac_list'].split(',') if a] if self.verbose: print("aircrafts: {}".format(ac_list)) self._ivy.send_request('ground', "AIRCRAFTS", aircrafts_cb) def new_ac_cb(sender, msg): ac_id = msg['ac_id'] self.get_config(ac_id) if self.verbose: print("new aircraft: {}".format(ac_id)) self._ivy.subscribe(new_ac_cb,PprzMessage('ground','NEW_AIRCRAFT')) def get_config(self, ac_id): """ Requsest a config from the server for a given ID :param ac_id: aircraft ID :type ac_id: str """ def conf_cb(sender, msg): conf = PprzConfig(msg['ac_id'], msg['ac_name'], msg['airframe'], msg['flight_plan'], msg['settings'], msg['radio'], msg['default_gui_color']) self._conf_list_by_name[conf.name] = conf self._conf_list_by_id[int(conf.id)] = conf if self._notify is not None: self._notify(conf) # user defined general callback if self.verbose: print(conf) self._ivy.send_request('ground', "CONFIG", conf_cb, ac_id=ac_id)
"--verbose", action="store_true", help="verbose mode") try: # --- Startup state initialization block args = parser.parse_args() static_init_configuration_data() static_init_client_configuration_data(args.file) if args.verbose: print_aircraft_data() if args.generate: template_configuration() sys.exit(0) if args.subscribe: ivy_interface.subscribe(callback_aircraft_messages) ivy_interface.start() # Handle misc. command line arguments if args.verbose: verbose = args.verbose if args.curl: curl = args.curl print_curl_header(args.ip, args.port) # --- Main loop # Supply flask the appropriate ip address and port for the server server_host = args.ip # Store for use in htlm template generation server_port = args.port # Store for use in htlm template generation app.run(host=args.ip, port=args.port, threaded=True)
class GVFFrame(wx.Frame): def __init__(self, ac_id): wx.Frame.__init__(self, id=-1, parent=None, \ name=u'GVF', size=wx.Size(WIDTH, HEIGHT), \ style=wx.DEFAULT_FRAME_STYLE, title=u'Guidance Vector Field') # Vehicle variables self.ac_id = ac_id self.course = 0 self.yaw = 0 self.XY = np.array([0, 0]) # Desired trajectory self.timer_traj = 0 # We do not update the traj every time we receive a msg self.timer_traj_lim = 7 # (7+1) * 0.25secs self.s = 0 self.kn = 0 self.ke = 0 self.map_gvf = map2d(np.array([0, 0]), 150000) self.traj = None # Frame self.canvas = FigureCanvas(self, -1, self.map_gvf.fig) self.Bind(wx.EVT_CLOSE, self.OnClose) self.redraw_timer = wx.Timer(self) self.Bind(wx.EVT_TIMER, self.OnRedrawTimer, self.redraw_timer) self.redraw_timer.Start(100) # Ivy self.interface = IvyMessagesInterface("GVF") self.interface.subscribe(self.message_recv) settings = PaparazziACSettings(ac_id) self.ke_index = None self.kn_index = None self.indexes_are_good = 0 self.list_of_indexes = ['gvf_ke', 'gvf_kn'] for setting_ in self.list_of_indexes: try: index = settings.name_lookup[setting_].index if setting_ == 'gvf_ke': self.ke_index = index if setting_ == 'gvf_kn': self.kn_index = index self.indexes_are_good = self.indexes_are_good + 1 except Exception as e: print(e) print(setting_ + " setting not found, \ have you forgotten gvf.xml in your settings?") def message_recv(self, ac_id, msg): if int(ac_id) == self.ac_id: if msg.name == 'GPS': self.course = int(msg.get_field(3)) * np.pi / 1800 if msg.name == 'NAVIGATION': self.XY[0] = float(msg.get_field(2)) self.XY[1] = float(msg.get_field(3)) if msg.name == 'ATTITUDE': self.yaw = float(msg.get_field(1)) if msg.name == 'DL_VALUE' and \ self.indexes_are_good == len(self.list_of_indexes): if int(msg.get_field(0)) == int(self.ke_index): self.ke = float(msg.get_field(1)) if self.traj is not None: self.traj.vector_field(self.traj.XYoff, \ self.map_gvf.area, self.s, self.kn, self.ke) if int(msg.get_field(0)) == int(self.kn_index): self.kn = float(msg.get_field(1)) if self.traj is not None: self.traj.vector_field(self.traj.XYoff, \ self.map_gvf.area, self.s, self.kn, self.ke) if msg.name == 'GVF': self.gvf_error = float(msg.get_field(0)) # Straight line if int(msg.get_field(1)) == 0 \ and self.timer_traj == self.timer_traj_lim: self.s = int(msg.get_field(2)) param = [float(x) for x in msg.get_field(3).split(',')] a = param[0] b = param[1] c = param[2] self.traj = traj_line(np.array([-100, 100]), a, b, c) self.traj.vector_field(self.traj.XYoff, self.map_gvf.area, \ self.s, self.kn, self.ke) # Ellipse if int(msg.get_field(1)) == 1 \ and self.timer_traj == self.timer_traj_lim: self.s = int(msg.get_field(2)) param = [float(x) for x in msg.get_field(3).split(',')] ex = param[0] ey = param[1] ea = param[2] eb = param[3] ealpha = param[4] self.traj = traj_ellipse(np.array([ex, ey]), ealpha, ea, eb) self.traj.vector_field(self.traj.XYoff, \ self.map_gvf.area, self.s, self.kn, self.ke) # Sin if int(msg.get_field(1)) == 2 \ and self.timer_traj == self.timer_traj_lim: self.s = int(msg.get_field(2)) param = [float(x) for x in msg.get_field(3).split(',')] a = param[0] b = param[1] alpha = param[2] w = param[3] off = param[4] A = param[5] self.traj = traj_sin(np.array([-100, 100]), a, b, alpha, \ w, off, A) self.traj.vector_field(self.traj.XYoff, \ self.map_gvf.area, self.s, self.kn, self.ke) self.timer_traj = self.timer_traj + 1 if self.timer_traj > self.timer_traj_lim: self.timer_traj = 0 def draw_gvf(self, XY, yaw, course): if self.traj is not None: self.map_gvf.draw(XY, yaw, course, self.traj) def OnClose(self, event): self.interface.shutdown() self.Destroy() def OnRedrawTimer(self, event): self.draw_gvf(self.XY, self.yaw, self.course) self.canvas.draw()
class WindFrame(wx.Frame): def message_recv(self, ac_id, msg): if msg.name == "ROTORCRAFT_FP": self.ground_gs_x[self.count_gs] = float(msg['veast']) * 0.0000019 self.ground_gs_y[self.count_gs] = float(msg['vnorth']) * 0.0000019 self.last_heading = float(msg['psi']) * 0.0139882 self.count_gs = self.count_gs + 1 if self.count_gs > MSG_BUFFER_SIZE: self.count_gs = 0 wx.CallAfter(self.update) elif msg.name =="AIR_DATA": self.airspeed[self.count_as] = float(msg['airspeed']) self.heading[self.count_as] = self.last_heading * math.pi / 180.0 self.count_as = self.count_as + 1 if self.count_as > MSG_BUFFER_SIZE: self.count_as = 0 wx.CallAfter(self.update) def update(self): self.Refresh() def OnSize(self, event): self.w = event.GetSize().x self.h = event.GetSize().y self.cfg.Write("width", str(self.w)); self.cfg.Write("height", str(self.h)); self.Refresh() def OnMove(self, event): self.x = event.GetPosition().x self.y = event.GetPosition().y self.cfg.Write("left", str(self.x)); self.cfg.Write("top", str(self.y)); def OnClickD(self,event): self.click_on = 1 def OnClickU(self,event): self.click_on = 0 def OnClickM(self,event): if self.click_on == 1: m = event.GetPosition().Get() self.click_x = m[0] - self.mid self.click_y = m[1] - self.mid self.Refresh() def OnPaint(self, e): w = float(self.w) h = float(self.h) if w/h > (WIDTH / (WIDTH+BARH)): w = (WIDTH / (WIDTH+BARH)) * h else: h = ((WIDTH+BARH) / (WIDTH)) * w bar = BARH / WIDTH * w tdx = -5.0 / WIDTH * w tdy = -7.0 / WIDTH * w th = 15.0 / WIDTH * w dc = wx.PaintDC(self) brush = wx.Brush("white") dc.SetBackground(brush) dc.Clear() self.mid = w/2 diameter = w/2 # Background dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT)) # Speed circles for v in range(0,40,5): dc.DrawCircle(self.mid, self.mid, diameter * v / MAX_AIRSPEED ) font = wx.Font(11, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD) dc.SetFont(font) dc.DrawText("N", self.mid + tdx, 2) dc.DrawText("S", self.mid + tdx, w - 17) dc.DrawText("E", w - 15, w / 2 + tdy) dc.DrawText("W", 2, w / 2 + tdy) # Ground Speed dc.SetBrush(wx.Brush(wx.Colour(0, 0, 255), wx.SOLID)) for i in range(0,MSG_BUFFER_SIZE): gx = self.ground_gs_x[i] gy = self.ground_gs_y[i] dc.DrawCircle(int(gx * diameter / MAX_AIRSPEED + self.mid + self.click_x), int(gy * diameter / MAX_AIRSPEED + self.mid + self.click_y), 2) # Airspeed in function of heading dc.SetBrush(wx.Brush(wx.Colour(255, 0, 0), wx.SOLID)) for i in range(0,MSG_BUFFER_SIZE): gx = self.airspeed[i] * math.cos(self.heading[i]) gy = self.airspeed[i] * math.sin(self.heading[i]) dc.DrawCircle(int(gx * diameter / MAX_AIRSPEED + self.mid), int(gy * diameter / MAX_AIRSPEED + self.mid), 2) # Result font = wx.Font(8, wx.ROMAN, wx.NORMAL, wx.NORMAL) dc.SetFont(font) dc.DrawText("#" + str(self.count_gs) + ", " + str(self.count_as) + " | " + str(self.click_x) + "-" + str(self.click_y), 0, h - 14) windspeed = math.sqrt(self.click_x * +self.click_x + +self.click_y * +self.click_y) / diameter * MAX_AIRSPEED windheading = math.atan2(self.click_x,-self.click_y) * 180 / math.pi fontsize = int(16.0 / WIDTH * w) font = wx.Font(fontsize, wx.ROMAN, wx.NORMAL, wx.NORMAL) dc.SetFont(font) dc.DrawText("Wind = " + str(round(windspeed,1)) + " m/s from " + str(round(windheading, 0)), 0, w ) def __init__(self): # own data self.count_gs = 0 self.ground_gs_x = [0] * MSG_BUFFER_SIZE self.ground_gs_y = [0] * MSG_BUFFER_SIZE self.count_as = 0 self.last_heading = 0; self.airspeed = [0] * MSG_BUFFER_SIZE self.heading = [0] * MSG_BUFFER_SIZE # Click self.click_x = 0 self.click_y = 0 self.click_on = 0 # Window self.w = WIDTH self.h = WIDTH + BARH self.cfg = wx.Config('wind_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) self.mid = self.w/2 wx.Frame.__init__(self, id=-1, parent=None, name=u'WindFrame', size=wx.Size(self.w, self.h), title=u'Wind Tool') if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x,self.y), wx.SIZE_USE_EXISTING) self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) self.Bind(wx.EVT_CLOSE, self.OnClose) self.Bind(wx.EVT_LEFT_DOWN, self.OnClickD) self.Bind(wx.EVT_MOTION, self.OnClickM) self.Bind(wx.EVT_LEFT_UP, self.OnClickU) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/wind/wind.png", wx.BITMAP_TYPE_PNG) self.SetIcon(ico) self.interface = IvyMessagesInterface("windframe") self.interface.subscribe(self.message_recv) def OnClose(self, event): self.interface.shutdown() self.Destroy()
class EnergyMonFrame(wx.Frame): def message_recv(self, ac_id, msg): if msg.name == "ENERGY": self.bat = EnergyMessage(msg) self.cell.fill_from_energy_msg(self.bat) wx.CallAfter(self.update) elif msg.name == "TEMP_ADC": self.temp = TempMessage(msg) self.cell.fill_from_temp_msg(self.temp) wx.CallAfter(self.update) elif msg.name == "AIR_DATA": self.air_data = AirDataMessage(msg) self.energy_prediction.fill_from_air_data_msg(self.air_data) wx.CallAfter(self.update) def update(self): self.Refresh() def OnSize(self, event): self.w = event.GetSize().x self.h = event.GetSize().y self.cfg.Write("width", str(self.w)); self.cfg.Write("height", str(self.h)); self.Refresh() def OnMove(self, event): self.x = event.GetPosition().x self.y = event.GetPosition().y self.cfg.Write("left", str(self.x)); self.cfg.Write("top", str(self.y)); def StatusBox(self, dc, nr, txt, percent, color): if percent < 0: percent = 0 if percent > 1: percent = 1 boxw = self.stat tdx = int(boxw * 10.0 / 300.0) tdy = int(boxw * 6.0 / 300.0) boxh = int(boxw * 40.0 / 300.0) boxw = self.stat - 2*tdx spacing = boxh+10 dc.SetPen(wx.Pen(wx.Colour(0,0,0))) dc.SetBrush(wx.Brush(wx.Colour(220,220,220))) dc.DrawRectangle(tdx, int(nr*spacing+tdx), int(boxw), boxh) dc.SetTextForeground(wx.Colour(0, 0, 0)) if color < 0.2: dc.SetTextForeground(wx.Colour(255, 255, 255)) dc.SetBrush(wx.Brush(wx.Colour(250,0,0))) elif color < 0.6: dc.SetBrush(wx.Brush(wx.Colour(250,180,0))) else: dc.SetBrush(wx.Brush(wx.Colour(0,250,0))) # dc.DrawLine(200,50,350,50) dc.DrawRectangle(tdx, int(nr*spacing+tdx), int(boxw * percent), boxh) dc.DrawText(txt,18,int(nr*spacing+tdy+tdx)) def plot_x(self, x): return int(self.stat+self.tdx + x * (self.w-self.stat-2*self.tdx)) def plot_y(self, y): return int(self.tdx + (1-y) * (self.h-self.tdx-self.tdx)) def plot(self, dc, i1, i2): dc.DrawLine(self.plot_x(i1[1]/3500), self.plot_y((i1[0]-2.5)/(4.2-2.5)), self.plot_x(i2[1]/3500), self.plot_y((i2[0]-2.5)/(4.2-2.5))) def DischargePlot(self, dc): self.tdx = int(self.stat * 10.0 / 300.0) dc.SetPen(wx.Pen(wx.Colour(0,0,0),1)) dc.SetBrush(wx.Brush(wx.Colour(250,250,250))) dc.DrawRectangle(self.plot_x(0.0), self.plot_y(1.0), self.w-self.stat-2*self.tdx, self.h-2*self.tdx) for i in range(0,6): dc.DrawLine(self.plot_x(0.0), self.plot_y(i/6.0), self.plot_x(1.0), self.plot_y(i/6.0)) for i in range(0,7): dc.DrawLine(self.plot_x(i/7.0), self.plot_y(0), self.plot_x(i/7.0), self.plot_y(1)) dc.SetPen(wx.Pen(wx.Colour(0,0,0),4)) dc.DrawLine(self.plot_x(self.cell.model/3500), self.plot_y(0), self.plot_x(self.cell.model/3500), self.plot_y(1)) dc.DrawLine(self.plot_x(0.0), self.plot_y(self.cell.get_volt_perc()), self.plot_x(1.0), self.plot_y(self.cell.get_volt_perc())) # Draw maximum charge point dc.SetPen(wx.Pen(wx.Colour(0,0,255),2)) dc.DrawLine(self.plot_x(self.energy_prediction.get_max_hover_charge() / 3500), self.plot_y(0), self.plot_x(self.energy_prediction.get_max_hover_charge() / 3500), self.plot_y(1)) dc.DrawLine(self.plot_x(1415. / 3500), self.plot_y(0), self.plot_x(1415. / 3500), self.plot_y(1)) # Competition latest land at joe (18km/h0degwind, 25m/s) font = wx.Font(8, wx.ROMAN, wx.NORMAL, wx.NORMAL) dc.SetFont(font) for i in range(0,3500,500): dc.DrawText(str(i) + "mAh", self.plot_x(float(i)/3500.0), self.plot_y(1.0)) for i in range(25,43,3): dc.DrawText(str(round(i/10.0,1)) + "V", self.plot_x(0), self.plot_y(self.cell.get_volt_percent(float(i/10.0)))) thickness = 3 dc.SetPen(wx.Pen(wx.Colour(0,0,0),thickness)) li = bat.batmodel[0,[0,1]] for i in bat.batmodel[:,[0,1]]: self.plot(dc,li,i) li=i dc.SetPen(wx.Pen(wx.Colour(0,0,255),thickness)) li = bat.batmodel[0,[0,2]] for i in bat.batmodel[:,[0,2]]: self.plot(dc,li,i) li=i dc.SetPen(wx.Pen(wx.Colour(0,255,0),thickness)) li = bat.batmodel[0,[0,3]] for i in bat.batmodel[:,[0,3]]: self.plot(dc,li,i) li=i dc.SetPen(wx.Pen(wx.Colour(255,255,0),thickness)) li = bat.batmodel[0,[0,4]] for i in bat.batmodel[:,[0,4]]: self.plot(dc,li,i) li=i dc.SetPen(wx.Pen(wx.Colour(255,0,0),thickness)) li = bat.batmodel[0,[0,5]] for i in bat.batmodel[:,[0,5]]: self.plot(dc,li,i) li=i def OnPaint(self, e): # Automatic Scaling w = self.w h = self.h self.stat = int(w/4) if self.stat<100: self.stat=100 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)) fontscale = int(w * 11.0 / 800.0) if fontscale < 6: fontscale = 6 font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD) dc.SetFont(font) self.StatusBox(dc,0, self.cell.get_volt(), self.cell.get_volt_perc(), self.cell.get_volt_color()) self.StatusBox(dc,1, self.cell.get_current(), self.cell.get_current_perc(), self.cell.get_current_color() ) self.StatusBox(dc,2, self.cell.get_energy(), self.cell.get_energy_perc(), self.cell.get_energy_color() ) self.StatusBox(dc,3, self.cell.get_mah_from_volt(), self.cell.get_energy_perc(), self.cell.get_energy_color() ) self.StatusBox(dc,4, self.cell.get_temp(), self.cell.get_temp_perc(), self.cell.get_temp_color()) self.StatusBox(dc,6, self.cell.get_power_text(), self.cell.get_power_perc(), self.cell.get_power_color()) self.StatusBox(dc,7, self.energy_prediction.get_power_fraction_text(), self.energy_prediction.get_power_fraction(), self.energy_prediction.get_power_fraction_color()) self.StatusBox(dc,8, self.energy_prediction.get_hover_seconds_left_text(), self.energy_prediction.get_hover_seconds_fraction(), self.energy_prediction.get_hover_seconds_color()) self.StatusBox(dc,9, self.energy_prediction.get_fw_seconds_left_text(), self.energy_prediction.get_fw_seconds_fraction(), self.energy_prediction.get_fw_seconds_color()) self.StatusBox(dc,10, self.energy_prediction.get_fw_seconds_left_20mps_text(), self.energy_prediction.get_fw_seconds_left_20mps_fraction(), self.energy_prediction.get_fw_seconds_left_20mps_color()) self.DischargePlot(dc) def __init__(self): self.w = WIDTH self.h = WIDTH + BARH self.cfg = wx.Config('energymon_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) wx.Frame.__init__(self, id=-1, parent=None, name=u'EnergyMonFrame', size=wx.Size(self.w, self.h), title=u'Energy Monitoring') if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x,self.y), wx.SIZE_USE_EXISTING) self.Bind(wx.EVT_PAINT, self.OnPaint) self.Bind(wx.EVT_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) self.Bind(wx.EVT_CLOSE, self.OnClose) ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/energy_mon/energy_mon.ico", wx.BITMAP_TYPE_ICO) self.SetIcon(ico) self.bat = {} self.temp = {} self.cell = BatteryCell() self.energy_prediction = EnergyPrediction(self.cell) self.interface = IvyMessagesInterface("energymonframe") self.interface.subscribe(self.message_recv) def OnClose(self, event): self.interface.shutdown() self.Destroy()
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()
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'] # try classic name except Exception as e: try: self.ap_mode = settings.name_lookup[ 'ap'] # in case it is a generated autopilot 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.index try: msg['value'] = self.ap_mode.ValueFromName( 'Guided') # AP_MODE_GUIDED except ValueError: try: msg['value'] = self.ap_mode.ValueFromName( 'GUIDED') # AP_MODE_GUIDED except ValueError: msg['value'] = 19 # fallback to fixed index 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.index try: msg['value'] = self.ap_mode.ValueFromName('Nav') # AP_MODE_NAV except ValueError: try: msg['value'] = self.ap_mode.ValueFromName( 'NAV') # AP_MODE_NAV except ValueError: msg['value'] = 13 # fallback to fixed index 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)
class MessageInterface: """ MessageInterface This is an abstraction layer used to simplify the use of paparazzi message interface, especially for time measurement and casting of message payload data (which sometimes stays in ascii) """ def prettify_message(msg): """ Sometimes IvyMessageInterface does not cast data to their binary types. This function cast all fields to their binary types. (supposed to be done by PprzMessage.payload_to_binay but does not seem to be working) It also measure reception time. """ timestamp = time.time() fieldValues = [] for fieldValue, fieldType in zip(msg.fieldvalues, msg.fieldtypes): if "int" in fieldType: castType = int elif "float" in fieldType: castType = float elif "string" in fieldType: castType = str elif "char" in fieldType: castType = int else: # Could not indentify type, leave field as is fieldValues.append(fieldValue) # Checking if is a list if '[' in fieldType: fieldValues.append([castType(value) for value in fieldValue]) else: fieldValues.append(castType(fieldValue)) msg.set_values(fieldValues) msg.timestamp = timestamp return msg def parse_pprz_msg(msg): """ Alias to IvyMessageInterface.parse_pprz_msg, but with prettify_message called at the end to ensure all data are in binary format. """ class Catcher: """ This is a type specifically to catch result from IvyMessageInterface.parse_pprz_msg which only outputs result via a callback. """ def set_message(self, aircraftId, message): self.message = message self.aircraftId = str(aircraftId) catcher = Catcher() IvyMessagesInterface.parse_pprz_msg(catcher.set_message, msg) return [MessageInterface.prettify_message(catcher.message), catcher.aircraftId] def __init__(self, ivyBusAddress=None): if ivyBusAddress is None: ivyBusAddress = os.getenv('IVY_BUS') if ivyBusAddress is None: ivyBusAddress == "" self.messageInterface = IvyMessagesInterface(ivy_bus=ivyBusAddress) def bind(self, callback, ivyRegex): return self.messageInterface.subscribe( lambda sender, msg: callback(MessageInterface.prettify_message(msg)), ivyRegex) def bind_raw(self, callback, ivyRegex): return self.messageInterface.bind_raw(callback, ivyRegex) def unbind(self, bindId): self.messageInterface.unbind(bindId) def send(self, msg): return self.messageInterface.send(msg)
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()
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.ROMAN, wx.BOLD, wx.NORMAL) 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()
class AtcFrame(wx.Frame): def message_recv(self, ac_id, msg): self.callsign = "ID=" + str(ac_id) 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 OnSize(self, event): self.w = event.GetSize().x self.h = event.GetSize().y self.cfg.Write("width", str(self.w)) self.cfg.Write("height", str(self.h)) self.Refresh() def OnMove(self, event): self.x = event.GetPosition().x self.y = event.GetPosition().y self.cfg.Write("left", str(self.x)) self.cfg.Write("top", str(self.y)) def OnPaint(self, e): w = self.w h = self.h if (float(w) / float(h)) > (WIDTH / HEIGHT): w = int(h * WIDTH / HEIGHT) else: h = int(w * HEIGHT / WIDTH) tdy = int(w * 75.0 / WIDTH) tdx = int(w * 15.0 / WIDTH) dc = wx.PaintDC(self) fontscale = int(w * 40.0 / WIDTH) if fontscale < 6: fontscale = 6 # Background dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT)) #dc.DrawCircle(w/2,w/2,w/2-1) font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD) dc.SetFont(font) dc.DrawText("Callsign: " + str(self.callsign) + " ", tdx, tdx + tdy * 0) dc.DrawText("Airspeed: " + str(self.airspeed) + " kt", tdx, tdx + tdy * 1) dc.DrawText("Ground Speed: " + str(self.gspeed) + " kt", tdx, tdx + tdy * 2) dc.DrawText("AMSL: " + str(self.amsl) + " ft (<2700ft)", tdx, tdx + tdy * 3) dc.DrawText("AGL: " + str(self.alt) + " ft (<1500ft)", tdx, tdx + tdy * 4) dc.DrawText( "QNH: " + str(self.qnh * 100.0) + " QFE: " + str(self.qfe) + "", tdx, tdx + tdy * 5) def __init__(self): self.w = WIDTH self.h = HEIGHT self.airspeed = 0 self.amsl = 0 self.qnh = 0 self.qfe = 0 self.alt = 0 #self.hmsl = 0; self.gspeed = 0 self.callsign = "" self.safe_to_approach = "" self.cfg = wx.Config('atc_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) 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_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) 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) if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x, self.y), wx.SIZE_USE_EXISTING) def OnClose(self, event): self.interface.shutdown() self.Destroy()
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)
class MessagesFrame(wx.Frame): def message_recv(self, ac_id, msg): """Handle incoming messages Callback function for IvyMessagesInterface :param ac_id: aircraft id :type ac_id: int :param msg: message :type msg: PprzMessage """ # only show messages of the requested class if msg.msg_class != self.msg_class: return if ac_id in self.aircrafts and msg.name in self.aircrafts[ ac_id].messages: if time.time() - self.aircrafts[ac_id].messages[ msg.name].last_seen < 0.2: return wx.CallAfter(self.gui_update, ac_id, msg) def find_page(self, book, name): if book.GetPageCount() < 1: return 0 start = 0 end = book.GetPageCount() while start < end: if book.GetPageText(start) >= name: return start start += 1 return start def update_leds(self): wx.CallAfter(self.update_leds_real) def update_leds_real(self): for ac_id in self.aircrafts: aircraft = self.aircrafts[ac_id] for msg_str in aircraft.messages: message = aircraft.messages[msg_str] if message.last_seen + 0.2 < time.time(): aircraft.messages_book.SetPageImage(message.index, 0) self.timer = threading.Timer(0.1, self.update_leds) self.timer.start() def setup_image_list(self, notebook): imageList = wx.ImageList(24, 24) image = wx.Image(PPRZ_HOME + "/data/pictures/gray_led24.png") bitmap = wx.BitmapFromImage(image) imageList.Add(bitmap) image = wx.Image(PPRZ_HOME + "/data/pictures/green_led24.png") bitmap = wx.BitmapFromImage(image) imageList.Add(bitmap) notebook.AssignImageList(imageList) def add_new_aircraft(self, ac_id): self.aircrafts[ac_id] = Aircraft(ac_id) ac_panel = wx.Panel(self.notebook, -1) self.notebook.AddPage(ac_panel, str(ac_id)) messages_book = wx.Notebook(ac_panel, style=wx.NB_LEFT) self.setup_image_list(messages_book) sizer = wx.BoxSizer(wx.VERTICAL) sizer.Add(messages_book, 1, wx.EXPAND) ac_panel.SetSizer(sizer) sizer.Layout() self.aircrafts[ac_id].messages_book = messages_book def add_new_message(self, aircraft, msg_class, name): messages_book = aircraft.messages_book aircraft.messages[name] = Message(msg_class, name) field_panel = wx.Panel(messages_book) grid_sizer = wx.FlexGridSizer(len(aircraft.messages[name].fieldnames), 2) index = self.find_page(messages_book, name) messages_book.InsertPage(index, field_panel, name, imageId=1) aircraft.messages[name].index = index # update indexes of pages which are to be moved for message_name in aircraft.messages: aircraft.messages[message_name].index = self.find_page( messages_book, message_name) for field_name in aircraft.messages[name].fieldnames: name_text = wx.StaticText(field_panel, -1, field_name) size = name_text.GetSize() size.x = LABEL_WIDTH name_text.SetMinSize(size) grid_sizer.Add(name_text, 1, wx.ALL, BORDER) value_control = wx.StaticText(field_panel, -1, "42", style=wx.EXPAND) size = value_control.GetSize() size.x = LABEL_WIDTH value_control.SetMinSize(size) grid_sizer.Add(value_control, 1, wx.ALL | wx.EXPAND, BORDER) if wx.MAJOR_VERSION > 2: if grid_sizer.IsColGrowable(1): grid_sizer.AddGrowableCol(1) else: grid_sizer.AddGrowableCol(1) aircraft.messages[name].field_controls.append(value_control) field_panel.SetAutoLayout(True) field_panel.SetSizer(grid_sizer) field_panel.Layout() def gui_update(self, ac_id, msg): if ac_id not in self.aircrafts: self.add_new_aircraft(ac_id) aircraft = self.aircrafts[ac_id] if msg.name not in aircraft.messages: self.add_new_message(aircraft, msg.msg_class, msg.name) aircraft.messages_book.SetPageImage(aircraft.messages[msg.name].index, 1) self.aircrafts[ac_id].messages[msg.name].last_seen = time.time() for index in range(0, len(msg.fieldvalues)): aircraft.messages[msg.name].field_controls[index].SetLabel( str(msg.get_field(index))) def __init__(self, msg_class="telemetry"): wx.Frame.__init__(self, id=-1, parent=None, name=u'MessagesFrame', size=wx.Size(WIDTH, HEIGHT), style=wx.DEFAULT_FRAME_STYLE, title=u'Messages') self.Bind(wx.EVT_CLOSE, self.OnClose) self.notebook = wx.Notebook(self) self.aircrafts = {} sizer = wx.BoxSizer(wx.HORIZONTAL) sizer.Add(self.notebook, 1, wx.EXPAND) self.SetSizer(sizer) sizer.Layout() self.timer = threading.Timer(0.1, self.update_leds) self.timer.start() self.msg_class = msg_class self.interface = IvyMessagesInterface("Paparazzi Messages Viewer") self.interface.subscribe(self.message_recv) def OnClose(self, event): self.timer.cancel() self.interface.shutdown() self.Destroy()
parser.add_argument("-c","--curl", action="store_true", help="dump actions as curl commands") parser.add_argument("-s","--subscribe", action="store_true", help="subscribe to the ivy bus") parser.add_argument("-v","--verbose", action="store_true", help="verbose mode") try: # --- Startup state initialization block args = parser.parse_args() static_init_configuration_data() static_init_client_configuration_data(args.file) if args.verbose: print_aircraft_data() if args.generate: template_configuration() sys.exit(0) if args.subscribe: ivy_interface.subscribe(callback_aircraft_messages) ivy_interface.start() # Handle misc. command line arguments if args.verbose: verbose=args.verbose if args.curl: curl=args.curl print_curl_header(args.ip, args.port) # --- Main loop # Supply flask the appropriate ip address and port for the server server_host = args.ip # Store for use in htlm template generation server_port = args.port # Store for use in htlm template generation app.run(host=args.ip,port=args.port,threaded=True)
class RadioBridge: def __init__(self, link_uri, msg_class='telemetry', verbose=False): """ Initialize and run with the specified link_uri """ self.verbose = verbose # Ivy interface and stream parser self._ivy = IvyMessagesInterface("cf2ivy") self._transport = PprzTransport(msg_class) # Create a Crazyradio self._driver = RadioDriver() self._driver.connect(link_uri, self._link_quality_cb, self._link_error_cb) if self.verbose: print('Connecting to %s' % link_uri) # Variable used to keep main loop occupied until disconnect self.is_connected = True # Bind to all messages from ac_id def _forward_to_cf(ac_id, msg): try: data = self._transport.pack_pprz_msg(0, msg) # sender_id 0 = GCS for i in range(0, len(data), 30): pk = CRTPPacket() pk.port = CRTP_PORT_PPRZLINK pk.data = data[i:(i + 30)] self._driver.send_packet(pk) if self.verbose: print('Forward message', msg.name) except: if self.verbose: print('Forward error for', ac_id) messages_datalink = messages_xml_map.get_msgs("datalink") for msg in messages_datalink: self._ivy.subscribe(_forward_to_cf, PprzMessage("datalink", msg)) def shutdown(self): if self.verbose: print('closing cf2ivy interfaces') self._ivy.shutdown() self._driver.close() def run(self): pk = self._driver.receive_packet( 0.1) # wait for next message with timeout if pk is not None: self._got_packet(pk) def _got_packet(self, pk): if pk.port == CRTP_PORT_PPRZLINK: for c in pk.data: if self._transport.parse_byte(bytes([c])): (sender_id, _, _, msg) = self._transport.unpack() if self.verbose: print("Got message {} from {}".format( msg.name, sender_id)) # Forward message to Ivy bus if self.is_connected: self._ivy.send(msg, sender_id=sender_id) def _link_quality_cb(self, quality): pass def _link_error_cb(self, msg): if self.verbose: print("Link error: {}".format(msg))
class AtcFrame(wx.Frame): def message_recv(self, ac_id, msg): self.callsign = "ID=" + str(ac_id) 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 OnSize(self, event): self.w = event.GetSize().x self.h = event.GetSize().y self.cfg.Write("width", str(self.w)); self.cfg.Write("height", str(self.h)); self.Refresh() def OnMove(self, event): self.x = event.GetPosition().x self.y = event.GetPosition().y self.cfg.Write("left", str(self.x)); self.cfg.Write("top", str(self.y)); def OnPaint(self, e): w = self.w h = self.h if (float(w)/float(h)) > (WIDTH/HEIGHT): w = int(h * WIDTH/HEIGHT) else: h = int(w * HEIGHT/WIDTH) tdy = int(w * 75.0 / WIDTH) tdx = int(w * 15.0 / WIDTH) dc = wx.PaintDC(self) fontscale = int(w * 40.0 / WIDTH) if fontscale < 6: fontscale = 6 # Background dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT)) #dc.DrawCircle(w/2,w/2,w/2-1) font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD) dc.SetFont(font) dc.DrawText("Callsign: " + str(self.callsign) + " ",tdx,tdx+tdy*0) dc.DrawText("Airspeed: " + str(self.airspeed) + " kt",tdx,tdx+tdy*1) dc.DrawText("Ground Speed: " + str(self.gspeed) + " kt",tdx,tdx+tdy*2) dc.DrawText("AMSL: " + str(self.amsl) + " ft (<2700ft)",tdx,tdx+tdy*3) dc.DrawText("AGL: " + str(self.alt) + " ft (<1500ft)",tdx,tdx+tdy*4) dc.DrawText("QNH: " + str(self.qnh*100.0) + " QFE: " + str(self.qfe) + "",tdx,tdx+tdy*5) def __init__(self): self.w = WIDTH self.h = HEIGHT self.airspeed = 0; self.amsl = 0; self.qnh = 0; self.qfe = 0; self.alt = 0; #self.hmsl = 0; self.gspeed = 0; self.callsign = "" self.safe_to_approach = ""; self.cfg = wx.Config('atc_conf') if self.cfg.Exists('width'): self.w = int(self.cfg.Read('width')) self.h = int(self.cfg.Read('height')) 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_SIZE, self.OnSize) self.Bind(wx.EVT_MOVE, self.OnMove) 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) if self.cfg.Exists('left'): self.x = int(self.cfg.Read('left')) self.y = int(self.cfg.Read('top')) self.SetPosition(wx.Point(self.x,self.y), wx.SIZE_USE_EXISTING) def OnClose(self, event): self.interface.shutdown() self.Destroy()