Guest User

Untitled

a guest
Sep 6th, 2017
126
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.         while not done:
  2.             try:
  3.                 if calibrating:
  4.                     print("[{0}] Setting up calibration handlers".format(self.name))
  5.  
  6.                     self.c.sensorfusion.notifications()
  7.                     time.sleep(0.5)
  8.  
  9.                     self.c.accelerometer.set_settings(data_rate=50, data_range=4.0)
  10.                     self.c.gyroscope.set_settings(data_rate=50.0, data_range=1000.0)
  11.                     self.c.accelerometer.notifications(self.handle_acc)
  12.                     self.c.gyroscope.notifications(self.handle_gyro)
  13.                     # self.c.sensorfusion.notifications(corrected_acc_callback=self.handle_acc,
  14.                     #                                 corrected_gyro_callback=self.handle_gyro)
  15.                     time.sleep(0.5)
  16.                 else:
  17.                     print("[{0}] Setting up sensor fusion handlers".format(self.name))
  18.  
  19.  
  20.                     print("[{0}] Setting up sensor fusion handlers (remove acc/gyro notif)".format(self.name))
  21.                     self.c.accelerometer.notifications()
  22.                     self.c.gyroscope.notifications()
  23.                     time.sleep(0.5)
  24.  
  25.                     print("[{0}] Setting up sensor fusion handlers (accgyro mode)".format(self.name))
  26.                     self.c.sensorfusion.set_mode(SensorFusionMode.IMU_PLUS)
  27.                     self.c.sensorfusion.set_acc_range(SensorFusionAccRange._4G)
  28.                     self.c.sensorfusion.set_gyro_range(SensorFusionGyroRange._500DPS)
  29.  
  30.                     print("[{0}] Setting up sensor fusion handlers (set_sample_delay)".format(self.name))
  31.                     self.last_quat_epoch = int(time.time()*1000)-1
  32.                     # self.c.sensorfusion.set_sample_delay(SensorFusionData.QUATERION, 20)
  33.                     print("[{0}] Setting up sensor fusion handlers (notif)".format(self.name))
  34.                     self.c.sensorfusion.notifications(quaternion_callback=self.handle_quat)
  35.  
  36.                     time.sleep(0.5)
  37.                 done = True
  38.             except Exception, e:
  39.                 print("[{0}] Could not setup handlers ({1}), trying to reconnect".format(self.name, e))
  40.                 self.c.disconnect()
  41.                 self.c.connect()
RAW Paste Data