def _cf_callback_pos(self, timestamp, data, logconf):
"""callback on the crazyflie's position update"""
x = data['kalman.stateX']
y = data['kalman.stateY']
z = data['kalman.stateZ']
# print("current height: {}".format(z))
# compute a difference between the previou current position and this one
# if there is a large jump, that means there is a chance the estimator has reset!
new_position = np.array([x, y, z])
dpos = new_position - self._current_position_xyz
dx = dpos[0]
dy = dpos[1]
pos_change = math.sqrt(dx * dx + dy * dy)
# DEBUG
# print("position change: ({}, {})".format(dx, dy))
# TODO: find the correct limit here for defining a jump
if pos_change >= 1:
# print("esitmator has reset, adjusting home position")
self._dynamic_home_xyz += np.array([dx, dy, 0])
# print("\tdynamic adjustment = ({}, {})".format(self._dynamic_home_xyz[0], self._dynamic_home_xyz[1]))
self._current_position_xyz = np.array([x, y,
z]) # save for our internal use
# the position that should be published is an NED position, adjusted for the set home position
adjusted_pos = self._current_position_xyz - self._home_position_xyz - self._dynamic_home_xyz
pos = mt.LocalFrameMessage(timestamp, adjusted_pos[0],
-adjusted_pos[1], -adjusted_pos[2])
self.notify_message_listeners(MsgID.LOCAL_POSITION, pos)
def _cf_callback_vel(self, timestamp, data, logconf):
"""callback on the crazyflie's velocity update"""
x = data['kalman.statePX']
y = data['kalman.statePY']
z = data['kalman.statePZ']
vel = mt.LocalFrameMessage(timestamp, x, y, z)
self.notify_message_listeners(MsgID.LOCAL_VELOCITY, vel)
def _cf_callback_pos(self, timestamp, data, logconf):
x = data['kalman.stateX']
y = data['kalman.stateY']
z = data['kalman.stateZ']
# print("current height: {}".format(z))
self._current_position_xyz = [x, y, z] # save for our internal use
pos = mt.LocalFrameMessage(timestamp, x, -y, -z)
self.notify_message_listeners(MsgID.LOCAL_POSITION, pos)