class CommandCenter: def __init__(self, portName): self.portName = portName self.serialLink = SerialPort(self.portName) #binaryCommand = self.__getWirelessBinaryCommand(command, sensorId) #self.__sendDataToSerialPort(binaryCommand) #return self.__getFormatedDataFromSerialPort(lengthOfPacket=19,format) def setStreamingTiming (self,interval,duration,delay, sensorId): commandData = [0.0] * 3 commandData[0] = interval commandData[1] = duration commandData[2] = delay data = self.__getWirelessBinaryCommand(Commands.SET_STREAMING_TIMING,sensorId,commandData=commandData,format=">III") self.__retryIfSetCommandFailed(data,"setStreamingTiming", sensorId=sensorId) def performOffsetOperation(self,sensorId): #we are trying to find offset here by getting gravity vector of device and gravity vector that we want it as northAndGravityVectors = self.__getNorthAndGravityVectorsInSensorFrame(sensorId) untaredOrientationAsQuaternion = self.__getUntaredOrientationAsQuaternion(sensorId,needToReturnSensorId=False) vectorAndQuaternionCalculator = VectorAndQuaternionCalculator() offsetAsQuaternion = vectorAndQuaternionCalculator.getOffsetOrientationAsQuaternion(northAndGravityVectors) #below two statements is to offset gd so that it is align with g taredData = vectorAndQuaternionCalculator.getMultiplicationOfQuaternion(untaredOrientationAsQuaternion,offsetAsQuaternion) self.__setTareOrientationAsSameAsSuppliedOrientationInQuaternionDomain(sensorId,tareData=taredData) return offsetAsQuaternion def getAngleBetweenThreeVectors(self, vectorOne,vectorTwo,vectorthree): vectorAndQuaternionCalculator = VectorAndQuaternionCalculator() return vectorAndQuaternionCalculator.calculateAngle(vectorOne,vectorTwo,vectorthree) def getDeviceVector(self,sensorId, suppliedVector, offsetAsQuaternion): taredOrientationAsQuaternion = self.__getTaredOrientationAsQuaternion(sensorId,needToReturnSensorId=False) #print "taredOrientationAsQuaternion" #print taredOrientationAsQuaternion vectorAndQuaternionCalculator = VectorAndQuaternionCalculator() vector = vectorAndQuaternionCalculator.getVectorOftheDeviceBasedOnSuppliedOrientation(suppliedVector,taredOrientationAsQuaternion,offsetAsQuaternion) return vector def calculateDeviceVector(self,sensorId,vector,offset): taredOrientationAsQuaternion = self.__getTaredOrientationAsQuaternion(sensorId) ## Apply the offset for the device quat = self.__quaternionMultiplication(taredOrientationAsQuaternion, offset) ## Calculate a vector for the device with its orientation vector = self.__quaternionVectorMultiplication(quat, vector) return vector def getThisData(self): numberOfByteAvaialbe = self.serialLink.getNumberOfByteAvailable() if numberOfByteAvaialbe>18: receivedData = self.serialLink.readData(numberOfByteAvaialbe) if (numberOfByteAvaialbe == 19): formatedData = self.__getFormatedData(receivedData,format=">ffff") return formatedData EmptyList = list() return EmptyList def getTaredOridntationAsQuaternionFromBuffer(self, sensorId): print (self.serialLink.getNumberOfByteAvailable()) def getSerialBuffer(self): print("the buffer is") print ("available number of Byte") print (self.serialLink.getNumberOfByteAvailable()) print repr(self.serialLink.readData(self.serialLink.getNumberOfByteAvailable())) print ("available number of Byte after read") print (self.serialLink.getNumberOfByteAvailable()) def closeSerialPort(self): self.serialLink.clostPort() def setStreamingSlot(self,sensorId, slot1, slot2 = chr(0xff),slot3 = chr(0xff), slot4= chr(0xff), slot5= chr(0xff), slot6= chr(0xff),slot7= chr(0xff),slot8= chr(0xff)): commandData = [0.0] * 8 commandData[0] = slot1 commandData[1] = slot2 commandData[2] = slot3 commandData[3] = slot4 commandData[4] = slot5 commandData[5] = slot6 commandData[6] = slot7 commandData[7] = slot8 data = self.__getWirelessBinaryCommand(Commands.SET_STREAMING_SLOTS,sensorId, commandData=commandData,format='cccccccc') self.__retryIfSetCommandFailed(data,"setStreamingSlots", sensorId=sensorId) def startStreaming(self, sensorId): binaryCommand = self.__getWirelessBinaryCommand(Commands.START_STREAMING, sensorId) self.__sendDataToSerialPort(binaryCommand) self.__retryIfSetCommandFailed(binaryCommand,"StartStreaming", sensorId=sensorId) def stopStreaming(self,sensorId): binaryCommand = self.__getWirelessBinaryCommand(Commands.STOP_STREAMING, sensorId) self.__sendDataToSerialPort(binaryCommand) self.__retryIfSetCommandFailed(binaryCommand,"StopStreaming", sensorId=sensorId) def getStreamingTiming(self,sensorId): binaryCommand = self.__getWirelessBinaryCommand(Commands.GET_STREAMING_TIMING, sensorId) self.__sendDataToSerialPort(binaryCommand) return self.__getFormatedDataFromSerialPort(lengthOfPacket=15,format="III") ######################################################################################################################## #Private function def __getTaredOrientationAsQuaternion(self, sensorId,needToReturnSensorId = True): command = Commands.getCommandAsChr(Commands.GET_TARED_ORIENTATION_AS_QUATERNION) format = Commands.getRetrunDataFormat(Commands.GET_TARED_ORIENTATION_AS_QUATERNION) lengthOfReturnData = Commands.getReturnDataLength(Commands.GET_TARED_ORIENTATION_AS_QUATERNION) binaryCommand = self.__getWirelessBinaryCommand(command, sensorId) receivedData = self.__getRawDataAndRetryIfGetCommandFailed(sensorId,binaryCommand,lengthOfReturnData) if needToReturnSensorId is True: return (self.__getFormatedData(receivedData,format)) else: return (self.__getFormatedDataWithoutSensorId(receivedData,format)) def __setTareOrientationAsSameAsSuppliedOrientationInQuaternionDomain(self, sensorId, tareData): command = Commands.getCommandAsChr(Commands.TARE_WITH_QUATERNION) dataFormat = Commands.getDataFormat(Commands.TARE_WITH_QUATERNION) binaryCommand = self.__getWirelessBinaryCommand(command, sensorId, tareData, dataFormat) self.__retryIfSetCommandFailed(binaryCommand,"tareWithQuaternion", sensorId=sensorId) def __getUntaredOrientationAsQuaternion(self, sensorId = 0,needToReturnSensorId = True): command = Commands.getCommandAsChr(Commands.GET_UNTARED_ORIENTATION_AS_QUATERNION) lengthOfReturnData = Commands.getReturnDataLength(Commands.GET_UNTARED_ORIENTATION_AS_QUATERNION) format = Commands.getRetrunDataFormat(Commands.GET_UNTARED_ORIENTATION_AS_QUATERNION) binaryCommand = self.__getWirelessBinaryCommand(command, sensorId) receivedData = self.__getRawDataAndRetryIfGetCommandFailed(sensorId,binaryCommand,lengthOfReturnData) if needToReturnSensorId is True: return (self.__getFormatedData(receivedData,format)) else: #Note: starting byte is sensorId followed by data, need to remove index 0 if you dont want to get sensorId as return data return (self.__getFormatedData(receivedData,format))[1:] def __getFormatedDataWithoutSensorId(self,data, format): actualData = data[3:len(data)] dataToBeReturned = list(struct.unpack(format, actualData)) return dataToBeReturned def __getNorthAndGravityVectorsInSensorFrame(self, sensorId): command = Commands.getCommandAsChr(Commands.GET_NORTH_AND_GRAVITY_VECTORS_IN_SENSOR_FRAME) lengthOfReturnData = Commands.getReturnDataLength(Commands.GET_NORTH_AND_GRAVITY_VECTORS_IN_SENSOR_FRAME) format = Commands.getRetrunDataFormat(Commands.GET_NORTH_AND_GRAVITY_VECTORS_IN_SENSOR_FRAME) binaryCommand = self.__getWirelessBinaryCommand(command, sensorId) receivedData = self.__getRawDataAndRetryIfGetCommandFailed(sensorId,binaryCommand,lengthOfReturnData) return (self.__getFormatedData(receivedData,format)) def __getRawDataAndRetryIfGetCommandFailed(self,sensorId,binaryCommand, returnDataLength): WIRELESS_HEADER_LENGTH = 3 SUCESSFULLY_SENT_BYTE = chr(0x00) TOTAL_PACKET_LENGTH = WIRELESS_HEADER_LENGTH + returnDataLength self.__sendDataToSerialPort(binaryCommand) receivedData = (self.serialLink.readData(TOTAL_PACKET_LENGTH)) while receivedData[0] != SUCESSFULLY_SENT_BYTE and receivedData[1] != sensorId and receivedData != returnDataLength: print "send to sensor Id" + str(sensorId) + "fail" self.serialLink.flushBuffer() self.__sendDataToSerialPort(binaryCommand) receivedData = (self.serialLink.readData(TOTAL_PACKET_LENGTH)) return receivedData def __getFormatedData(self, data, format): actualData = data[3:len(data)] sensorId = data[1] dataToBeReturned = list(struct.unpack(format, actualData)) dataToBeReturned.insert(0,ord(sensorId)) return dataToBeReturned def __getSensorId(self,data): return data[1] def __flushBuffer(self): self.serialLink.readData(self.serialLink.getNumberOfByteAvailable()) self.serialLink.flushBuffer() def __retryIfSetCommandFailed(self, data, nameOfCommand, sensorId): self.__sendDataToSerialPort(data) while self.__isCommandSuccessful(sensorId) == False: self.__flushBuffer() print ("going to send again") self.__sendDataToSerialPort(data) print (nameOfCommand + " command is successfully sent to sensor ID: " + str(sensorId)) def __isCommandSuccessful(self, sensorId): print(self.serialLink.getNumberOfByteAvailable()) numberOfbyte = self.serialLink.getNumberOfByteAvailable() print (numberOfbyte) receivedData = (self.serialLink.readData(3)) if ((receivedData[0] == chr(0x00))and(receivedData[1]==chr(int(sensorId)))): #print ("command successful" + repr(receivedData)) return True else: #print ("command failed" + repr(receivedData)) return False def __getFormatedDataFromSerialPort(self, lengthOfPacket, format): receivedData = (self.serialLink.readData(lengthOfPacket)) if receivedData[0] == chr(0x00): print "successful" actualData = receivedData[3:lengthOfPacket] outputData = list(struct.unpack(format, actualData)) return outputData else: print("data not avaialbe yet" + repr(receivedData)) return None def __sendDataToSerialPort(self, data): self.serialLink.sendData(data) def __getWirelessBinaryCommand(self, command, sensorId, commandData=[], format=""): sensorId = chr(int(sensorId)) if len(commandData) > 0: packedData = struct.pack(format, *commandData) sendingData = Commands.WIRELESS_STARTING_BYTE + sensorId + command + packedData + self.__getCheckSum( sensorId + command + packedData) return sendingData else: sendingData = Commands.WIRELESS_STARTING_BYTE + sensorId + command + self.__getCheckSum( sensorId + command) return sendingData def __getCheckSum(self, char_data): """ Calculates the checksum for the given data. Args: char_data: A string of data. """ checksum = 0 for byte in char_data: checksum += ord(byte) return chr(checksum % 256)