while not done:
            try:
                if calibrating:
                    print("[{0}] Setting up calibration handlers".format(self.name))

                    self.c.sensorfusion.notifications()
                    time.sleep(0.5)

                    self.c.accelerometer.set_settings(data_rate=50, data_range=4.0)
                    self.c.gyroscope.set_settings(data_rate=50.0, data_range=1000.0)
                    self.c.accelerometer.notifications(self.handle_acc)
                    self.c.gyroscope.notifications(self.handle_gyro)
                    # self.c.sensorfusion.notifications(corrected_acc_callback=self.handle_acc,
                    #                                 corrected_gyro_callback=self.handle_gyro)
                    time.sleep(0.5)
                else:
                    print("[{0}] Setting up sensor fusion handlers".format(self.name))


                    print("[{0}] Setting up sensor fusion handlers (remove acc/gyro notif)".format(self.name))
                    self.c.accelerometer.notifications()
                    self.c.gyroscope.notifications()
                    time.sleep(0.5)

                    print("[{0}] Setting up sensor fusion handlers (accgyro mode)".format(self.name))
                    self.c.sensorfusion.set_mode(SensorFusionMode.IMU_PLUS)
                    self.c.sensorfusion.set_acc_range(SensorFusionAccRange._4G)
                    self.c.sensorfusion.set_gyro_range(SensorFusionGyroRange._500DPS)

                    print("[{0}] Setting up sensor fusion handlers (set_sample_delay)".format(self.name))
                    self.last_quat_epoch = int(time.time()*1000)-1
                    # self.c.sensorfusion.set_sample_delay(SensorFusionData.QUATERION, 20)
                    print("[{0}] Setting up sensor fusion handlers (notif)".format(self.name))
                    self.c.sensorfusion.notifications(quaternion_callback=self.handle_quat)

                    time.sleep(0.5)
                done = True
            except Exception, e:
                print("[{0}] Could not setup handlers ({1}), trying to reconnect".format(self.name, e))
                self.c.disconnect()
                self.c.connect()