def run(self):
myok = self.ok
while myok:
if self.tick_chrono is None:
self.tick_chrono = chrono.Chrono()
self.dt = None
else:
self.dt = self.tick_chrono.measure()
self.mutex.acquire()
self.alive = True
self.update_imu()
self.calib_tick()
if self.dt is not None: self.update_orientation()
self.mutex.release()
time.sleep(0.001)
self.mutex.acquire()
myok = self.ok
self.count += 1
if self.count % 100 == 0:
delta_t = self.chrono.measure()
period = delta_t / 100
self.frequency = 1.0 / period
self.mutex.release()
self.mutex.acquire()
self.alive = False
self.mutex.release()
def __init__(self, robot):
threading.Thread.__init__(self)
self.mutex = threading.Lock()
self.robot = robot
self.alive = False
self.accelero = np.array([0, 0, 0],
dtype=np.float32) # acceleration in g
self.gyro = np.array([0, 0, 0], dtype=np.float32) # gyro in deg/sec
self.gyro_zero = None
self.magneto = np.array([0, 0, 0], dtype=np.float32)
self.orientation = 0.0
self.imu_cycle = None
self.count = 0
self.frequency = None # frequency in Hz
self.chrono = chrono.Chrono()
self.tick_chrono = None
self.dt = None
self.ok = True
self.calibration = False
self.calib_chrono = None
self.calib_idx = 0
self.calib_duration = None
def load_chrono(self):
chrono_directory_list = [
os.path.relpath(x, self.directory)
for x in glob.glob(os.path.join(self.directory, "Chrono", "*", ""))
]
for chrono_directory in chrono_directory_list:
self._chronos.append(chrono.Chrono(chrono_directory))
self._chronos[-1].load(self.directory, self._global_config._map)
def calib(self, duration=3):
if self.calibration: return
self.mutex.acquire()
self.calibration = True
self.calib_idx = 0
self.calib_chrono = chrono.Chrono()
self.calib_duration = duration
self.gyro_zero = np.array([0.0, 0.0, 0.0], dtype=np.float32)
self.mutex.release()
def return_to_chrono(self):
self.new_window = chrono.Chrono()
self.new_window.show()
self.setVisible(False)
self.close()
def start_chrono(self):
self.new_window = chrono.Chrono()
self.close()
