def createRandomMAGDataPacketList(samplingFrequency=50.0, numSamples=300, freq=1.0): packetList = [] mag = [0] * 3 accel = [0] * 3 timestamp = 0 maxMAGValue = 10000 #TBD for n in range(numSamples): mag[0] = maxMAGValue * math.sin( (2 * math.pi * freq * n) / samplingFrequency + (math.pi / 4)) + random.gauss(0, maxMAGValue / 20) mag[1] = maxMAGValue * math.sin( (2 * math.pi * freq * n) / samplingFrequency + (math.pi / 2)) + random.gauss(0, maxMAGValue / 20) mag[2] = maxMAGValue * math.sin( 2 * math.pi * freq * n / samplingFrequency) + random.gauss( 0, maxMAGValue / 20) accel[0] = maxMAGValue * math.cos( (2 * math.pi * freq * n) / samplingFrequency + (math.pi / 3)) + random.gauss(0, maxMAGValue / 20) accel[1] = maxMAGValue * math.cos( (2 * math.pi * freq * n) / samplingFrequency + (7 * math.pi / 12)) + random.gauss(0, maxMAGValue / 20) accel[2] = maxMAGValue * math.cos( (2 * math.pi * freq * n) / samplingFrequency + (9 * math.pi / 15)) + random.gauss(0, maxMAGValue / 20) timestamp += int((1.0 / samplingFrequency) * usPerSecond) packet = nebResponsePacket.createMAGResponsePacket( timestamp, mag, accel) packetList.append(packet) return packetList
def createRandomIMUDataPacketList(samplingFrequency=50.0, numSamples=300, freq=1.0): packetList = [] accel = [0] * 3 gyro = [0] * 3 timestamp = 0 maxIMUValue = 16739 # TBD for n in range(numSamples): accel[0] = maxIMUValue * math.sin((2 * math.pi * freq * n) / samplingFrequency + (math.pi / 4)) + random.gauss( 0, maxIMUValue / 10 ) accel[1] = maxIMUValue * math.sin((2 * math.pi * freq * n) / samplingFrequency + (math.pi / 2)) + random.gauss( 0, maxIMUValue / 10 ) accel[2] = maxIMUValue * math.sin(2 * math.pi * freq * n / samplingFrequency) + random.gauss( 0, maxIMUValue / 10 ) gyro[0] = maxIMUValue * math.cos((2 * math.pi * freq * n) / samplingFrequency + (math.pi / 3)) + random.gauss( 0, maxIMUValue / 10 ) gyro[1] = maxIMUValue * math.cos( (2 * math.pi * freq * n) / samplingFrequency + (7 * math.pi / 12) ) + random.gauss(0, maxIMUValue / 10) gyro[2] = maxIMUValue * math.cos( (2 * math.pi * freq * n) / samplingFrequency + (9 * math.pi / 15) ) + random.gauss(0, maxIMUValue / 10) timestamp += int((1.0 / samplingFrequency) * usPerSecond) packet = nebResponsePacket.createIMUResponsePacket(timestamp, accel, gyro) packetList.append(packet) return packetList
def createRandomIMUDataPacketList(samplingFrequency=50.0, numSamples=300, freq=1.0): packetList = [] accel = [0] * 3 gyro = [0] * 3 timestamp = 0 maxIMUValue = 16739 #TBD for n in range(numSamples): accel[0] = maxIMUValue * math.sin( (2 * math.pi * freq * n) / samplingFrequency + (math.pi / 4)) + random.gauss(0, maxIMUValue / 10) accel[1] = maxIMUValue * math.sin( (2 * math.pi * freq * n) / samplingFrequency + (math.pi / 2)) + random.gauss(0, maxIMUValue / 10) accel[2] = maxIMUValue * math.sin( 2 * math.pi * freq * n / samplingFrequency) + random.gauss( 0, maxIMUValue / 10) gyro[0] = maxIMUValue * math.cos( (2 * math.pi * freq * n) / samplingFrequency + (math.pi / 3)) + random.gauss(0, maxIMUValue / 10) gyro[1] = maxIMUValue * math.cos( (2 * math.pi * freq * n) / samplingFrequency + (7 * math.pi / 12)) + random.gauss(0, maxIMUValue / 10) gyro[2] = maxIMUValue * math.cos( (2 * math.pi * freq * n) / samplingFrequency + (9 * math.pi / 15)) + random.gauss(0, maxIMUValue / 10) timestamp += int((1.0 / samplingFrequency) * usPerSecond) packet = nebResponsePacket.createIMUResponsePacket( timestamp, accel, gyro) packetList.append(packet) return packetList
def createRandomMAGDataPacketList(samplingFrequency=50.0, numSamples=300, freq=1.0): packetList = [] mag = [0] * 3 accel = [0] * 3 timestamp = 0 maxMAGValue = 10000 # TBD for n in range(numSamples): mag[0] = maxMAGValue * math.sin((2 * math.pi * freq * n) / samplingFrequency + (math.pi / 4)) + random.gauss( 0, maxMAGValue / 20 ) mag[1] = maxMAGValue * math.sin((2 * math.pi * freq * n) / samplingFrequency + (math.pi / 2)) + random.gauss( 0, maxMAGValue / 20 ) mag[2] = maxMAGValue * math.sin(2 * math.pi * freq * n / samplingFrequency) + random.gauss(0, maxMAGValue / 20) accel[0] = maxMAGValue * math.cos((2 * math.pi * freq * n) / samplingFrequency + (math.pi / 3)) + random.gauss( 0, maxMAGValue / 20 ) accel[1] = maxMAGValue * math.cos( (2 * math.pi * freq * n) / samplingFrequency + (7 * math.pi / 12) ) + random.gauss(0, maxMAGValue / 20) accel[2] = maxMAGValue * math.cos( (2 * math.pi * freq * n) / samplingFrequency + (9 * math.pi / 15) ) + random.gauss(0, maxMAGValue / 20) timestamp += int((1.0 / samplingFrequency) * usPerSecond) packet = nebResponsePacket.createMAGResponsePacket(timestamp, mag, accel) packetList.append(packet) return packetList
def waitForPacket(self, packetType, subSystem, command, timeout=3): packet = None currentTime = time.time() while not packet or \ (not packet.isPacketValid(packetType, subSystem, command) and not packet.isPacketError()): if time.time() - currentTime > timeout: raise TimeoutError try: bytes = self.device.receivePacket() if bytes: packet = NebResponsePacket(bytes) else: packet = None except NotImplementedError as e: logging.error("Dropped bad packet.") packet = None continue except InvalidPacketFormatError as e: logging.error("InvalidPacketFormatError") packet = None continue except CRCError as e: logging.error("CRCError : " + str(e)) packet = None continue except KeyError as e: logging.error( "Tried creating a packet with an invalid subsystem or command : " + str(e)) packet = None continue except TimeoutError as e: logging.error('Read timed out.') return NebResponsePacket.createEmptyResponsePacket( subSystem, command) except KeyboardInterrupt as e: logging.error("KeyboardInterrupt.") return NebResponsePacket.createEmptyResponsePacket( subSystem, command) except: packet = None logging.error("Unexpected error : ", exc_info=True) return NebResponsePacket.createEmptyResponsePacket( subSystem, command) return packet
def waitForPacket(self, packetType, subSystem, command, timeout=3): packet = None currentTime = time.time() while not packet or \ (not packet.isPacketValid(packetType, subSystem, command) and not packet.isPacketError()): if time.time() - currentTime > timeout: raise TimeoutError try: bytes = self.device.receivePacket() if bytes: packet = NebResponsePacket(bytes) else: packet = None except NotImplementedError as e: logging.error("Dropped bad packet.") packet = None continue except InvalidPacketFormatError as e: logging.error("InvalidPacketFormatError") packet = None continue except CRCError as e: logging.error("CRCError : " + str(e)) packet = None continue except KeyError as e: logging.error("Tried creating a packet with an invalid subsystem or command : " + str(e)) packet = None continue except TimeoutError as e: logging.error('Read timed out.') return NebResponsePacket.createEmptyResponsePacket(subSystem, command) except KeyboardInterrupt as e: logging.error("KeyboardInterrupt.") return NebResponsePacket.createEmptyResponsePacket(subSystem, command) except: packet = None logging.error("Unexpected error : ", exc_info=True) return NebResponsePacket.createEmptyResponsePacket(subSystem, command) return packet
def createWalkingPathPacketList(numSteps, averageSPM=61.0, maxDegreesDeviation=2.5, turns=2, startingPath=0.0): packetList = [] timestamp = 0 stepCount = 0 walkingDirection = startingPath timestampDivergence = 0.5 # Half a second time divergence # Choose at which step numbers the turns should occur turnStepIndices = [] for ii in range(0, turns): turnStepIndices.append(random.randint(1, numSteps)) for ii in range(0, numSteps): # Choose a new random direction if (ii in turnStepIndices): angleVariation = random.randrange(-90, 91, 180) else: angleVariation = random.gauss(0, 1.0) if (angleVariation > maxDegreesDeviation): angleVariation = maxDegreesDeviation elif (angleVariation < -maxDegreesDeviation): angleVariation = -maxDegreesDeviation # Update new walking direction walkingDirection = walkingDirection + angleVariation # Make sure the direction stays within -180 and +180 if (walkingDirection > 180.0): walkingDirection -= 360 elif (walkingDirection < -180.0): walkingDirection += 360 # Choose a new timestamp secondsPerStep = 60.0 / averageSPM timeDelta = random.gauss(secondsPerStep, 0.05) if (timeDelta < 0.1): timeDelta = 0.1 elif (timeDelta > secondsPerStep + 0.1): timeDelta = secondsPerStep + 0.1 timestamp = timestamp + int(secondsPerStep * usPerSecond) stepsPerMinute = int(60.0 / timeDelta) # Build the packet object packet = nebResponsePacket.createPedometerResponsePacket(\ timestamp, ii, stepsPerMinute, walkingDirection) packetList.append(packet) return packetList
def testLEDCommandDecoding(self): commandHeaderBytes = b'\x04\x08\x3a\x02' commandDataBytes= b'\x01\x01\x01\x01\x01\x01\x01\x01' commandBytes = commandHeaderBytes+commandDataBytes packet = NebResponsePacket(commandBytes) self.assertEqual(len(packet.data.ledState), 8) self.assertEqual(packet.data.ledState[0], 1) self.assertEqual(packet.data.ledState[1], 1) self.assertEqual(packet.data.ledState[2], 1) self.assertEqual(packet.data.ledState[3], 1) self.assertEqual(packet.data.ledState[4], 1) self.assertEqual(packet.data.ledState[5], 1) self.assertEqual(packet.data.ledState[6], 1) self.assertEqual(packet.data.ledState[7], 1)
def testDebugCommandDecoding(self): commandHeaderBytes = b'\x00\x10\xbc\x02' commandDataBytes= b'\xde\xea\xbe\xef\xa5\x01\x11\x01\x02\xba\xbe\x00\x01\x02\x03\x04' commandBytes = commandHeaderBytes+commandDataBytes packet = NebResponsePacket(commandBytes) self.assertEqual(packet.data.motionStatus.distance, True ) self.assertEqual(packet.data.motionStatus.force, False) self.assertEqual(packet.data.motionStatus.euler, True ) self.assertEqual(packet.data.motionStatus.quaternion, False) self.assertEqual(packet.data.motionStatus.imuData, False) self.assertEqual(packet.data.motionStatus.motion, True ) self.assertEqual(packet.data.motionStatus.steps, False) self.assertEqual(packet.data.motionStatus.magData, True ) self.assertEqual(packet.data.motionStatus.sitStand, True ) self.assertEqual(packet.data.recorderStatus.status, 2)
def testCreatePedometerPackets(self): #print("\n*** Testing Encoding and Decoding of Pedometer Packets ***") responsePackets = [] packets = nebsim.createWalkingPathPacketList(1000, 61.0, 5.0, 10) for packet in packets: packetString = packet.stringEncode() responsePackets.append(NebResponsePacket(packetString)) for idx,packet in enumerate(responsePackets): self.assertEqual(packets[idx].header.subSystem, SubSystem.Motion) self.assertEqual(packets[idx].header.command, Commands.Motion.Pedometer) self.assertEqual(packets[idx].data.timestamp, packet.data.timestamp) self.assertEqual(packets[idx].data.stepCount, packet.data.stepCount) self.assertEqual(packets[idx].data.stepsPerMinute, packet.data.stepsPerMinute) self.assertEqual(packets[idx].data.walkingDirection, packet.data.walkingDirection) self.assertGreaterEqual(packets[idx].data.walkingDirection, -180.0) self.assertLessEqual(packets[idx].data.walkingDirection, 180.0)
def testCreateEulerPackets(self): #print("\n*** Testing Encoding and Decoding of Euler Angle Packets ***") responsePackets = [] packets = nebsim.createSpinningObjectPacketList(50.0, 1.0, 2.0, 6.0) for packet in packets: packetString = packet.stringEncode() responsePackets.append(NebResponsePacket(packetString)) for idx,packet in enumerate(responsePackets): self.assertEqual(packets[idx].header.subSystem, SubSystem.Motion) self.assertEqual(packets[idx].header.command, Commands.Motion.EulerAngle) self.assertEqual(packets[idx].data.timestamp, packet.data.timestamp) self.assertEqual(packets[idx].data.yaw, packet.data.yaw) self.assertEqual(packets[idx].data.pitch, packet.data.pitch) self.assertEqual(packets[idx].data.roll, packet.data.roll) self.assertEqual(packets[idx].data.demoHeading, packet.data.demoHeading)
def storePacketsUntil(self, packetType, subSystem, command): packetList = [] packet = None while not packet or \ (not packet.isPacketValid(packetType, subSystem, command) and not packet.isPacketError()): try: if packet and packet.header.subSystem != SubSystem.Debug: packetList.append(packet) print('Received {0} packets'.format(len(packetList)), end="\r", flush=True) bytes = self.device.receivePacket() if bytes: packet = NebResponsePacket(bytes) else: packet = None except NotImplementedError as e: logging.error("Dropped bad packet.") packet = None continue except InvalidPacketFormatError as e: logging.error("InvalidPacketFormatError.") packet = None continue except CRCError as e: logging.error("CRCError : " + str(e)) packet = None continue except KeyError as e: logging.error( "Tried creating a packet with an invalid subsystem or command : " + str(e)) packet = None continue except TimeoutError as e: logging.error('Read timed out.') return None except KeyboardInterrupt as e: logging.error("KeyboardInterrupt.") return None except: packet = None logging.error("Unexpected error : ", exc_info=True) continue return packetList
def createWalkingPathPacketList(numSteps, averageSPM=61.0, maxDegreesDeviation=2.5, turns=2, startingPath=0.0): packetList = [] timestamp = 0 stepCount = 0 walkingDirection = startingPath timestampDivergence = 0.5 # Half a second time divergence # Choose at which step numbers the turns should occur turnStepIndices = [] for ii in range(0, turns): turnStepIndices.append(random.randint(1, numSteps)) for ii in range(0, numSteps): # Choose a new random direction if ii in turnStepIndices: angleVariation = random.randrange(-90, 91, 180) else: angleVariation = random.gauss(0, 1.0) if angleVariation > maxDegreesDeviation: angleVariation = maxDegreesDeviation elif angleVariation < -maxDegreesDeviation: angleVariation = -maxDegreesDeviation # Update new walking direction walkingDirection = walkingDirection + angleVariation # Make sure the direction stays within -180 and +180 if walkingDirection > 180.0: walkingDirection -= 360 elif walkingDirection < -180.0: walkingDirection += 360 # Choose a new timestamp secondsPerStep = 60.0 / averageSPM timeDelta = random.gauss(secondsPerStep, 0.05) if timeDelta < 0.1: timeDelta = 0.1 elif timeDelta > secondsPerStep + 0.1: timeDelta = secondsPerStep + 0.1 timestamp = timestamp + int(secondsPerStep * usPerSecond) stepsPerMinute = int(60.0 / timeDelta) # Build the packet object packet = nebResponsePacket.createPedometerResponsePacket(timestamp, ii, stepsPerMinute, walkingDirection) packetList.append(packet) return packetList
def testCreateMAGPackets(self): #print("\n*** Testing Encoding and Decoding of MAG Packets ***") responsePackets = [] packets = nebsim.createRandomMAGDataPacketList(50.0, 300, 1.0) for packet in packets: packetString = packet.stringEncode() responsePackets.append(NebResponsePacket(packetString)) for idx,packet in enumerate(responsePackets): self.assertEqual( packets[idx].header.subSystem, SubSystem.Motion) self.assertEqual( packets[idx].header.command, Commands.Motion.MAG) self.assertEqual( packets[idx].data.timestamp, packet.data.timestamp) self.assertEqual( packets[idx].data.mag[0], packet.data.mag[0]) self.assertEqual( packets[idx].data.mag[1], packet.data.mag[1]) self.assertEqual( packets[idx].data.mag[2], packet.data.mag[2]) self.assertEqual( packets[idx].data.accel[0], packet.data.accel[0]) self.assertEqual( packets[idx].data.accel[1], packet.data.accel[1]) self.assertEqual( packets[idx].data.accel[2], packet.data.accel[2])
def createSpinningObjectPacketList(samplingFrequency = 50.0,\ yawRPS=0.5, pitchRPS=0.0, rollRPS=0.0, startTimestamp = 0): packetList = [] degreesPerSecond = [0] * 3 degreesPerSecond[0] = yawRPS * 360 degreesPerSecond[1] = pitchRPS * 360 degreesPerSecond[2] = rollRPS * 360 samplingPeriod = 1 / samplingFrequency # Figure out the longest rotation without it being 0 dps # Remove the non-moving elements movingRotationsList = [x for x in degreesPerSecond if x != 0.0] # If it isn't moving, create a dummy packet list if len(movingRotationsList) == 0: time = range(0, 100) else: # Find the lowest DPS slowestMovingRotation = min(movingRotationsList) # Create enough samples for the longest rotation. numSamples = int(samplingFrequency * (1 / slowestMovingRotation) * 360) time = range(0, numSamples) for n in time: # A rotation is a sawtooth function that starts at 0 for n=0 yawDegrees = round( (((degreesPerSecond[0] * samplingPeriod * n) + 180) % 360) - 180, 2) pitchDegrees = round( (((degreesPerSecond[1] * samplingPeriod * n) + 90) % 180) - 90, 2) rollDegrees = round( (((degreesPerSecond[2] * samplingPeriod * n) + 180) % 360) - 180, 2) # Simulate the heading 'demo mode' activated demoDegrees = round( (((1.5 * degreesPerSecond[0] * samplingPeriod * n) + 180) % 360) - 180, 2) timestamp = (n * samplingPeriod * usPerSecond) + startTimestamp packet = nebResponsePacket.createEulerAngleResponsePacket(\ timestamp, yawDegrees, pitchDegrees, rollDegrees, demoDegrees) packetList.append(packet) return packetList
def buildPacketList(self, packetList): packets = [] errorList = [] for idx,packetString in enumerate(packetList): try: # print(binascii.hexlify(bytearray(packetString))) nebPacket = NebResponsePacket(packetString) # print('Appending {0}'.format(nebPacket)) packets.append(nebPacket) except KeyError as keyError: # print('Invalid Subsystem or Command Code') errorList.append(keyError) except NotImplementedError as notImplError: # print('Got a non-standard packet at #{0}'\ # .format(idx)) errorList.append(notImplError) except CRCError as crcError: errorList.append(crcError) # print('Got a CRCError at packet #{0}'\ # .format(idx)) # print(crcError) except InvalidPacketFormatError as invPacketError: errorList.append(invPacketError) return (packets, errorList)
def createSpinningObjectPacketList(samplingFrequency=50.0, yawRPS=0.5, pitchRPS=0.0, rollRPS=0.0, startTimestamp=0): packetList = [] degreesPerSecond = [0] * 3 degreesPerSecond[0] = yawRPS * 360 degreesPerSecond[1] = pitchRPS * 360 degreesPerSecond[2] = rollRPS * 360 samplingPeriod = 1 / samplingFrequency # Figure out the longest rotation without it being 0 dps # Remove the non-moving elements movingRotationsList = [x for x in degreesPerSecond if x != 0.0] # If it isn't moving, create a dummy packet list if len(movingRotationsList) == 0: time = range(0, 100) else: # Find the lowest DPS slowestMovingRotation = min(movingRotationsList) # Create enough samples for the longest rotation. numSamples = int(samplingFrequency * (1 / slowestMovingRotation) * 360) time = range(0, numSamples) for n in time: # A rotation is a sawtooth function that starts at 0 for n=0 yawDegrees = round((((degreesPerSecond[0] * samplingPeriod * n) + 180) % 360) - 180, 2) pitchDegrees = round((((degreesPerSecond[1] * samplingPeriod * n) + 90) % 180) - 90, 2) rollDegrees = round((((degreesPerSecond[2] * samplingPeriod * n) + 180) % 360) - 180, 2) # Simulate the heading 'demo mode' activated demoDegrees = round((((1.5 * degreesPerSecond[0] * samplingPeriod * n) + 180) % 360) - 180, 2) timestamp = (n * samplingPeriod * usPerSecond) + startTimestamp packet = nebResponsePacket.createEulerAngleResponsePacket( timestamp, yawDegrees, pitchDegrees, rollDegrees, demoDegrees ) packetList.append(packet) return packetList