def getEulerAngles(self):
try:
serv = self.periph.getServiceByUUID(EulerAngle_Service_UUID)
angle = [0]*3
charact = serv.getCharacteristics()
packetString = charact[0].read()
try:
nebPacket = neb.NebResponsePacket(packetString)
angle[0] = nebPacket.data.yaw
angle[1] = nebPacket.data.pitch
angle[2] = nebPacket.data.roll
except KeyError as keyError:
# print('Invalid Motion Engine Code')
print(keyError)
except NotImplementedError as notImplError:
# print(binascii.hexlify(bytearray(packetString)))
# print('Got a non-standard packet at #{0}'\
# .format(idx))
print(notImplError)
except neb.CRCError as crcError:
print(crcError)
# print('Got a CRCError at packet #{0}'\
# .format(idx))
except neb.InvalidPacketFormatError as invPacketError:
print(invPacketError)
return angle
except btle.BTLEException as e:
print("Exception =>",e)
self.periph.disconnect()
def testDecodeLED(self):
print("\n*** Testing LED Command Decoding ***")
commandHeaderBytes = b'\x04\x10\x1c\x02'
commandDataBytes = b'\x03\x04\x23\x05\xfe\x08\xaa\x01\x02\xba\xbe\x00\x01\x02\x03\x04'
commandBytes = commandHeaderBytes + commandDataBytes
packet = neb.NebResponsePacket(commandBytes)
self.assertEqual(len(packet.data.ledTupleList), 3)
self.assertEqual(packet.data.ledTupleList[0][0], 4)
self.assertEqual(packet.data.ledTupleList[0][1], 35)
self.assertEqual(packet.data.ledTupleList[1][0], 5)
self.assertEqual(packet.data.ledTupleList[1][1], 254)
self.assertEqual(packet.data.ledTupleList[2][0], 8)
self.assertEqual(packet.data.ledTupleList[2][1], 170)
def testDecodeDebug(self):
print("\n*** Testing Debug Command Decoding ***")
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 = neb.NebResponsePacket(commandBytes)
self.assertEqual(packet.data.distance, True)
self.assertEqual(packet.data.force, False)
self.assertEqual(packet.data.euler, True)
self.assertEqual(packet.data.quaternion, False)
self.assertEqual(packet.data.imuData, False)
self.assertEqual(packet.data.motion, True)
self.assertEqual(packet.data.steps, False)
self.assertEqual(packet.data.magData, True)
self.assertEqual(packet.data.sitStand, True)
self.assertEqual(packet.data.recorderStatus, 2)
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(neb.NebResponsePacket(packetString))
for idx, packet in enumerate(responsePackets):
self.assertEqual(packets[idx].header.subSystem,
neb.Subsys_MotionEngine)
self.assertEqual(packets[idx].header.command, neb.MotCmd_MAG_Data)
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 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(neb.NebResponsePacket(packetString))
for idx, packet in enumerate(responsePackets):
self.assertEqual(packets[idx].header.subSystem,
neb.Subsys_MotionEngine)
self.assertEqual(packets[idx].header.command,
neb.MotCmd_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 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(neb.NebResponsePacket(packetString))
for idx, packet in enumerate(responsePackets):
self.assertEqual(packets[idx].header.subSystem,
neb.Subsys_MotionEngine)
self.assertEqual(packets[idx].header.command, neb.MotCmd_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 getNeblinaPacket(self):
try:
serv = self.periph.getServiceByUUID(EulerAngle_Service_UUID)
charact = serv.getCharacteristics()
packetString = charact[0].read()
try:
nebPacket = neb.NebResponsePacket(packetString)
except KeyError as keyError:
print(keyError)
except NotImplementedError as notImplError:
print(notImplError)
except neb.CRCError as crcError:
print(crcError)
except neb.InvalidPacketFormatError as invPacketError:
print(invPacketError)
return nebPacket
except btle.BTLEException as e:
print("Exception =>",e)
self.periph.disconnect()
def buildPacketList(self, packetList):
packets = []
errorList = []
for idx, packetString in enumerate(packetList):
try:
# print(binascii.hexlify(bytearray(packetString)))
nebPacket = neb.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 neb.CRCError as crcError:
errorList.append(crcError)
# print('Got a CRCError at packet #{0}'\
# .format(idx))
# print(crcError)
except neb.InvalidPacketFormatError as invPacketError:
errorList.append(invPacketError)
return (packets, errorList)
def receivePacket(self):
consoleBytes = self.comslip.receivePacketFromStream(self.sc)
packet = neb.NebResponsePacket(consoleBytes)
return packet
