Skip to content

Instantly share code, notes, and snippets.

@gmsanchez
Last active January 18, 2018 14:45
Show Gist options
  • Save gmsanchez/1145c42ae6a20d2867bec42cadc2e636 to your computer and use it in GitHub Desktop.
Save gmsanchez/1145c42ae6a20d2867bec42cadc2e636 to your computer and use it in GitHub Desktop.
Python example for Navio2 GPS and IMU
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))
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment