Beispiel #1
0
def main(args):
  """
  Get configuration, get driver, get logger, and build handler and start it.
  """
  port = args.port
  baud = args.baud
  timeout = args.timeout
  log_filename = args.log_filename
  append_log_filename = args.append_log_filename
  tags = args.tags
  # State for handling a networked base stations.
  channel = args.channel_id
  serial_id = int(args.serial_id) if args.serial_id is not None else None
  base = args.base
  use_broker = args.broker
  # Driver with context
  with get_driver(args.ftdi, port, baud, args.file) as driver:
    # Handler with context
    with Handler(Framer(driver.read, driver.write, args.verbose)) as link:
      # Logger with context
      with get_logger(args.log, log_filename) as logger:
        with get_append_logger(append_log_filename, tags) as append_logger:
          link.add_callback(printer, SBP_MSG_PRINT_DEP)
          link.add_callback(log_printer, SBP_MSG_LOG)
          Forwarder(link, logger).start()
          Forwarder(link, append_logger).start()
          if use_broker and base and serial_id:
            device_id = get_uuid(channel, serial_id)
            with HTTPDriver(str(device_id), base) as http:
              with Handler(Framer(http.read, http.write, args.verbose)) as slink:
                Forwarder(slink, swriter(link)).start()
                run(args, link)
          else:
            run(args, link)
Beispiel #2
0
    def connect_piksi(self):
        self.piksi_driver = TCPDriver(self.piksi_host, self.piksi_port)
        #self.piksi_driver = PySerialDriver(self.piksi_port, baud=1000000)
        self.piksi_framer = Framer(self.piksi_driver.read,
                                   self.piksi_driver.write,
                                   verbose=False)
        self.piksi = Handler(self.piksi_framer)

        # Only setup log and settings messages
        # We will wait to set up rest until piksi is configured
        self.piksi.add_callback(self.callback_sbp_heartbeat,
                                msg_type=SBP_MSG_HEARTBEAT)
        self.piksi.add_callback(self.callback_sbp_log, msg_type=SBP_MSG_LOG)
        self.piksi.add_callback(self.callback_sbp_settings_read_resp,
                                msg_type=SBP_MSG_SETTINGS_READ_RESP)
        self.piksi.add_callback(self.callback_sbp_settings_read_by_index_resp,
                                msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_REQ)
        self.piksi.add_callback(self.callback_sbp_settings_read_by_index_resp,
                                msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
        self.piksi.add_callback(self.callback_sbp_settings_read_by_index_done,
                                msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_DONE)

        self.piksi.add_callback(self.callback_sbp_startup, SBP_MSG_STARTUP)

        self.piksi.start()
Beispiel #3
0
 def __init__(self, port, baud_rate, callback):
     self._callback = callback
     self.driver = PySerialDriver(port, baud=baud_rate)
     self.framer = Framer(self.driver.read, None, verbose=False)
     self.piksi = Handler(self.framer)
     self.piksi.add_callback(self.callback)
     self.piksi.start()
Beispiel #4
0
 def __init__(self, port, ext_callback):
     self._callback = ext_callback
     self.driver = UDPDriver(' ', port)
     self.framer = Framer(self.driver.read, None, verbose=False)
     self.piksi = Handler(self.framer)
     self.piksi.add_callback(self.recv_callback)
     self.piksi.start()
Beispiel #5
0
def main(args):
    """
    Get configuration, get driver, get logger, and build handler and start it.
    """
    timeout = args.timeout
    log_filename = args.logfilename
    log_dirname = args.log_dirname
    if not log_filename:
        log_filename = logfilename()
    if log_dirname:
        log_filename = os.path.join(log_dirname, log_filename)
    # State for handling a networked base stations.
    channel = args.channel_id
    serial_id = int(args.serial_id) if args.serial_id is not None else None
    base = args.base
    use_broker = args.broker
    # Driver with context
    driver = get_base_args_driver(args)
    with Handler(Framer(driver.read, driver.write, args.verbose)) as link:
        # Logger with context
        with get_logger(args.log, log_filename, args.expand_json) as logger:
            link.add_callback(printer, SBP_MSG_PRINT_DEP)
            link.add_callback(log_printer, SBP_MSG_LOG)
            Forwarder(link, logger).start()
            if use_broker and base and serial_id:
                device_id = get_uuid(channel, serial_id)
                with HTTPDriver(str(device_id), base) as http:
                    with Handler(Framer(http.read, http.write,
                                        args.verbose)) as slink:
                        Forwarder(slink, swriter(link)).start()
                        run(args, link)
            else:
                run(args, link)
Beispiel #6
0
 def __init__(self, host, port, callback):
     self._callback = callback
     self.driver = UDPDriver(host, port)
     self.framer = Framer(self.driver.read, None, verbose=False)
     self.piksi = Handler(self.framer)
     self.piksi.add_callback(self.callback)
     self.piksi.start()
Beispiel #7
0
class UDPReceiver(object):
    def __init__(self, host, port, callback):
        self._callback = callback
        self.driver = UDPDriver(host, port)
        self.framer = Framer(self.driver.read, None, verbose=False)
        self.piksi = Handler(self.framer)
        self.piksi.add_callback(self.callback)
        self.piksi.start()
    def callback(self, msg, **metadata):
        self._callback(msg, **metadata)
Beispiel #8
0
class SbpUdpMulticastReceiver:
    def __init__(self, port, ext_callback):
        self._callback = ext_callback
        self.driver = UDPDriver(' ', port)
        self.framer = Framer(self.driver.read, None, verbose=False)
        self.piksi = Handler(self.framer)
        self.piksi.add_callback(self.recv_callback)
        self.piksi.start()

    def recv_callback(self, msg, **metadata):
        self._callback(msg, **metadata)
Beispiel #9
0
class SerialReceiver(object):
    def __init__(self, port, baud_rate, callback):
        self._callback = callback
        self.driver = PySerialDriver(port, baud=baud_rate)
        self.framer = Framer(self.driver.read, None, verbose=False)
        self.piksi = Handler(self.framer)
        self.piksi.add_callback(self.callback)
        self.piksi.start()

    def callback(self, msg, **metadata):
        self._callback(msg, **metadata)
 def __init__(self):
     self.data_lock = threading.Lock()
     self.data = None
     # Set up the Duro
     # Assumes the Duro is plugged into the core via serial
     self.d = PySerialDriver('/dev/ttyS0')
     self.f = Framer(self.d.read, None, verbose=True)
     self.h = Handler(self.f)
     self.h.add_callback(self._update_gps_callback, SBP_MSG_POS_LLH_DEP_A)
     self.h.add_callback(self._update_gps_callback, SBP_MSG_POS_LLH)
     self.h.start()
Beispiel #11
0
class UDPReceiver(object):
    def __init__(self, host, port, callback):
        self._callback = callback
        self.driver = UDPDriver(host, port)
        self.framer = Framer(self.driver.read, None, verbose=False)
        self.piksi = Handler(self.framer)
        self.piksi.add_callback(self.callback)
        self.piksi.start()

    def callback(self, msg, **metadata):
        self._callback(msg, **metadata)
Beispiel #12
0
class SerialReceiver(object):
    def __init__(self, port, baud_rate, callback):
        self._callback = callback
        self.driver = PySerialDriver(port, baud=baud_rate)
        self.framer = Framer(self.driver.read, None, verbose=False)
        self.piksi = Handler(self.framer)
        self.piksi.add_callback(self.callback)
        self.piksi.start()

    def callback(self, msg, **metadata):
        self._callback(msg, **metadata)
Beispiel #13
0
def main(args):
    """
    Get configuration, get driver, get logger, and build handler and start it.
    """
    port = args.port
    baud = args.baud
    timeout = args.timeout
    log_filename = args.logfilename
    log_dirname = args.log_dirname
    if not log_filename:
        log_filename = logfilename()
    if log_dirname:
        log_filename = os.path.join(log_dirname, log_filename)
    append_log_filename = args.append_log_filename
    tags = args.tags
    # State for handling a networked base stations.
    channel = args.channel_id
    serial_id = int(args.serial_id) if args.serial_id is not None else None
    base = args.base
    use_broker = args.broker
    # Driver with context
    if args.tcp:
        try:
            host, port = port.split(':')
            driver = TCPDriver(host, int(port))
        except:  # noqa
            raise Exception('Invalid host and/or port')
    else:
        driver = get_driver(args.ftdi,
                            port,
                            baud,
                            args.file,
                            rtscts=args.rtscts)
        # Handler with context
    with Handler(Framer(driver.read, driver.write, args.verbose)) as link:
        # Logger with context
        with get_logger(args.log, log_filename, args.expand_json) as logger:
            with get_append_logger(append_log_filename, tags) as append_logger:
                link.add_callback(printer, SBP_MSG_PRINT_DEP)
                link.add_callback(log_printer, SBP_MSG_LOG)
                Forwarder(link, logger).start()
                Forwarder(link, append_logger).start()
                if use_broker and base and serial_id:
                    device_id = get_uuid(channel, serial_id)
                    with HTTPDriver(str(device_id), base) as http:
                        with Handler(
                                Framer(http.read, http.write,
                                       args.verbose)) as slink:
                            Forwarder(slink, swriter(link)).start()
                            run(args, link)
                else:
                    run(args, link)
def radio_corrections(q_radio):
    global radio_sender  # get radio sender id
    radio_rate = 5  #hz
    with PySerialDriver(RADIO_PORT, baud=RADIO_BAUDRATE) as driver:
        print(driver.read)
        with Handler(Framer(driver.read, None, verbose=False)) as source:
            try:
                for sbp_msg, metadata in source.filter():

                    start_time = rospy.get_time()

                    if radio_sender is None:
                        radio_sender = sbp_msg.sender

                    q_radio.put(sbp_msg)

                    if debug and sbp_msg.msg_type == sbp.observation.SBP_MSG_OBS:
                        radio_txt_file.write(str(dispatch(sbp_msg)) + "\n")

                    end_time = rospy.get_time()
                    sleep_time = max(
                        [0, 1 / radio_rate - (end_time - start_time)])
                    rospy.sleep(sleep_time)

            except KeyboardInterrupt:
                pass
Beispiel #15
0
def getData(portNum):
    '''
    Gets NED vector from Piksi GPS located at /dev/ttyUSB[portNum].
    If two Piksi GPSs are talking, NED vector is in meters.
    '''
    directory = '/dev/ttyUSB' + str(portNum)
    north = None
    east = None
    down = None

    # Open a connection to Piksi using the default baud rate (1Mbaud)
    try:
        with PySerialDriver(directory, baud=1000000) as driver:
            with Handler(Framer(driver.read, None)) as source:
                for msg, metadata in source.filter(SBP_MSG_BASELINE_NED):
                    north = msg.n * 1e-3
                    east = msg.e * 1e-3
                    down = msg.d * 1e-3

                    if north != None and east != None and down != None:
                        return north,east,down

    except Exception as e:
        print('Error: ' + str(e))
        return
Beispiel #16
0
  def run(self):
    '''Thread loop here'''
    # Problems? See: https://pylibftdi.readthedocs.org/en/latest/troubleshooting.html
    # with PyFTDIDriver(self.sbp_baud) as driver:
    try:
      with PySerialDriver(self.sbp_port, baud=self.sbp_baud) as driver:
        self.driver = driver
        self.framer = Framer(driver.read, driver.write)
        with Handler(self.framer) as handler:

          self.handler = handler
          handler.add_callback(self.handle_basestation_obs, msg_type=[SBP_MSG_OBS, SBP_MSG_BASE_POS_LLH])
          
          self.ready()

          #TODO: Get rid of "for msg, metadaya..."

          try:
            for msg, metadata in handler:
              if self.stopped():
                return
              pass
          except KeyboardInterrupt:
            raise
          except socket.error:
            raise
          except SystemExit:
            print "Exit Forced. We're dead."
            return


    except:
      traceback.print_exc()
      self.ready()
Beispiel #17
0
def main(args=None):
    """
    Get configuration, get driver, and build handler and start it.
    """
    args = get_args(args)
    command = args.command
    driver = serial_link.get_base_args_driver(args)
    with Handler(Framer(driver.read, driver.write,
                        verbose=args.verbose)) as link:
        settings = Settings(link, timeout=args.timeout)
        with settings:
            if command == 'write':
                settings.write(args.section,
                               args.setting,
                               args.value,
                               verbose=args.verbose)
            elif command == 'read':
                print(
                    settings.read(args.section,
                                  args.setting,
                                  verbose=args.verbose))
            elif command == 'all':
                settings.read_all(verbose=True)
            elif command == 'save':
                settings.save()
            elif command == 'reset':
                settings.reset()
            elif command == 'read_to_file':
                settings.read_to_file(args.output, verbose=args.verbose)
            elif command == 'write_from_file':
                settings.write_from_file(args.filename, verbose=args.verbose)
            # If saving was requested, we have done a write command, and the write was requested, we save
            if command.startswith("write") and args.save_after_write:
                print("Saving Settings to Flash.")
                settings.save()
Beispiel #18
0
 def __init__(self, host, port, callback):
     self._callback = callback
     self.driver = UDPDriver(host, port)
     self.framer = Framer(self.driver.read, None, verbose=False)
     self.piksi = Handler(self.framer)
     self.piksi.add_callback(self.callback)
     self.piksi.start()
Beispiel #19
0
 def __init__(self, port, baud_rate, callback):
     self._callback = callback
     self.driver = PySerialDriver(port, baud=baud_rate)
     self.framer = Framer(self.driver.read, None, verbose=False)
     self.piksi = Handler(self.framer)
     self.piksi.add_callback(self.callback)
     self.piksi.start()
Beispiel #20
0
def main():
    parser = argparse.ArgumentParser(
        description="Swift Navigation SBP Example.")
    parser.add_argument("-a",
                        "--host",
                        default='localhost',
                        help="specify the host address.")
    parser.add_argument("-p",
                        "--port",
                        default=55555,
                        help="specify the port to use.")
    args = parser.parse_args()

    # Open a connection to Piksi using TCP
    with TCPDriver(args.host, args.port) as driver:
        with Handler(Framer(driver.read, None, verbose=True)) as source:
            try:
                for msg, metadata in source.filter(SBP_MSG_POS_LLH):
                    # Print out the N, E, D coordinates of the baseline
                    print(
                        "%d,%.16f,%.16f,%.16f,%d,%d,%d,%d" %
                        (msg.tow, msg.lat, msg.lon, msg.height, msg.h_accuracy,
                         msg.v_accuracy, msg.n_sats, msg.flags))
            except KeyboardInterrupt:
                pass
Beispiel #21
0
def radio_corrections():
    last_radio_epoch = None
    radio_epoch = None
    radio_tow = 0

    with PySerialDriver(RADIO_PORT, baud=RADIO_BAUDRATE) as driver:
        print(driver.read)
        with Handler(Framer(driver.read, None, verbose=False)) as source:
            try:
                for msg, metadata in source.filter(
                    [SBP_MSG_OBS, SBP_MSG_GLO_BIASES, SBP_MSG_BASE_POS_ECEF]):
                    # change radio sender ID to ntrip to avoid temporal glitch
                    #msg.sender = 65202
                    # update epoch
                    if msg.msg_type == 74:
                        radio_epoch = radio_tow = msg.header.t.tow
                        rospy.loginfo("Radio, %i, %i, %s", radio_tow,
                                      msg.msg_type, hex(msg.header.n_obs))
                    if msg.msg_type == 117:
                        rospy.loginfo("Radio, %i, %i, None", radio_tow,
                                      msg.msg_type)

                    udp.call(msg)

            except KeyboardInterrupt:
                pass
def main():
    # Initialize ROS node. We imitate node name present in
    # http://github.com/ethz-asl/ethz_piksi_ros/tree/master/piksi_multi_rtk_ros
    rospy.init_node('piksi')

    # Initialize publisher. We imitate publisher names present in
    # http://github.com/ethz-asl/ethz_piksi_ros/tree/master/piksi_multi_rtk_ros
    pub = rospy.Publisher('~navsatfix_best_fix', NavSatFix, queue_size=10)

    # Retrive parameters from the ROS parameter serve
    # We imitate parameter names present in
    # http://github.com/ethz-asl/ethz_piksi_ros/tree/master/piksi_multi_rtk_ros
    host = rospy.get_param('~tcp_addr', '192.168.0.222')
    port = rospy.get_param('~tcp_port', 55555)

    # Start listening for GNSS data on a TCP socket with a ROS-param-provided
    # host and port. This code is almost copy-pasted from examples of the sbp
    # package
    with TCPDriver(host, port) as driver:
        with Handler(Framer(driver.read, None, verbose=True)) as source:
            try:
                # Filter on SBP_MSG_POS_LLH-messages (LLH stands for
                # Longitude Latitude Height)
                for msg, metadata in source.filter(SBP_MSG_POS_LLH):
                    _publish_navsatfix(pub, msg)

                    if rospy.is_shutdown():
                        break
            except KeyboardInterrupt:
                pass
Beispiel #23
0
def main():
    args = get_args()
    selected_driver = serial_link.get_base_args_driver(args)

    # Driver with context
    with selected_driver as driver:
        # Handler with context
        with Handler(Framer(driver.read, driver.write, args.verbose)) as link:
            f = FileIO(link)
            try:
                if args.write:
                    f.write(raw_filename(args.write[1]), bytearray(open(args.write[0], 'rb').read()))
                elif args.read:
                    if len(args.read) not in [1, 2]:
                        sys.stderr.write("Error: fileio read requires either 1 or 2 arguments, SOURCE and optionally DEST.")
                        sys.exit(1)
                    data = f.read(raw_filename(args.read[0]))
                    if len(args.read) == 2:
                        with open(args.read[1], ('w' if args.hex else 'wb')) as fd:
                            fd.write(hexdump(data) if args.hex else data)
                    elif args.hex:
                        print(hexdump(data))
                    else:
                        print(printable_text_from_device(data))
                elif args.delete:
                    f.remove(raw_filename(args.delete[0]))
                elif args.list is not None:
                    print_dir_listing(f.readdir(raw_filename(args.list[0])))
                else:
                    print("No command given, listing root directory:")
                    print_dir_listing(f.readdir())
            except KeyboardInterrupt:
                pass
Beispiel #24
0
def main():
    parser = argparse.ArgumentParser(
        description="Swift Navigation SBP Example.")
    parser.add_argument(
        "-a",
        "--host",
        default='localhost',
        help="specify the host address.")
    parser.add_argument(
        "-p",
        "--port",
        default=55555,
        help="specify the port to use.")
    args = parser.parse_args()

    # Open a connection to Piksi using TCP
    with TCPDriver(args.host, args.port) as driver:
        with Handler(Framer(driver.read, None, verbose=True)) as source:
            try:
                for msg, metadata in source.filter(SBP_MSG_BASELINE_NED):
                    # Print out the N, E, D coordinates of the baseline
                    print("%.4f,%.4f,%.4f" % (msg.n * 1e-3, msg.e * 1e-3,
                                              msg.d * 1e-3))
            except KeyboardInterrupt:
                pass
def write_ini_file(file_name, port='/dev/ttyUSB0', baud=115200):
    '''
    Write setting from an ini file to the Multi

    Args:
        file_name: file name of the ini file
        port: serial port [default='/dev/ttyUSB0']
        baud: baud rate [default=115200]

    Returns:
        None
    '''

    print('Reading from {} at {}'.format(port, baud))

    with PySerialDriver(port, baud) as driver:
        with Handler(Framer(driver.read, driver.write,
                            verbose=True)) as source:

            parser = configparser.ConfigParser()
            parser.optionxform = str
            with open(file_name, 'r') as f:
                parser.read_file(f)

            source.add_callback(settings_callback, SBP_MSG_SETTINGS_READ_RESP)

            for section, settings in parser.items():
                print('\nSECTION: {}'.format(section))
                for setting, value in settings.items():
                    print('{} = {}'.format(setting, value))
                    write(source, section, setting, value)

            source(MsgSettingsSave())
Beispiel #26
0
def main():

    with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as sbp_udp:
        sbp_udp.setblocking(1)
        sbp_udp.settimeout(None)
        sbp_udp.bind((DEFAULT_UDP_BIND_ADDRESS, DEFAULT_UDP_RECV_PORT))

        def handle_incoming(msg, **metadata):
            sbp_udp.sendto(msg.pack(),
                           (DEFAULT_UDP_SEND_ADDRESS, DEFAULT_UDP_SEND_PORT))

        with PySerialDriver(DEFAULT_SERIAL_PORT,
                            DEFAULT_SERIAL_BAUD) as driver:
            with Handler(Framer(driver.read, driver.write)) as handler:

                handler.add_callback(handle_incoming)

                print "***"
                print "*** Solo Relay Running"
                print "***"

                print "Sending to %s : %s" % (DEFAULT_UDP_SEND_ADDRESS,
                                              DEFAULT_UDP_SEND_PORT)
                print "Recving on %s : %s" % (DEFAULT_UDP_BIND_ADDRESS,
                                              DEFAULT_UDP_RECV_PORT)

                try:
                    while True:
                        data, addr = sbp_udp.recvfrom(4096)
                        driver.write(data)
                except KeyboardInterrupt:
                    pass
Beispiel #27
0
def main():
    parser = argparse.ArgumentParser(
        description="Export position suitable for KML file.")
    parser.add_argument("-a",
                        "--host",
                        default='192.168.5.51',
                        help="specify the host address.")
    parser.add_argument("-p",
                        "--port",
                        default=55555,
                        help="specify the port to use.")
    args = parser.parse_args()
    prev = None
    # Open a connection to Piksi using TCP
    with TCPDriver(args.host, args.port) as driver:
        with Handler(Framer(driver.read, None, verbose=True)) as source:
            try:
                for msg, metadata in source.filter(SBP_MSG_POS_LLH):
                    cur = (msg.lat, msg.lon)
                    if prev:
                        dist = vincenty(prev, cur).meters
                        #print(dist)
                        if dist > 0.1:
                            print("%.16f,%.16f,%.16f" %
                                  (msg.lon, msg.lat, msg.height))
                            prev = cur
                    else:
                        prev = cur
            except KeyboardInterrupt:
                pass
Beispiel #28
0
def main():
    """
    Get configuration, get driver, and build handler and start it.
    """
    args = get_args()
    port = args.port[0]
    baud = args.baud[0]
    use_ftdi = args.ftdi
    # Driver with context
    with serial_link.get_driver(use_ftdi, port, baud) as driver:
        # Handler with context
        with Handler(Framer(driver.read, driver.write)) as link:
            link.add_callback(serial_link.log_printer, SBP_MSG_LOG)
            link.add_callback(serial_link.printer, SBP_MSG_PRINT_DEP)

            data = open(args.file, 'rb').read()

            def progress_cb(size):
                sys.stdout.write("\rProgress: %d%%    \r" %
                                 (100 * size / len(data)))
                sys.stdout.flush()

            print('Transferring image file...')
            FileIO(link).write("upgrade.image_set.bin",
                               data,
                               progress_cb=progress_cb)
            print('Committing file to flash...')
            code = shell_command(link, "upgrade_tool upgrade.image_set.bin",
                                 300)
            if code != 0:
                print('Failed to perform upgrade (code = %d)' % code)
                return
            print('Resetting Piksi...')
            link(MsgReset(flags=0))
Beispiel #29
0
def test_http_test_pass():
    assert is_enabled()
    msg = MsgPrintDep(text='abcd')
    register_uri(GET,
                 BASE_STATION_URI,
                 msg.to_binary(),
                 content_type="application/vnd.swiftnav.broker.v1+sbp2")
    register_uri(PUT,
                 BASE_STATION_URI,
                 '',
                 content_type="application/vnd.swiftnav.broker.v1+sbp2")
    with HTTPDriver(device_uid="Swift22", url=BASE_STATION_URI) as driver:
        assert not driver.read_ok
        assert driver.connect_read()
        assert driver.read_ok
        assert driver.read(size=255) == msg.to_binary()
        with pytest.raises(IOError):
            assert driver.read(size=255)
        assert not driver.read_close()
        assert driver.read_response is None
        assert not driver.read_ok
        with pytest.raises(ValueError):
            driver.read(size=255)
    with HTTPDriver(device_uid="Swift22", url=BASE_STATION_URI) as http:
        with Handler(Framer(http.read, http.write, False)) as link:

            def tester(sbp_msg, **metadata):
                assert sbp_msg.payload == msg.payload

            link.add_callback(SBP_MSG_PRINT_DEP, tester)
            t0 = time.time()
            sleep = 0.1
            while True:
                if time.time() - t0 < sleep:
                    break
Beispiel #30
0
def main():
    """
    Get configuration, get driver, and build handler and start it.
    """
    args = get_args()
    driver = serial_link.get_base_args_driver(args)
    # Driver with context
    # Handler with context
    with Handler(Framer(driver.read, driver.write,
                        verbose=args.verbose)) as link:
        data = bytearray(open(args.firmware, 'rb').read())

        def progress_cb(size, _):
            sys.stdout.write("\rProgress: %d%%    \r" %
                             (100 * size / len(data)))
            sys.stdout.flush()

        print('Transferring image file...')
        FileIO(link).write(b"upgrade.image_set.bin",
                           data,
                           progress_cb=progress_cb)
        print('Committing file to flash...')
        link.add_callback(serial_link.log_printer, SBP_MSG_LOG)
        link.add_callback(serial_link.printer, SBP_MSG_PRINT_DEP)

        code = shell_command(link, b"upgrade_tool upgrade.image_set.bin", 300)
        if code != 0:
            print('Failed to perform upgrade (code = %d)' % code)
            return
        print('Resetting Piksi...')
        link(MsgReset(flags=0))
Beispiel #31
0
def main():
  """
  Get configuration, get driver, and build handler and start it.
  """
  args = get_args()
  port = args.port[0]
  baud = args.baud[0]
  # Driver with context
  with serial_link.get_driver(args.ftdi, port, baud) as driver:
    # Handler with context
    with Handler(Framer(driver.read, driver.write)) as link:
      print("Resetting mask to 0xff")
      send_setting(link, "uart_ftdi", "sbp_message_mask", "65535")
      time.sleep(0.5)

      print("Resetting baudrate to 1Mbps")
      send_setting(link, "uart_ftdi", "baudrate", "1000000")
      time.sleep(0.5)

      print("Resetting mode to SBP")
      send_setting(link, "uart_ftdi", "mode", "SBP")
      time.sleep(0.5)

      print("Attempting to save settings")
      link.send(SBP_MSG_SETTINGS_SAVE, "")
      time.sleep(0.5)
      link.send(SBP_MSG_SETTINGS_SAVE, "")
      time.sleep(0.5)
      link.send(SBP_MSG_SETTINGS_SAVE, "")
      time.sleep(0.5)
      link.send(SBP_MSG_SETTINGS_SAVE, "")

      print("Sent Settings Reset message to return FTDI to defaults")
Beispiel #32
0
def test_tcp_logger():
    handler = tcp_handler(MsgPrintDep(text=b'abc').to_binary())
    ip, port = tcp_server(handler)
    t0 = time.time()
    sleep = 0.1
    timeout = 5.0
    cb_context = {'assert_logger_called': False}

    def assert_logger(s, **metadata):
        cb_context['assert_logger_called'] = True
        assert s.preamble == 0x55
        assert s.msg_type == 0x10
        assert s.sender == 66
        assert s.length == 3
        assert s.payload == b'abc'
        assert s.crc == 0xDAEE

    with TCPDriver(ip, port) as driver:
        with Handler(Framer(driver.read, None, verbose=False),
                     autostart=False) as link:
            link.add_callback(assert_logger)
            link.start()
            while True:
                if time.time() - t0 > timeout or cb_context[
                        'assert_logger_called']:
                    break
                time.sleep(sleep)
    assert cb_context[
        'assert_logger_called'], "SBP msg callback function was not called"
Beispiel #33
0
def main():
    """
    Get configuration, get driver, get logger, and build handler and start it.
    Create relevant TestState object and perform associated actions.
    Modeled after serial_link main function.
    """
    args = get_args()
    port = args.port
    baud = args.baud
    timeout = args.timeout[0]
    log_filename = args.log_filename[0]
    interval = int(args.interval[0])
    minsats = int(args.minsats[0])

    # Driver with context
    with sl.get_driver(args.ftdi, port, baud) as driver:
        # Handler with context
        with Handler(Framer(driver.read, driver.write, args.verbose)) as link:
            # Logger with context
            with sl.get_logger(args.log, log_filename) as logger:
                # print out SBP_MSG_PRINT_DEP messages
                link.add_callback(sl.printer, SBP_MSG_PRINT_DEP)
                link.add_callback(sl.log_printer, SBP_MSG_LOG)
                # add logger callback
                Forwarder(link, logger).start()
                try:
                    # Get device info
                    # Diagnostics reads out the device settings and resets the Piksi
                    piksi_diag = ptd.Diagnostics(link)
                    while not piksi_diag.heartbeat_received:
                        time.sleep(0.1)
                    # add Teststates and associated callbacks
                    with DropSatsState(
                            link,
                            piksi_diag.sbp_version,
                            interval,
                            minsats,
                            debug=args.verbose) as drop:
                        link.add_callback(drop.process_message)

                        if timeout is not None:
                            expire = time.time() + float(args.timeout[0])

                        while True:
                            if timeout is None or time.time() < expire:
                                # Wait forever until the user presses Ctrl-C
                                time.sleep(1)
                            else:
                                print("Timer expired!")
                                break
                            if not link.is_alive():
                                sys.stderr.write("ERROR: link is gone!\n")
                                sys.exit(1)
                except KeyboardInterrupt:
                    # Callbacks call thread.interrupt_main(), which throw a KeyboardInterrupt
                    # exception. To get the proper error condition, return exit code
                    # of 1. Note that the finally block does get caught since exit
                    # itself throws a SystemExit exception.
                    sys.exit(1)
Beispiel #34
0
    def main():
        parser = argparse.ArgumentParser(
            description="Swift Navigation SBP Setting Monitor example.")
        parser.add_argument("-H",
                            "--host",
                            required=True,
                            help="specify the host address.")
        parser.add_argument("-p",
                            "--port",
                            default=55556,
                            help="specify the port to use.")
        args = parser.parse_args()

        monitor = SettingMonitor()

        with TCPDriver(args.host, args.port) as driver:
            with Handler(Framer(driver.read, driver.write,
                                verbose=True)) as link:
                driver.flush()
                time.sleep(2)

                # Capture setting messages
                link.add_callback(monitor.capture_setting,
                                  SBP_MSG_SETTINGS_READ_RESP)

                link.add_callback(print_setting, SBP_MSG_SETTINGS_READ_RESP)

                # Disable spectrum analyzer
                link(
                    MsgSettingsWrite(
                        setting=b'%s\0%s\0%s\0' %
                        (b'system_monitor', b'spectrum_analyzer', b'False')))

                # Request the value of the system_monitor:spectrum_analyzer setting
                link(
                    MsgSettingsReadReq(
                        setting=b'%s\0%s\0' %
                        (b'system_monitor', b'spectrum_analyzer')))

                # Wait up to 5 seconds to see the setting we want
                specan_off = monitor.wait_for_setting_value(
                    b'system_monitor', b'spectrum_analyzer', b'False')

                assert (specan_off == True)
                print("Spectrum analyzer turned off!")

                # Request the value of the system_monitor:spectrum_analyzer setting
                link(
                    MsgSettingsReadReq(
                        setting=b'%s\0%s\0' %
                        (b'system_monitor', b'spectrum_analyzer')))

                # Wait up to 5 seconds to see the setting we (don't) want
                specan_off = monitor.wait_for_setting_value(
                    b'system_monitor', b'spectrum_analyzer', b'True')

                assert (specan_off == False)
                print("Spectrum analyzer still off!")
Beispiel #35
0
    def connect_piksi(self):
        self.piksi_driver = PySerialDriver(self.piksi_port, baud=1000000)
        self.piksi_framer = Framer(self.piksi_driver.read, self.piksi_driver.write, verbose=False)
        self.piksi = Handler(self.piksi_framer)

        # Only setup log and settings messages
        # We will wait to set up rest until piksi is configured
        self.piksi.add_callback(self.callback_sbp_heartbeat, msg_type=SBP_MSG_HEARTBEAT)
        self.piksi.add_callback(self.callback_sbp_log, msg_type=SBP_MSG_LOG)
        self.piksi.add_callback(self.callback_sbp_settings_read_resp, msg_type=SBP_MSG_SETTINGS_READ_RESP)
        self.piksi.add_callback(self.callback_sbp_settings_read_by_index_resp, msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_REQ)
        self.piksi.add_callback(self.callback_sbp_settings_read_by_index_resp, msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
        self.piksi.add_callback(self.callback_sbp_settings_read_by_index_done, msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_DONE)

        self.piksi.add_callback(self.callback_sbp_startup, SBP_MSG_STARTUP)

        self.piksi.start()
Beispiel #36
0
class PiksiROS(object):
    def __init__(self):

        rospy.init_node("piksi_ros")

        self.send_observations = False
        self.serial_number = None

        self.last_baseline = None
        self.last_vel = None
        self.last_pos = None
        self.last_dops = None
        self.last_rtk_pos = None

        self.last_spp_time = 0
        self.last_rtk_time = 0
        self.fix_mode = -1
        self.rtk_fix_mode = 2

        self.read_piksi_settings_info()

        self.piksi_settings = tree()

        self.proj = None

        self.disconnect_piksi()

        self.read_params()

        self.setup_comms()

        self.odom_msg = Odometry()
        self.odom_msg.header.frame_id = self.rtk_frame_id
        self.odom_msg.child_frame_id = self.child_frame_id

        self.odom_msg.pose.covariance[0] = self.rtk_h_accuracy**2
        self.odom_msg.pose.covariance[7] = self.rtk_h_accuracy**2
        self.odom_msg.pose.covariance[14] = self.rtk_v_accuracy**2
        self.odom_msg.twist.covariance[0] = self.odom_msg.pose.covariance[0] * 4
        self.odom_msg.twist.covariance[7] = self.odom_msg.pose.covariance[7] * 4
        self.odom_msg.twist.covariance[14] = self.odom_msg.pose.covariance[14] * 4

        self.odom_msg.pose.covariance[21] = COV_NOT_MEASURED
        self.odom_msg.pose.covariance[28] = COV_NOT_MEASURED
        self.odom_msg.pose.covariance[35] = COV_NOT_MEASURED
        self.odom_msg.twist.covariance[21] = COV_NOT_MEASURED
        self.odom_msg.twist.covariance[28] = COV_NOT_MEASURED
        self.odom_msg.twist.covariance[35] = COV_NOT_MEASURED

        self.transform = TransformStamped()
        self.transform.header.frame_id = self.utm_frame_id
        self.transform.child_frame_id = self.rtk_frame_id
        self.transform.transform.rotation = Quaternion(x=0,y=0,z=0,w=1)

        # Used to publish transform from rtk_frame_id -> child_frame_id
        self.base_link_transform = TransformStamped()
        self.base_link_transform.header.frame_id = self.rtk_frame_id
        self.base_link_transform.child_frame_id = self.child_frame_id
        self.base_link_transform.transform.rotation = Quaternion(x=0,y=0,z=0,w=1)

        self.diag_updater = diagnostic_updater.Updater()
        self.heartbeat_diag = diagnostic_updater.FrequencyStatus(diagnostic_updater.FrequencyStatusParam({'min':self.diag_heartbeat_freq, 'max':self.diag_heartbeat_freq}, self.diag_freq_tolerance, self.diag_window_size))
        self.diag_updater.add("Piksi status", self.diag)
        self.diag_updater.add(self.heartbeat_diag)

        self.setup_pubsub()

    def read_piksi_settings_info(self):
        self.piksi_settings_info = tree()
        settings_info = yaml.load(open(os.path.join(PKG_PATH, 'piksi_settings.yaml'), 'r'))
        for s in settings_info:

            if s['type'].lower() == 'boolean':
                s['parser'] = lambda x: x.lower()=='true'
            elif s['type'].lower() in ('float', 'double','int'):
                s['parser'] = ast.literal_eval
            elif s['type'] == 'enum':
                s['parser'] = s['enum'].index
            else:
                s['parser'] = lambda x: x

            self.piksi_settings_info[s['group']][s['name']] = s

    def init_proj(self, latlon):
        self.proj = Proj(proj='utm',zone=calculate_utm_zone(*latlon)[0],ellps='WGS84')

    def reconfig_callback(self, config, level):
        for p,v in config.iteritems():
            if p=='groups':
                continue
            n = p.split('__')
            if self.piksi_settings[n[1]][n[2]] != v:
                i = self.piksi_settings_info[n[1]][n[2]]
                p_val = v
                if i['type'] == 'enum':
                    try:
                        p_val = i['enum'][v]
                    except:
                        p_val = i['enum'][0]
                self.piksi_set(n[1],n[2],p_val)
                self.piksi_settings[n[1]][n[2]] = v
        return config

    def read_params(self):
        self.debug = rospy.get_param('~debug', False)
        self.frame_id = rospy.get_param('~frame_id', "piksi")
        self.rtk_frame_id = rospy.get_param('~rtk_frame_id', "rtk_gps")
        self.utm_frame_id = rospy.get_param('~utm_frame_id', "utm")
        self.child_frame_id = rospy.get_param('~child_frame_id', "base_link")
        self.piksi_port = rospy.get_param('~port', "/dev/ttyUSB0")

        self.publish_utm_rtk_tf = rospy.get_param('~publish_utm_rtk_tf', False)
        self.publish_rtk_child_tf = rospy.get_param('~publish_rtk_child_tf', False)

        self.diag_heartbeat_freq = rospy.get_param('~diag/heartbeat_freq', 1.0)
        self.diag_update_freq = rospy.get_param('~diag/update_freq', 10.0)
        self.diag_freq_tolerance = rospy.get_param('~diag/freq_tolerance', 0.1)
        self.diag_window_size = rospy.get_param('~diag/window_size', 10)
        self.diag_min_delay = rospy.get_param('~diag/min_delay', 0.0)
        self.diag_max_delay = rospy.get_param('~diag/max_delay', 0.2)

        # Send/receive observations through udp
        self.obs_udp_send = rospy.get_param('~obs/udp/send', False)
        self.obs_udp_recv = rospy.get_param('~obs/udp/receive', False)
        self.obs_udp_host = rospy.get_param('~obs/udp/host', "")
        self.obs_udp_port = rospy.get_param('~obs/udp/port', 50785)

        # Send/receive observations through serial port
        self.obs_serial_send = rospy.get_param('~obs/serial/send', False)
        self.obs_serial_recv = rospy.get_param('~obs/serial/receive', False)
        self.obs_serial_port = rospy.get_param('~obs/serial/port', None)
        self.obs_serial_baud_rate = rospy.get_param('~obs/serial/baud_rate', 115200)

        self.rtk_h_accuracy = rospy.get_param("~rtk_h_accuracy", 0.04)
        self.rtk_v_accuracy = rospy.get_param("~rtk_v_accuracy", self.rtk_h_accuracy*3)

        self.sbp_log = rospy.get_param('~sbp_log_file', None)

        self.rtk_fix_timeout = rospy.get_param('~rtk_fix_timeout', 0.2)
        self.spp_fix_timeout = rospy.get_param('~spp_fix_timeout', 1.0)

        self.publish_ephemeris = rospy.get_param('~publish_ephemeris', False)
        self.publish_observations = rospy.get_param('~publish_observations', False)

        self.piksi_update_settings = rospy.get_param('~piksi',{})
        self.piksi_save_settings = rospy.get_param('~piksi_save_settings', False)

    def setup_comms(self):
        self.obs_senders = []
        self.obs_receivers = []

        if self.obs_udp_send or self.obs_serial_send:
            self.send_observations = True

        if self.obs_udp_send:
            self.obs_senders.append(UDPSender(self.obs_udp_host, self.obs_udp_port))

        if self.obs_serial_send:
            self.obs_senders.append(SerialSender(self.obs_serial_port, self.obs_serial_baud_rate))

        if self.obs_udp_recv:
            self.obs_receivers.append(UDPReceiver(self.obs_udp_host, self.obs_udp_port, self.callback_external))

        if self.obs_serial_recv:
            self.obs_receivers.append(SerialReceiver(self.obs_serial_port, self.obs_serial_baud_rate, self.callback_external))

    def callback_external(self, msg, **metadata):
        if self.debug:
            rospy.loginfo("Received external SBP msg.")

        if self.piksi_framer:
            self.piksi_framer(msg, **metadata)
        else:
            rospy.logwarn("Received external SBP msg, but Piksi not connected.")

    def setup_pubsub(self):

        freq_params = diagnostic_updater.FrequencyStatusParam({'min':self.diag_update_freq, 'max':self.diag_update_freq}, self.diag_freq_tolerance, self.diag_window_size)
        time_params = diagnostic_updater.TimeStampStatusParam(self.diag_min_delay, self.diag_max_delay)

        self.pub_fix = rospy.Publisher("~fix", NavSatFix, queue_size=1000)

        self.pub_spp_fix = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~spp_fix", NavSatFix, queue_size=1000), self.diag_updater, freq_params, time_params)

        self.pub_rtk_fix = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~rtk_fix", NavSatFix, queue_size=1000), self.diag_updater, freq_params, time_params)
        #self.pub_rtk = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~rtk_odom", Odometry, queue_size=1000), self.diag_updater, freq_params, time_params)
        self.pub_odom = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~odom", Odometry, queue_size=1000), self.diag_updater, freq_params, time_params)
        self.pub_time = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~time", TimeReference, queue_size=1000), self.diag_updater, freq_params, time_params)

        if self.publish_utm_rtk_tf or self.publish_rtk_child_tf:
            self.tf_br = tf2_ros.TransformBroadcaster()

        if self.publish_ephemeris:
            self.pub_eph = rospy.Publisher("~ephemeris", Ephemeris, queue_size=1000)

        if self.publish_observations:
            self.pub_obs = rospy.Publisher('~observations', Observations, queue_size=1000)


    def piksi_start(self):

        self.dr_srv = GroupServer(PiksiDriverConfig, self.reconfig_callback, ns_prefix=('dynamic_reconfigure',), nest_groups=False)

        self.piksi.add_callback(self.callback_sbp_gps_time, msg_type=SBP_MSG_GPS_TIME)
        self.piksi.add_callback(self.callback_sbp_dops, msg_type=SBP_MSG_DOPS)

        self.piksi.add_callback(self.callback_sbp_pos, msg_type=SBP_MSG_POS_LLH)
        self.piksi.add_callback(self.callback_sbp_baseline, msg_type=SBP_MSG_BASELINE_NED)
        self.piksi.add_callback(self.callback_sbp_vel, msg_type=SBP_MSG_VEL_NED)

        #if self.send_observations:
        self.piksi.add_callback(self.callback_sbp_obs, msg_type=SBP_MSG_OBS)
        self.piksi.add_callback(self.callback_sbp_base_pos, msg_type=SBP_MSG_BASE_POS)
        if self.publish_ephemeris:
            self.piksi.add_callback(self.callback_sbp_ephemeris, msg_type=SBP_MSG_EPHEMERIS)

        if self.sbp_log is not None:
            self.sbp_logger = RotatingFileLogger(self.sbp_log, when='M', interval=60, backupCount=0)
            self.piksi.add_callback(self.sbp_logger)

    def connect_piksi(self):
        self.piksi_driver = PySerialDriver(self.piksi_port, baud=1000000)
        self.piksi_framer = Framer(self.piksi_driver.read, self.piksi_driver.write, verbose=False)
        self.piksi = Handler(self.piksi_framer)

        # Only setup log and settings messages
        # We will wait to set up rest until piksi is configured
        self.piksi.add_callback(self.callback_sbp_heartbeat, msg_type=SBP_MSG_HEARTBEAT)
        self.piksi.add_callback(self.callback_sbp_log, msg_type=SBP_MSG_LOG)
        self.piksi.add_callback(self.callback_sbp_settings_read_resp, msg_type=SBP_MSG_SETTINGS_READ_RESP)
        self.piksi.add_callback(self.callback_sbp_settings_read_by_index_resp, msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_REQ)
        self.piksi.add_callback(self.callback_sbp_settings_read_by_index_resp, msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
        self.piksi.add_callback(self.callback_sbp_settings_read_by_index_done, msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_DONE)

        self.piksi.add_callback(self.callback_sbp_startup, SBP_MSG_STARTUP)

        self.piksi.start()

    def disconnect_piksi(self):
        try:
            self.piksi.stop()
            self.piksi.remove_callback()
        except:
            pass
        self.piksi = None
        self.piksi_framer = None
        self.piksi_driver = None

    def set_serial_number(self, serial_number):
        rospy.loginfo("Connected to piksi #%d" % serial_number)
        self.serial_number = serial_number
        self.diag_updater.setHardwareID("Piksi %d" % serial_number)

    def read_piksi_settings(self):
        self.settings_index = 0
        self.piksi_framer(MsgSettingsReadByIndexReq(index=self.settings_index))
        #self.piksi_framer(MsgSettingsReadReq(setting='simulator\0enabled\0'))

    def piksi_set(self, section, setting, value):
        m = MsgSettingsWrite(setting="%s\0%s\0%s\0" % (section, setting, value))
        self.piksi_framer(m)

    def update_dr_param(self, section, name, value):
        #print 'set %s:%s to %s' % (section, name, value)
        #print '~dynamic_reconfigure/piksi__%s__%s' % (section, name), value
        rospy.set_param('~dynamic_reconfigure/piksi__%s__%s' % (section, name), value)

    def set_piksi_settings(self):
        save_needed = False
        for s in nested_dict_iter(self.piksi_update_settings):
            cval = self.piksi_settings[s[0][0]][s[0][1]]
            if len(cval) != 0:
                if cval != str(s[1]):
                    rospy.loginfo('Updating piksi setting %s:%s to %s.' % (s[0][0], s[0][1], s[1]))
                    self.piksi_set(s[0][0], s[0][1], s[1])
                    self.update_dr_param(s[0][0], s[0][1], s[1])
                    save_needed = True
                else:
                    rospy.loginfo('Piksi setting %s:%s already set to %s.' % (s[0][0], s[0][1], s[1]))
            else:
                rospy.logwarn('Invalid piksi setting: %s' % ':'.join(s[0]))

        if self.piksi_save_settings and save_needed:
            rospy.loginfo('Saving piksi settings to flash')
            m = MsgSettingsSave()
            self.piksi_framer(m)

    def callback_sbp_startup(self, msg, **metadata):
        rospy.loginfo('Piksi startup packet received: %s' % repr(msg))

    def callback_sbp_gps_time(self, msg, **metadata):
        if self.debug:
            rospy.loginfo("Received SBP_MSG_GPS_TIME (Sender: %d): %s" % (msg.sender, repr(msg)))

        out = TimeReference()
        out.header.frame_id = self.frame_id
        out.header.stamp = rospy.Time.now()
        out.time_ref = ros_time_from_sbp_time(msg)
        out.source = "gps"
        self.pub_time.publish(out)

    def callback_sbp_heartbeat(self, msg, **metadata):
        if self.debug:
            rospy.loginfo("Received SBP_MSG_HEARTBEAT (Sender: %d): %s" % (msg.sender, repr(msg)))

        self.heartbeat_diag.tick()
        if self.serial_number is None:
            self.set_serial_number(msg.sender)
            self.read_piksi_settings()


    def callback_sbp_dops(self, msg, **metadata):
        if self.debug:
            rospy.loginfo("Received SBP_MSG_DOPS (Sender: %d): %s" % (msg.sender, repr(msg)))

        self.last_dops = msg

    def callback_sbp_pos(self, msg, **metadata):
        if self.debug:
            rospy.loginfo("Received SBP_MSG_POS_LLH (Sender: %d): %s" % (msg.sender, repr(msg)))

        out = NavSatFix()
        out.header.frame_id = self.frame_id
        out.header.stamp = rospy.Time.now()

        out.status.service = NavSatStatus.SERVICE_GPS

        out.latitude = msg.lat
        out.longitude = msg.lon
        out.altitude = msg.height

        out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED

        if msg.flags & 0x03:
            self.last_rtk_time = time.time()
            self.rtk_fix_mode = msg.flags & 0x03

            out.status.status = NavSatStatus.STATUS_GBAS_FIX

            # TODO this should probably also include covariance of base fix?
            out.position_covariance[0] = self.rtk_h_accuracy**2
            out.position_covariance[4] = self.rtk_h_accuracy**2
            out.position_covariance[8] = self.rtk_v_accuracy**2

            pub = self.pub_rtk_fix
            self.last_rtk_pos = msg

            # If we are getting this message, RTK is our best fix, so publish this as our best fix.
            self.pub_fix.publish(out)
        else:

            self.last_spp_time = time.time()

            out.status.status = NavSatStatus.STATUS_FIX

            # TODO hack, piksi should provide these numbers
            if self.last_dops:
                out.position_covariance[0] = (self.last_dops.hdop * 1E-2)**2
                out.position_covariance[4] = (self.last_dops.hdop * 1E-2)**2
                out.position_covariance[8] = (self.last_dops.vdop * 1E-2)**2
            else:
                out.position_covariance[0] = COV_NOT_MEASURED
                out.position_covariance[4] = COV_NOT_MEASURED
                out.position_covariance[8] = COV_NOT_MEASURED

            pub = self.pub_spp_fix
            self.last_pos = msg

            # Check if SPP is currently our best available fix
            if self.rtk_fix_mode <= 0:
                self.pub_fix.publish(out)

        pub.publish(out)

    def publish_odom(self):
        if self.last_baseline is None or self.last_vel is None:
            return

        if self.last_baseline.tow == self.last_vel.tow:
            self.odom_msg.header.stamp = rospy.Time.now()

            self.odom_msg.pose.pose.position.x = self.last_baseline.e/1000.0
            self.odom_msg.pose.pose.position.y = self.last_baseline.n/1000.0
            self.odom_msg.pose.pose.position.z = -self.last_baseline.d/1000.0

            self.odom_msg.twist.twist.linear.x = self.last_vel.e/1000.0
            self.odom_msg.twist.twist.linear.y = self.last_vel.n/1000.0
            self.odom_msg.twist.twist.linear.z = -self.last_vel.d/1000.0

            self.pub_odom.publish(self.odom_msg)

    def callback_sbp_vel(self, msg, **metadata):
        if self.debug:
            rospy.loginfo("Received SBP_MSG_VEL_NED (Sender: %d): %s" % (msg.sender, repr(msg)))
        self.last_vel = msg
        self.publish_odom()

    def callback_sbp_baseline(self, msg, **metadata):
        if self.debug:
            rospy.loginfo("Received SBP_MSG_BASELINE_NED (Sender: %d): %s" % (msg.sender, repr(msg)))

        if self.publish_rtk_child_tf:
            self.base_link_transform.header.stamp = rospy.Time.now()
            self.base_link_transform.transform.translation.x = msg.e/1000.0
            self.base_link_transform.transform.translation.y = msg.n/1000.0
            self.base_link_transform.transform.translation.z = -msg.d/1000.0
            self.tf_br.sendTransform(self.base_link_transform)

        self.last_baseline = msg
        self.publish_odom()

    def callback_sbp_ephemeris(self, msg, **metadata):
        if self.debug:
            rospy.loginfo("Received SBP_MSG_EPHEMERIS (Sender: %d): %s" % (msg.sender, repr(msg)))

        if not hasattr(self, 'eph_msg'):
            self.eph_msg = Ephemeris()
        self.eph_msg.header.stamp = rospy.Time.now()
        self.eph_msg.header.frame_id = self.frame_id

        self.eph_msg.tgd = msg.tgd
        self.eph_msg.c_rs = msg.c_rs
        self.eph_msg.c_rc = msg.c_rc
        self.eph_msg.c_uc = msg.c_uc
        self.eph_msg.c_us = msg.c_us
        self.eph_msg.c_ic = msg.c_ic
        self.eph_msg.c_is = msg.c_is
        self.eph_msg.dn = msg.dn
        self.eph_msg.m0 = msg.m0
        self.eph_msg.ecc = msg.ecc
        self.eph_msg.sqrta = msg.sqrta
        self.eph_msg.omega0 = msg.omega0
        self.eph_msg.omegadot = msg.omegadot
        self.eph_msg.w = msg.w
        self.eph_msg.inc = msg.inc
        self.eph_msg.inc_dot = msg.inc_dot
        self.eph_msg.af0 = msg.af0
        self.eph_msg.af1 = msg.af1
        self.eph_msg.af2 = msg.af2
        self.eph_msg.toe_tow = msg.toe_tow
        self.eph_msg.toe_wn = msg.toe_wn
        self.eph_msg.toc_tow = msg.toc_tow
        self.eph_msg.toc_wn = msg.toc_wn
        self.eph_msg.valid = msg.valid
        self.eph_msg.healthy = msg.healthy
        self.eph_msg.sid.sat = msg.sid.sat
        self.eph_msg.sid.band = msg.sid.band
        self.eph_msg.sid.constellation = msg.sid.constellation
        self.eph_msg.iode = msg.iode
        self.eph_msg.iodc = msg.iodc

        self.pub_eph.publish(m)

    def callback_sbp_obs(self, msg, **metadata):
        if self.debug:
            rospy.loginfo("Received SBP_MSG_OBS (Sender: %d): %s" % (msg.sender, repr(msg)))

        if self.send_observations:
            for s in self.obs_senders:
                s.send(msg)

        if self.publish_observations:
            if not hasattr(self, 'obs_msg'):
                self.obs_msg = Observations()

            # Need to do some accounting to figure out how many sbp packets to expect
            num_packets = (msg.header.n_obs >> 4) & 0x0F
            packet_no = msg.header.n_obs & 0x0F

            if self.obs_msg.tow != msg.header.t.tow:

                self.obs_msg.header.stamp = rospy.Time.now()
                self.obs_msg.header.frame_id = self.frame_id

                self.obs_msg.tow = msg.header.t.tow
                self.obs_msg.wn = msg.header.t.wn
                self.obs_msg.n_obs = 0

                self.obs_msg.obs = []

            # lets use this field to count how many packets we have so far
            self.obs_msg.n_obs += 1

            for obs in msg.obs:
                x = Obs()
                x.P = obs.P
                x.L.i = obs.L.i
                x.L.f = obs.L.f
                x.cn0 = obs.cn0
                x.lock = obs.lock
                x.sid.sat = obs.sid.sat
                x.sid.band = obs.sid.band
                x.sid.constellation = obs.sid.constellation
                self.obs_msg.obs.append(x)

            if num_packets == self.obs_msg.n_obs:
                # Now use the field to indicate how many observations we have
                self.obs_msg.n_obs = len(self.obs_msg.obs)

                self.pub_obs.publish(self.obs_msg)


    def callback_sbp_base_pos(self, msg, **metadata):

        if self.debug:
            rospy.loginfo("Received SBP_MSG_BASE_POS (Sender: %d): %s" % (msg.sender, repr(msg)))

        if self.send_observations:
            for s in self.obs_senders:
                s.send(msg)

        # publish tf for rtk frame
        if self.publish_utm_rtk_tf:
            if not self.proj:
                self.init_proj((msg.lat, msg.lon))

            E,N = self.proj(msg.lon,msg.lat, inverse=False)

            self.transform.header.stamp = rospy.Time.now()
            self.transform.transform.translation.x = E
            self.transform.transform.translation.y = N
            self.transform.transform.translation.z = -msg.height
            self.tf_br.sendTransform(self.transform)

    def callback_sbp_settings_read_resp(self, msg, **metadata):
        pass


    def callback_sbp_settings_read_by_index_resp(self, msg, **metadata):
        p = msg.setting.split('\0')
        try:
            i = self.piksi_settings_info[p[0]][p[1]]
            p[2] = i['parser'](p[2])
        except:
            pass

        rospy.set_param('~piksi_original_settings/%s/%s' % (p[0],p[1]), p[2])
        self.piksi_settings[p[0]][p[1]] = p[2]
        self.update_dr_param(p[0], p[1], p[2])


        self.settings_index += 1
        self.piksi_framer(MsgSettingsReadByIndexReq(index=self.settings_index))

    def callback_sbp_settings_read_by_index_done(self, msg, **metadata):
        self.set_piksi_settings()
        self.piksi_start()

    def callback_sbp_log(self, msg, **metadata):
        PIKSI_LOG_LEVELS_TO_ROS[msg.level]("Piksi LOG: %s" % msg.text)

    def check_timeouts(self):
        if time.time() - self.last_rtk_time > self.rtk_fix_timeout:
            if time.time() - self.last_spp_time > self.spp_fix_timeout:
                self.fix_mode = -1
            else:
                self.fix_mode = 0
        else:
            self.fix_mode = self.rtk_fix_mode

    def diag(self, stat):
        fix_mode = self.fix_mode
        num_sats = 0
        last_pos = None

        if self.last_rtk_pos is not None:
            last_pos = self.last_rtk_pos
        elif self.last_pos is not None:
            last_pos = self.last_pos

        if last_pos:
            stat.add("GPS latitude", last_pos.lat)
            stat.add("GPS longitude", last_pos.lon)
            stat.add("GPS altitude", last_pos.height)
            stat.add("GPS #sats", last_pos.n_sats)
            num_sats = last_pos.n_sats

            stat.add("Height mode", HEIGHT_MODE[(last_pos.n_sats & 0x04) >> 2])
            stat.add("RAIM availability", RAIM_AVAILABILITY[(last_pos.n_sats & 0x08) >> 3])
            stat.add("RAIM repair", RAIM_REPAIR[(last_pos.n_sats & 0x10) >> 4])

        stat.add("Fix mode", FIX_MODE[self.fix_mode])

        if self.last_vel:
            stat.add("Velocity N", self.last_vel.n * 1E-3)
            stat.add("Velocity E", self.last_vel.e * 1E-3)
            stat.add("Velocity D", self.last_vel.d * 1E-3)
            stat.add("Velocity #sats", self.last_vel.n_sats)

        if self.last_baseline:
            stat.add("Baseline N", self.last_baseline.n * 1E-3)
            stat.add("Baseline E", self.last_baseline.e * 1E-3)
            stat.add("Baseline D", self.last_baseline.d * 1E-3)
            stat.add("Baseline #sats", self.last_baseline.n_sats)
            stat.add("Baseline fix mode", FIX_MODE[self.last_baseline.flags & 0x03])

        if self.last_dops:
            stat.add("GDOP", self.last_dops.gdop * 1E-2)
            stat.add("PDOP", self.last_dops.pdop * 1E-2)
            stat.add("TDOP", self.last_dops.tdop * 1E-2)
            stat.add("HDOP", self.last_dops.hdop * 1E-2)
            stat.add("VDOP", self.last_dops.vdop * 1E-2)

        if not self.piksi:
            stat.summary(DiagnosticStatus.ERROR, "Piksi not connected")
        elif fix_mode < 0:
            stat.summary(DiagnosticStatus.ERROR, "No fix")
        elif fix_mode < 1:
            stat.summary(DiagnosticStatus.WARN, "No RTK fix")
        elif num_sats < 5:
            stat.summary(DiagnosticStatus.WARN, "Low number of satellites in view")
        else:
            stat.summary(DiagnosticStatus.OK, "Piksi connected")

    def spin(self):
        reconnect_delay = 1.0
        while not rospy.is_shutdown():
            try:
                rospy.loginfo("Connecting to SwiftNav Piksi on port %s" % self.piksi_port)
                self.connect_piksi()

                while not rospy.is_shutdown():
                    rospy.sleep(0.05)
                    if not self.piksi.is_alive():
                        raise IOError
                    self.diag_updater.update()
                    self.check_timeouts()

                break # should only happen if rospy is trying to shut down
            except IOError as e:
                rospy.logerr("IOError")
                self.disconnect_piksi()
            except SystemExit as e:
                rospy.logerr("Unable to connect to Piksi on port %s" % self.piksi_port)
                self.disconnect_piksi()
            except: # catch *all* exceptions
                e = sys.exc_info()[0]
                rospy.logerr("Uncaught error: %s" % repr(e))
                self.disconnect_piksi()
            rospy.loginfo("Attempting to reconnect in %fs" % reconnect_delay)
            rospy.sleep(reconnect_delay)