def connect(self, copter_id):
     print("[INTF] Connecting to ", copter_id)
     self.logger.info("using cache dir: {0}".format(CopterConfigs.ro_cache))
     # self.crazyflie = Crazyflie(ro_cache=CopterConfigs.ro_cache, rw_cache=CopterConfigs.rw_cache)
     self.crazyflie = Crazyflie()  # rw_cache=CopterConfigs.rw_cache)
     self.crazyflie.open_link(copter_id)
     self._add_callback_for_connection()
class CopterInterface():
    def __init__(self):
        cflib.crtp.init_drivers()
        self.crazyflie = None
        self.copter_commander = CopterCommander()
        # optional
        self.connection_callbacks = [self._create_log_packet]
        self.close_callbacks = []
        self.log_configs = []
        self.logger = CopterLogger.get_logger(__name__)

    def configure(self, auto_pilot=False, imu_logger=None):
        if auto_pilot:
            self.imu_logger = imu_logger
            self._auto_pilot()
        else:
            self._initialize_gui_event_handler()
        print("initialized CopterInterface.")

    def get_radio_interfaces(self):
        available_interfaces = cflib.crtp.scan_interfaces()
        for i in available_interfaces:
            print("[INTF] Interface with URI {0} found and name/comment {1}".format(i[0], i[1]))
        return [interface for interface in available_interfaces if "radio" in interface[0]]

    def get_first_copter(self, duration=CopterConfigs.connection_wait_time):
        radio_interfaces = None
        while not radio_interfaces and duration > 0:
            time.sleep(0.5)
            radio_interfaces = self.get_radio_interfaces()
            if radio_interfaces: return radio_interfaces[0][0]
            duration -= 0.5

    def connect(self, copter_id):
        print("[INTF] Connecting to ", copter_id)
        self.logger.info("using cache dir: {0}".format(CopterConfigs.ro_cache))
        # self.crazyflie = Crazyflie(ro_cache=CopterConfigs.ro_cache, rw_cache=CopterConfigs.rw_cache)
        self.crazyflie = Crazyflie()  # rw_cache=CopterConfigs.rw_cache)
        self.crazyflie.open_link(copter_id)
        self._add_callback_for_connection()

    def close(self):
        print("[INTF] cleanup...")
        if self.crazyflie:
            self.crazyflie.close_link()
        for callbacks in self.close_callbacks:
            callbacks()

    def _callbacks_after_connection(self):
        for callbacks in self.connection_callbacks:
            callbacks()

    def _disconnected(self, uri):
        print("disconnected :some error occured callabck" + uri)

    def _lost(self, uri, msg):
        print("lost :some error occured callabck" + uri + msg)

    def _failed(self, uri):
        print("failed : some error occured callabck" + uri)

    def _add_callback_for_connection(self):
        self.crazyflie.connected.add_callback(self._link_commander_to_copter)
        self.crazyflie.disconnected.add_callback(self._disconnected)
        self.crazyflie.connection_failed.add_callback(self._failed)
        self.crazyflie.connection_lost.add_callback(self._lost)

    def test_flight(self):
        while not self.copter_commander.is_commander_link_set():
            print("In Test Flight Mode ...")
            time.sleep(0.3)
        Utils.test_flight_for_short_duration(CopterConfigs.TEST_FLIGHT_TIME, self.crazyflie,
                                             CopterControlParams(thrust=25000))
        print("Test Flight Success.")

    def _link_commander_to_copter(self, link):
        print("[INTF] link to copter commander successful.", link)
        self.copter_commander.set_commander(self.crazyflie.commander)
        self.test_flight()
        self._callbacks_after_connection()

    def _initialize_gui_event_handler(self):
        gui_manager = GUI_manager(self.copter_commander)
        gui_manager.handle_events()

    def add_connected_callbacks(self, callback):
        self.connection_callbacks.append(callback)

    def add_close_callbacks(self, callback):
        self.close_callbacks.append(callback)

    def add_log_configs_dicts(self, log_conf):
        self.log_configs += log_conf

    def _create_log_packet(self):
        print("In create log packet {0}".format(len(self.log_configs)))

        for lc in self.log_configs:
            print("log configs: {0}".format(lc))
            log_conf = lc['conf']
            self.crazyflie.log.add_config(log_conf)
            if log_conf.valid:
                log_conf.data_received_cb.add_callback(lc['handler'])
                log_conf.error_cb.add_callback(self._log_error_logconfig)
                log_conf.start()
                print("loggers setup in create log packet...")
            else:
                print("acc.x/y/z not found in log TOC or its invalid")
        self._test_toc()

    def _log_error_logconfig(self, logconf, msg):
        print("Error in logconfig " + logconf.name)
        self.logger.error("Error when logging {0}: {1}".format(logconf.name, msg))

    def _test_toc(self):
        toc = self.crazyflie.log.toc
        self.logger.info("toc: ", toc)
        for group in toc.toc.keys():
            for param in toc.toc[group].keys():
                toc_element = toc.toc[group][param]
                self.logger.info("name=%s.%s, index=%d, pytype=%s, ctype=%s" %
                                 (group, param, toc_element.ident, toc_element.pytype,
                                  toc_element.ctype))

    def _auto_pilot(self):
        self.auto_pilot = AutoPilot(self.copter_commander)
        self.imu_logger.add_log_listener(self.auto_pilot)
        self.auto_pilot.fly()