""" main loop """
timestamp = None
try:
while commander == None or commander.is_alive():
""" calculate time since last iteration """
prev_timestamp = timestamp
timestamp = time()
time_delta = timestamp - prev_timestamp if prev_timestamp else 0
""" get data """
image = drone.wait_for_data(keep_alive = lambda: commander == None or commander.is_alive())
if not image: # timeout
continue
simpledata = {}
for var in SIMPLEDATA_VARS:
simpledata[var] = drone.getFromNavdata(var)
# transform vx and vy
vnav = array([simpledata['vx'], simpledata['vy']])
psi = simpledata['psi'] / 180.0 * pi
R = matrix([
[cos(psi), -sin(psi)],
[sin(psi), cos(psi)]
])
v = matrix(vnav) * R.T
simpledata['vx'], simpledata['vy'] = array(v)[0]
# add v to pos-estimation
x += simpledata['vx'] * time_delta # (conversion from mm/s to mm)
y += simpledata['vy'] * time_delta
z += simpledata['vz'] * time_delta
simpledata['x'] = x
simpledata['y'] = y
""" main loop """
timestamp = None
try:
while commander == None or commander.is_alive():
""" calculate time since last iteration """
prev_timestamp = timestamp
timestamp = time()
time_delta = timestamp - prev_timestamp if prev_timestamp else 0
""" get data """
image = drone.wait_for_data(
keep_alive=lambda: commander == None or commander.is_alive())
if not image: # timeout
continue
simpledata = {}
for var in SIMPLEDATA_VARS:
simpledata[var] = drone.getFromNavdata(var)
# transform vx and vy
vnav = array([simpledata['vx'], simpledata['vy']])
psi = simpledata['psi'] / 180.0 * pi
R = matrix([[cos(psi), -sin(psi)], [sin(psi), cos(psi)]])
v = matrix(vnav) * R.T
simpledata['vx'], simpledata['vy'] = array(v)[0]
# add v to pos-estimation
x += simpledata['vx'] * time_delta # (conversion from mm/s to mm)
y += simpledata['vy'] * time_delta
z += simpledata['vz'] * time_delta
simpledata['x'] = x
simpledata['y'] = y
simpledata['z'] = z
simpledata['timestamp'] = timestamp
simpledata[
