class MainProcess(Process): ## Constructor # # Inits the process, the CSV file, the PCA9547 Multiplexor, the settings # file for each sensor (with calibration) # @param self The object pointer def __init__(self): Process.__init__(self) self.exit = Event() self.csv = CSVExport() self.i2cMux = PCA9547() self.settings0 = RTIMU.Settings(SETTINGS_FILE_0) self.settings1 = RTIMU.Settings(SETTINGS_FILE_1) self.settings2 = RTIMU.Settings(SETTINGS_FILE_2) self.settings3 = RTIMU.Settings(SETTINGS_FILE_3) self.settings4 = RTIMU.Settings(SETTINGS_FILE_4) self.settings5 = RTIMU.Settings(SETTINGS_FILE_5) self.imu0 = RTIMU.RTIMU(self.settings0) self.imu1 = RTIMU.RTIMU(self.settings1) self.imu2 = RTIMU.RTIMU(self.settings2) self.imu3 = RTIMU.RTIMU(self.settings3) self.imu4 = RTIMU.RTIMU(self.settings4) self.imu5 = RTIMU.RTIMU(self.settings5) self.settings = [ self.settings0, self.settings1, self.settings2, self.settings3, self.settings4, self.settings5 ] self.imus = [ self.imu0, self.imu1, self.imu2, self.imu3, self.imu4, self.imu5 ] self.detectedIMU = [False] * 6 self.detectImu() self.pollInterval = 2 ## Detectes the number of IMUs avaliable in each channel # # @param self The object pointer # @return number of detected IMUs def detectImu(self): try: for n, imu in enumerate(self.imus): self.i2cMux.setChannel(n) if imu.IMUInit(): self.detectedIMU[n] = True else: self.detectedIMU[n] = False except: print("Error in detection") return self.detectedIMU def setPollInterval(self, pollInterval): self.pollInterval = pollInterval ## Sets file name for the CSV file # # @param self The object pointer # @param txt file name def setFileName(self, txt): self.csv.setTitle(txt) ## gets the current data from the current sensor # # @param self The object pointer # @return all sensor data as a vector def getData(self): return self.allData ## stops the thread and signals to exit # # @param self The object pointer def stop(self): self.exit.set() def saveSettings(self): try: for s in self.setting: s.save() except: print("Error in detection") return self.detectedIMU ## Thread loop # # Get data in the polling interval definid time, and stores # data form all the avaliable sensor with his identificator and timestamp # @param self The object pointer # @return finalized job def run(self): self.csv.createFile() init_time = time.time() while not self.exit.is_set(): try: for n, imu in enumerate(self.imus): if self.detectedIMU[n]: self.i2cMux.setChannel(n) if imu.IMURead(): data0 = imu.getIMUData() quaternion = data0["fusionQPose"] acceleration = data0["accel"] gyro = data0["gyro"] compass = data0["compass"] nowTime = time.time() - init_time self.allData = [n] + \ [nowTime] + \ [acceleration[0]] + \ [acceleration[1]] + \ [acceleration[2]] + \ [gyro[0]] + \ [gyro[1]] + \ [gyro[2]] + \ [compass[0]] + \ [compass[1]] + \ [compass[2]] + \ [quaternion[0]] + \ [quaternion[1]] + \ [quaternion[2]] + \ [quaternion[3]] self.csv.csvWrite(self.allData) #print(self.allData) time.sleep(self.pollInterval * 1.0 / 1000.0) except (KeyboardInterrupt, SystemExit): print("Finishing Thread bad") self.exit.set() print("Finished Adquisition") return