Advertisement
Guest User

Python iOS dead reckoning

a guest
Aug 23rd, 2014
200
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 2.61 KB | None | 0 0
  1. def navigate():
  2.     import _motion as motion
  3.     from numpy import zeros, divide
  4.     from math import cos
  5.     from operator import add, sub
  6.     from console import clear
  7.     m = motion.MotionManager()
  8.     m.stop()
  9.     m.start()
  10.     import time
  11.     print "\nStarting motion update, please wait 20 seconds for the values to stabilize. During this time, please avoid touching the phone"
  12.     """First we start updating, we allow a 5 second time out because there is a significant amount of drift initially present"""
  13.     time.sleep(10)
  14.     print "\nStarting calibration in 10 seconds. Please keep the device UTTERLY still in an position that you expect it to be most commonly in during the measurements later."
  15.     time.sleep(6)
  16.     print "\nCalibration starting in 5 seconds."
  17.     time.sleep(5)
  18.     print "\nCalibration starting..."
  19.     caliacc = zeros(3)
  20.     caliangle = zeros(3)
  21.     cali=1
  22.     for u in xrange(300):
  23.         caliacc = caliacc + m.user_acceleration
  24.         caliangle = caliangle + m.rotation_rate
  25.     zeroingacc= divide(caliacc, 300)
  26.     zeroingang=divide(caliangle, 300)
  27.     print zeroingacc
  28.     time.sleep(10)
  29.     print "\nCalibration ended, you may start to move."
  30.     zeroingacc0=zeroingacc[0]
  31.     zeroingacc1=zeroingacc[1]
  32.     zeroingacc2=zeroingacc[2]
  33.     zeroingang0=zeroingang[0]
  34.     zeroingang1=zeroingang[1]
  35.     zeroingang2=zeroingang[2]
  36.     speed0=0
  37.     speed1=0
  38.     speed2=0
  39.     angle0=0
  40.     angle1=0
  41.     angle2=0
  42.     movement0=0
  43.     movement1=0
  44.     movement2=0
  45.     time1=0.083*9.83
  46.     time2=0.083
  47.     """Apparently for small lists, direct assignment is twice as fast as map and lists! Very unexpected!"""
  48.     for j in xrange(500):
  49.         speed0 += (-zeroingacc0 + m.user_acceleration[0]) * time1
  50.         speed1 += (-zeroingacc1 + m.user_acceleration[1]) * time1
  51.         speed2 += (-zeroingacc2 + m.user_acceleration[2]) * time1
  52.         angle0 += (m.rotation_rate[0]-zeroingang0) * time2
  53.         angle1 += (m.rotation_rate[1]-zeroingang1)* time2
  54.         angle2 += (m.rotation_rate[2]-zeroingang2)* time2
  55.         cos0=cos(angle0)
  56.         cos1=cos(angle1)
  57.         cos2=cos(angle2)
  58.         movement0 = movement0+speed0*time2*cos0*cos2
  59.         movement1 = movement1+speed1*time2*cos1*cos2
  60.         movement2 = movement2+speed2*time2*cos0*cos1
  61.         #speed = map(lambda x, y: time*add(x, y), speed, map(sub, list(m.user_acceleration), zeroingacc))
  62.         #angle = map(add, angle, list(m.rotation_rate))
  63.         #movement = map(lambda x,y: time*add(x,y), movement, map(lambda l, o: l*o, map(cos, angle), speed))
  64.         print movement0, movement1, movement2
  65.         clear()
  66. navigate()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement