average position

This commit is contained in:
Sebastien Bourdeauducq 2020-08-11 18:35:30 +08:00
parent cb1f1c047e
commit 702ee463dd
1 changed files with 7 additions and 2 deletions

9
dmi.py
View File

@ -35,6 +35,8 @@ def main():
buf_sdr.start()
try:
throttle = 0
throttle_factor = 8
position_acc = 0.0
while True:
buffers = buf_sdr.get()
try:
@ -43,7 +45,7 @@ def main():
# Throttle certain things to avoid overflows due to the limited speed of
# the MHS5200A serial interface and GUI plotting.
throttle += 1
if throttle == 8:
if throttle == throttle_factor:
throttle = 0
if throttle == 0:
@ -52,10 +54,13 @@ def main():
if stabilizer.locked():
position = position_tracker.input(samples_ref, samples_meas)
position_acc += np.sum(position)/len(position)
if throttle == 0:
gui.update_position(np.sum(position)/len(position))
gui.update_position(position_acc/throttle_factor)
position_acc = 0.0
else:
position_tracker.reset()
position_acc = 0.0
finally:
buf_sdr.dispose(buffers)
finally: