Python example for Navio2 GPS and IMU

GPS_AGM.py

import navio.util
import navio.ublox

import spidev
import time
import argparse
import sys
import struct

if __name__ == "__main__":

    # ubl = navio.ublox.UBlox("spi:0.0", baudrate=5000000, timeout=2)
    ubl = navio.ublox.UBlox("spi:0.0", baudrate=200000, timeout=2)

    # ubl.module_reset(0,2)
    ubl.configure_poll_port()
    ubl.configure_poll(navio.ublox.CLASS_CFG, navio.ublox.MSG_CFG_USB)
    #ubl.configure_poll(navio.ublox.CLASS_MON, navio.ublox.MSG_MON_HW)

    ubl.configure_port(port=navio.ublox.PORT_SERIAL1, inMask=1, outMask=0)
    ubl.configure_port(port=navio.ublox.PORT_USB, inMask=1, outMask=1)
    ubl.configure_port(port=navio.ublox.PORT_SERIAL2, inMask=1, outMask=0)
    ubl.configure_poll_port()
    ubl.configure_poll_port(navio.ublox.PORT_SERIAL1)
    ubl.configure_poll_port(navio.ublox.PORT_SERIAL2)
    ubl.configure_poll_port(navio.ublox.PORT_USB)
    payload = struct.pack('<BBHHBB', 0, 0, 0, 0, 0, 0)
    ubl.send_message(navio.ublox.CLASS_CFG, 0x86, payload)

    ubl.configure_solution_rate(rate_ms=100, nav_rate=1)

    ubl.set_preferred_dynamic_model(None)
    ubl.set_preferred_usePPP(None)

    ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_POSLLH, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_STATUS, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_PVT, 1)
    ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_SOL, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_VELNED, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_SVINFO, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_VELECEF, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_POSECEF, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_RAW, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_SFRB, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_SVSI, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_ALM, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_EPH, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_TIMEGPS, 0)
    ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_CLOCK, 0)
    #ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_DGPS, 5

    imu = navio.lsm9ds1.LSM9DS1()

    if imu.testConnection():
        print "IMU Connection established: True"
    else:
        sys.exit("IMU Connection established: False")

    imu.initialize()

    time.sleep(1)

    prev_time = 0
    pi_t0 = 0
    while True:
        msg = ubl.receive_message()
        if msg is None:
            if opts.reopen:
                ubl.close()
                ubl = navio.ublox.UBlox("spi:0.0", baudrate=200000, timeout=2)
                continue
            print(empty)
            break
        #print(msg.name())
        if msg.name() == "NAV_PVT":
            pi_t1 = time.time()
            outstr = str(msg).split(",")[0]
            outstr0 = "".join(outstr)
            act_time = int(outstr0.split("=")[1])
            outstr = str(msg).split(",")[10]
            outstr1 = "".join(outstr)
            outstr = str(msg).split(",")[13]
            outstr = "".join(outstr)
            print(outstr0 + outstr1 + outstr + " Elapsed time: " + str(act_time-prev_time))
            prev_time = act_time
            m9a, m9g, m9m = imu.getMotion9()
            print "Acc:", "{:+7.3f}".format(m9a[0]), "{:+7.3f}".format(m9a[1]), "{:+7.3f}".format(m9a[2]),
            print " Gyr:", "{:+8.3f}".format(m9g[0]), "{:+8.3f}".format(m9g[1]), "{:+8.3f}".format(m9g[2]),
            print " Mag:", "{:+7.3f}".format(m9m[0]), "{:+7.3f}".format(m9m[1]), "{:+7.3f}".format(m9m[2])
            print "RPi elapsed time: ", pi_t1 - pi_t0
            pi_t0 = pi_t1
            time.sleep(0.000001)	
        #if msg.name() == "NAV_STATUS":
        #    outstr = str(msg).split(",")[0:2]
        #    outstr = "".join(outstr)
        #    print(outstr)
        #print(str(msg))