Exemplo n.º 1
0
count = shm.hydrophones_results.ping_count.get()

while 1:
  # rpos = [shm.kalman.north.get(), shm.kalman.east.get(), shm.kalman.depth.get()]
  rpos = [0., 0., 0.]
  pos = vectorr([realr(x) for x in rpos])
  n_count = shm.hydrophones_results.ping_count.get()

  if n_count > count or 1:
    n_count = count

    period, elevation, h = shm.hydrophones_results.ping_time.get(), shm.hydrophones_results.elevation.get(), heading()

    h = h + random.random() * 60 + time.time()
    h = t(h)

    #if period < MIN_PERIOD or period > MAX_PERIOD or elevation > MAX_ELEVATION:
      #print('Ignoring ping: period {0}, elevation {1}, heading {2}.'.format(period, elevation, h))
      #continue  

    slam.observe('heading', {'x': pactual(pos), 'y': pobject('octagon_1')}, realr(math.radians(h)), 0.5)
    octpos = [x['realr'] for x in slam.estimate('octagon_1')['estimated']['vectorr']]
    heading_recalc = math.degrees(math.atan2(octpos[1] - rpos[1], octpos[0] - rpos[0]))
    
    shm.hydrophones_filtered.heading.set(heading_recalc)
    shm.hydrophones_filtered.count.set(shm.hydrophones_filtered.count.get() + 1)
    # shm.hydrophones_filtered.elevation.set(elevation)

    print('Heading: {0} OctPos: {1} Recalc Heading: {2}'.format(h, octpos, heading_recalc))
Exemplo n.º 2
0
#!/usr/bin/env python3.4

from model import slam, objects

import shm

while 1:
  s = shm.slam.get()

  for obj in objects:
    pos = [x['realr'] for x in slam.estimate(obj)['estimated']['vectorr']]
    setattr(s, obj + '_n', pos[0])
    setattr(s, obj + '_e', pos[1])
    setattr(s, obj + '_d', pos[2])

  shm.slam.set(s)
  
  print('SHM updated!')
Exemplo n.º 3
0
#!/usr/bin/env python3.4

from model import slam, objects

import shm

while 1:
    s = shm.slam.get()

    for obj in objects:
        pos = [x['realr'] for x in slam.estimate(obj)['estimated']['vectorr']]
        setattr(s, obj + '_n', pos[0])
        setattr(s, obj + '_e', pos[1])
        setattr(s, obj + '_d', pos[2])

    shm.slam.set(s)

    print('SHM updated!')