Example #1
0
  def __init__(self, main, instance_name=None):
    spooky.modules.SpookyModule.__init__(self, main, "solo_sbp", singleton=True)
    self.local_bind_ip  = self.main.config.get_my('my-ip')
    self.sololink_bind_ip  = self.main.config.get_my('sololink-my-ip')
    self.solo_sbp_port = self.main.config.get_my('solo-sbp-port')

    self.relay_recv_port = self.main.config.get_my('solo-sbp-relay-recv-port')
    self.relay_send_port = self.main.config.get_my('solo-sbp-relay-send-port')

    self.last_update = 0
    self.msg_cache = SbpMsgCache()

    self._sendToPiksi   = Queue.Queue()
Example #2
0
 def __init__(self, main, instance_name=None):
     spooky.modules.SpookyModule.__init__(self,
                                          main,
                                          "odroidperson_sbp",
                                          instance_name=instance_name)
     self.bind_ip = self.main.config.get_my('my-ip')
     self.sbp_port = self.main.config.get_foreign(instance_name,
                                                  'sbp-server-port')
     self.relay_send_port = self.main.config.get_foreign(
         instance_name, 'sbp-relay-send-port')
     self.relay_recv_port = self.main.config.get_foreign(
         instance_name, 'sbp-relay-recv-port')
     self.last_update = 0
     self.msg_cache = SbpMsgCache()
 def __init__(self, main, instance_name=None):
   spooky.modules.SpookyModule.__init__(self, main, "odroidperson_sbp", instance_name=instance_name)
   self.bind_ip  = self.main.config.get_my('my-ip')
   self.sbp_port = self.main.config.get_foreign(instance_name, 'sbp-server-port')
   self.relay_send_port = self.main.config.get_foreign(instance_name, 'sbp-relay-send-port')
   self.relay_recv_port = self.main.config.get_foreign(instance_name, 'sbp-relay-recv-port')
   self.last_update = 0
   self.msg_cache = SbpMsgCache()
class OdroidPersonSBPModule(spooky.modules.SpookyModule):
  '''
  This is the Swift Binary Protocol receiver of a remote OdroidPerson.
  '''

  def __init__(self, main, instance_name=None):
    spooky.modules.SpookyModule.__init__(self, main, "odroidperson_sbp", instance_name=instance_name)
    self.bind_ip  = self.main.config.get_my('my-ip')
    self.sbp_port = self.main.config.get_foreign(instance_name, 'sbp-server-port')
    self.relay_send_port = self.main.config.get_foreign(instance_name, 'sbp-relay-send-port')
    self.relay_recv_port = self.main.config.get_foreign(instance_name, 'sbp-relay-recv-port')
    self.last_update = 0
    self.msg_cache = SbpMsgCache()

  def cmd_status(self):
    if self.last_update == 0:
      print self, "never received message."
    else:
      print self, "last received message %.2fs ago" % (time.time() - self.last_update)

  def handle_incoming(self, msg, **metadata):
    '''
    Callback for handling incoming SBP messages
    '''
    self.last_update = time.time()
    msg_instance = msg.__class__.__name__

    maybe_batch = self.msg_cache.handle_new_message(msg)

    if maybe_batch:
      update = [(msg.__class__.__name__, spooky.swift.fmt_dict(msg)) for msg in maybe_batch]
      self.main.modules.trigger('update_partial_state', self.instance_name, update)

  def handle_relay(self, msg, **metadata):
    self.relay_udp.sendto(msg.to_binary(), (self.bind_ip, self.relay_send_port))

  def run(self):
    '''Thread loop here'''
    try:
      with SBPUDPDriver(self.bind_ip, self.sbp_port) as driver:
        with Handler(Framer(driver.read, None, verbose=True)) as source:

          with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as relay_udp:
            relay_udp.setblocking(1)
            relay_udp.settimeout(0.05)
            relay_udp.bind((self.bind_ip, self.relay_recv_port))
            self.relay_udp = relay_udp

            print "Module %s listening on %s : %s and relaying to %s (send: %d, recv: %d)" % (self, self.bind_ip, self.sbp_port, self.bind_ip, self.relay_send_port, self.relay_recv_port)

            source.add_callback(self.handle_relay)
            source.add_callback(self.handle_incoming, 
              msg_type=[SBP_MSG_POS_LLH, SBP_MSG_GPS_TIME, SBP_MSG_DOPS, SBP_MSG_BASELINE_NED, SBP_MSG_VEL_NED, SBP_MSG_BASELINE_HEADING, SBP_MSG_IAR_STATE])

            self.ready()

            while not self.stopped():

              try:
                data, addr = relay_udp.recvfrom(4096)
                driver.write(data)
              except socket.timeout:
                pass
              except socket.error:
                traceback.print_exc()
    except:
      traceback.print_exc()
      print "FUUU"
Example #5
0
class SoloSBPModule(spooky.modules.SpookyModule):
  '''
  This is the Swift Binary Protocol receiver of a remote Solo. 

  







  TODO: It's almost identical to the odroidperson_sbp
  The only difference is a few parameters. Seriously, unify the two!
  
  






  '''

  def __init__(self, main, instance_name=None):
    spooky.modules.SpookyModule.__init__(self, main, "solo_sbp", singleton=True)
    self.local_bind_ip  = self.main.config.get_my('my-ip')
    self.sololink_bind_ip  = self.main.config.get_my('sololink-my-ip')
    self.solo_sbp_port = self.main.config.get_my('solo-sbp-port')

    self.relay_recv_port = self.main.config.get_my('solo-sbp-relay-recv-port')
    self.relay_send_port = self.main.config.get_my('solo-sbp-relay-send-port')

    self.last_update = 0
    self.msg_cache = SbpMsgCache()

    self._sendToPiksi   = Queue.Queue()
  
  def cmd_status(self):
    if self.last_update == 0:
      print self, "never received message."
    else:
      print self, "last received message %.2fs ago" % (time.time() - self.last_update)

  def handle_incoming(self, msg, **metadata):
    '''
    Callback for handling incoming SBP messages
    '''
    self.last_update = time.time()
    msg_instance = msg.__class__.__name__

    maybe_batch = self.msg_cache.handle_new_message(msg)

    if maybe_batch:
      update = [(msg.__class__.__name__, spooky.swift.fmt_dict(msg)) for msg in maybe_batch]
      self.main.modules.trigger('update_partial_state', "solo_sbp", update)

  def handle_relay(self, msg, **metadata):
    self.relay_udp.sendto(msg.to_binary(), (self.local_bind_ip, self.relay_send_port))

  def disable_piksi_sim(self):
    print 'DISABLING SIM'
    section = "simulator"
    name    = "enabled"
    value   = "False"
    msg = MsgSettingsWrite(setting='%s\0%s\0%s\0' % (section, name, value))
    self.send_to_piksi(msg.to_binary())

  def enable_piksi_sim(self):
    print 'ENABLING SIM'
    section = "simulator"
    name    = "enabled"
    value   = "True"
    msg = MsgSettingsWrite(setting='%s\0%s\0%s\0' % (section, name, value))
    self.send_to_piksi(msg.to_binary())

  def reset_piksi(self):
    print 'RESET PIKSI'
    msg = MsgReset()
    self.send_to_piksi(msg.to_binary())

  def send_to_piksi(self, data):
    '''
    Call from an external thread to enqueue data
    for this thread to upload to Piksi.

    This might block!
    '''
    self._sendToPiksi.put(data, True)

  def run(self):
    '''Thread loop here'''
    try:
      with SBPUDPDriver(self.sololink_bind_ip, self.solo_sbp_port) as driver:
        with Handler(Framer(driver.read, None, verbose=True)) as source:

          with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as relay_udp:
            relay_udp.setblocking(1)
            relay_udp.settimeout(0.05)
            relay_udp.bind((self.local_bind_ip, self.relay_recv_port))
            self.relay_udp = relay_udp

            print "Module %s listening on %s : %s and relaying to %s (send: %d, recv: %d)" % (self, self.sololink_bind_ip, self.solo_sbp_port, self.local_bind_ip, self.relay_send_port, self.relay_recv_port)

            source.add_callback(self.handle_relay)
            source.add_callback(self.handle_incoming, 
              msg_type=[SBP_MSG_POS_LLH, SBP_MSG_GPS_TIME, SBP_MSG_DOPS, SBP_MSG_BASELINE_NED, SBP_MSG_VEL_NED, SBP_MSG_BASELINE_HEADING, SBP_MSG_IAR_STATE])

            self.ready()

            while not self.stopped():

              # Uploading data TO Piksi
              try:
                while not self._sendToPiksi.empty():
                  data = self._sendToPiksi.get(False)
                  driver.write(data)
              except (Queue.Empty, serial.SerialException):
                pass

              try:
                data, addr = relay_udp.recvfrom(4096)
                driver.write(data)
              except socket.timeout:
                pass
              except socket.error:
                traceback.print_exc()
    except:
      traceback.print_exc()
      print "SoloLink SBP FUUUU"
Example #6
0
class OdroidPersonSBPModule(spooky.modules.SpookyModule):
    '''
  This is the Swift Binary Protocol receiver of a remote OdroidPerson.
  '''
    def __init__(self, main, instance_name=None):
        spooky.modules.SpookyModule.__init__(self,
                                             main,
                                             "odroidperson_sbp",
                                             instance_name=instance_name)
        self.bind_ip = self.main.config.get_my('my-ip')
        self.sbp_port = self.main.config.get_foreign(instance_name,
                                                     'sbp-server-port')
        self.relay_send_port = self.main.config.get_foreign(
            instance_name, 'sbp-relay-send-port')
        self.relay_recv_port = self.main.config.get_foreign(
            instance_name, 'sbp-relay-recv-port')
        self.last_update = 0
        self.msg_cache = SbpMsgCache()

    def cmd_status(self):
        if self.last_update == 0:
            print self, "never received message."
        else:
            print self, "last received message %.2fs ago" % (time.time() -
                                                             self.last_update)

    def handle_incoming(self, msg, **metadata):
        '''
    Callback for handling incoming SBP messages
    '''
        self.last_update = time.time()
        msg_instance = msg.__class__.__name__

        maybe_batch = self.msg_cache.handle_new_message(msg)

        if maybe_batch:
            update = [(msg.__class__.__name__, spooky.swift.fmt_dict(msg))
                      for msg in maybe_batch]
            self.main.modules.trigger('update_partial_state',
                                      self.instance_name, update)

    def handle_relay(self, msg, **metadata):
        self.relay_udp.sendto(msg.to_binary(),
                              (self.bind_ip, self.relay_send_port))

    def run(self):
        '''Thread loop here'''
        try:
            with SBPUDPDriver(self.bind_ip, self.sbp_port) as driver:
                with Handler(Framer(driver.read, None,
                                    verbose=True)) as source:

                    with closing(
                            socket.socket(socket.AF_INET,
                                          socket.SOCK_DGRAM)) as relay_udp:
                        relay_udp.setblocking(1)
                        relay_udp.settimeout(0.05)
                        relay_udp.bind((self.bind_ip, self.relay_recv_port))
                        self.relay_udp = relay_udp

                        print "Module %s listening on %s : %s and relaying to %s (send: %d, recv: %d)" % (
                            self, self.bind_ip, self.sbp_port, self.bind_ip,
                            self.relay_send_port, self.relay_recv_port)

                        source.add_callback(self.handle_relay)
                        source.add_callback(self.handle_incoming,
                                            msg_type=[
                                                SBP_MSG_POS_LLH,
                                                SBP_MSG_GPS_TIME, SBP_MSG_DOPS,
                                                SBP_MSG_BASELINE_NED,
                                                SBP_MSG_VEL_NED,
                                                SBP_MSG_BASELINE_HEADING,
                                                SBP_MSG_IAR_STATE
                                            ])

                        self.ready()

                        while not self.stopped():

                            try:
                                data, addr = relay_udp.recvfrom(4096)
                                driver.write(data)
                            except socket.timeout:
                                pass
                            except socket.error:
                                traceback.print_exc()
        except:
            traceback.print_exc()
            print "FUUU"