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
