diff --git a/cflib/crazyflie/mem.py b/cflib/crazyflie/mem.py index 5321055c026ed1e0d987a7acffba3a065bc36ef1..071b3f0f129ceb98b9df0ea632f58e52d8bc893f 100644 --- a/cflib/crazyflie/mem.py +++ b/cflib/crazyflie/mem.py @@ -671,6 +671,7 @@ class Memory(): self.cf = crazyflie self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb) + self.cf.disconnected.add_callback(self._disconnected) self._refresh_callback = None self._fetch_id = 0 @@ -783,6 +784,15 @@ class Memory(): pk.data = (CMD_INFO_NBR,) self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,)) + def _disconnected(self, uri): + """The link to the Crazyflie has been broken. Reset state""" + for m in self.mems: + try: + m.disconnect() + except Exception as e: + logger.info( + 'Error when resetting after disconnect: {}'.format(e)) + def _new_packet_cb(self, packet): """Callback for newly arrived packets for the memory port""" chan = packet.channel