def __init__(self, arg):
self.samplingFrequency = 50.0
self.imuRollIdx = 0
self.headingRollIdx = 0
self.numIMUSamples = 1000
graphSize = 250
self.timebase = np.arange(0, graphSize)
self.accelData = [[],[],[]]
self.gyroData = [[],[],[]]
self.headingData = 0.0
self.demoHeading = 0.0
if(streamBLE):
packet = ble.getNeblinaPacket()
if (packet.header.subSytem == Subsys_MotionEngine):
else:
self.imuPackets = sim.createRandomIMUDataPacketList(
self.samplingFrequency, self.numIMUSamples, 0.5)
self.eulerAnglePackets = sim.createSpinningObjectPacketList(
self.samplingFrequency, yawRPS=0.25)
# Populate the initial IMU data list
for elem in (packet for idx,packet in enumerate(self.imuPackets) if idx < graphSize):
# For all three axis
for idx,axisSample in enumerate(elem.data.accel):
# Gyro and Accel have same num of axis
self.accelData[idx].append(axisSample)
self.gyroData[idx].append(axisSample)
# Populate the initial euler heading data
self.headingData = self.eulerAnglePackets[0].data.yaw
self.demoHeading = self.eulerAnglePackets[0].data.demoHeading
def testCreateIMUPackets(self):
print("\n*** Testing Encoding and Decoding of IMU Packets ***")
responsePackets = []
packets = nebsim.createRandomIMUDataPacketList(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_IMU_Data)
self.assertEqual( packets[idx].data.timestamp, packet.data.timestamp )
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] )
self.assertEqual( packets[idx].data.gyro[0], packet.data.gyro[0] )
self.assertEqual( packets[idx].data.gyro[1], packet.data.gyro[1] )
self.assertEqual( packets[idx].data.gyro[2], packet.data.gyro[2] )
def testCreateIMUPackets(self):
#print("\n*** Testing Encoding and Decoding of IMU Packets ***")
responsePackets = []
packets = nebsim.createRandomIMUDataPacketList(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.IMU)
self.assertEqual( packets[idx].data.timestamp, packet.data.timestamp)
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])
self.assertEqual( packets[idx].data.gyro[0], packet.data.gyro[0])
self.assertEqual( packets[idx].data.gyro[1], packet.data.gyro[1])
self.assertEqual( packets[idx].data.gyro[2], packet.data.gyro[2])
def testEncodeCommandPackets(self):
print("\n*** Testing Encoding of Packets ***")
packetList = []
# Testing downsample command
# Downsample to 1Hz example
downSampleFactor = 1000
downSampleCommandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_Downsample,
downSampleFactor)
packetString = downSampleCommandPacket.stringEncode()
packetBytes = bytearray(packetString)
self.assertEqual(len(packetBytes), 20)
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)
| neb.Subsys_MotionEngine)
self.assertEqual(packetBytes[1], 16)
self.assertEqual(packetBytes[3], neb.MotCmd_Downsample)
self.assertEqual(packetBytes[8],
struct.pack('<H', downSampleFactor)[0])
self.assertEqual(packetBytes[9],
struct.pack('<H', downSampleFactor)[1])
# Make sure these calls dont cause an exception
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_MotionState, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_MotionState, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_IMU_Data, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_IMU_Data, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_Quaternion, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_Quaternion, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_EulerAngle, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_EulerAngle, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_ExtForce, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_ExtForce, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_TrajectoryInfo, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_TrajectoryInfo, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_Pedometer, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_Pedometer, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_MAG_Data, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine,
neb.MotCmd_MAG_Data, False)
# Test encoding of recording packets
recordCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage,
neb.StorageCmd_EraseAll)
packetBytes = bytearray(recordCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Storage)
self.assertEqual(packetBytes[1], 16)
self.assertEqual(packetBytes[3], neb.StorageCmd_EraseAll)
recordCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage,
neb.StorageCmd_Record, True)
packetBytes = bytearray(recordCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Storage)
self.assertEqual(packetBytes[3], neb.StorageCmd_Record)
self.assertEqual(packetBytes[8], 0x01)
recordCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage,
neb.StorageCmd_Record,
False)
packetBytes = bytearray(recordCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Storage)
self.assertEqual(packetBytes[3], neb.StorageCmd_Record)
self.assertEqual(packetBytes[8], 0x00)
recordCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage,
neb.StorageCmd_Playback,
True,
sessionID=42)
packetBytes = bytearray(recordCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Storage)
self.assertEqual(packetBytes[3], neb.StorageCmd_Playback)
self.assertEqual(packetBytes[8], 0x01)
recordCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage,
neb.StorageCmd_Playback,
False,
sessionID=42)
packetBytes = bytearray(recordCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Storage)
self.assertEqual(packetBytes[3], neb.StorageCmd_Playback)
self.assertEqual(packetBytes[8], 0x00)
readCommandPacket = neb.NebCommandPacket(neb.Subsys_EEPROM,
neb.EEPROMCmd_Read,
pageNumber=5)
packetBytes = bytearray(readCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_EEPROM)
self.assertEqual(packetBytes[3], neb.EEPROMCmd_Read)
self.assertEqual(packetBytes[4], 0x05)
# EEPROM Command packet testing
writeCommandPacket = neb.NebCommandPacket(neb.Subsys_EEPROM, neb.EEPROMCmd_Write,\
pageNumber=11, dataBytes=b'\xde\xad\xbe\xef\xba\xbe\x92\x74')
packetBytes = bytearray(writeCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_EEPROM)
self.assertEqual(packetBytes[3], neb.EEPROMCmd_Write)
self.assertEqual(packetBytes[4], 0x0B)
self.assertEqual(packetBytes[5], 0x00)
self.assertEqual(packetBytes[6], 0xde)
self.assertEqual(packetBytes[7], 0xad)
self.assertEqual(packetBytes[8], 0xbe)
self.assertEqual(packetBytes[9], 0xef)
self.assertEqual(packetBytes[10], 0xba)
self.assertEqual(packetBytes[11], 0xbe)
self.assertEqual(packetBytes[12], 0x92)
self.assertEqual(packetBytes[13], 0x74)
# Debug packet set interface
switchInterfaceCommandPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_SetInterface,\
True)
packetBytes = bytearray(switchInterfaceCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Debug)
self.assertEqual(packetBytes[3], neb.DebugCmd_SetInterface)
self.assertEqual(packetBytes[8], 0x01)
switchInterfaceCommandPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_SetInterface,\
False)
packetBytes = bytearray(switchInterfaceCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Debug)
self.assertEqual(packetBytes[3], neb.DebugCmd_SetInterface)
self.assertEqual(packetBytes[8], 0x00)
flashAndRecorderStateCommandPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_MotAndFlashRecState)
packetBytes = bytearray(
flashAndRecorderStateCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Debug)
self.assertEqual(packetBytes[3], neb.DebugCmd_MotAndFlashRecState)
# Unit test start command packet
unitTestStartCommandPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_StartUnitTestMotion, True)
packetBytes = bytearray(unitTestStartCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Debug)
self.assertEqual(packetBytes[3], neb.DebugCmd_StartUnitTestMotion)
self.assertEqual(packetBytes[8], 0x01)
unitTestStartCommandPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_StartUnitTestMotion, False)
packetBytes = bytearray(unitTestStartCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Debug)
self.assertEqual(packetBytes[3], neb.DebugCmd_StartUnitTestMotion)
self.assertEqual(packetBytes[8], 0x00)
# Unit test data command packet
imuPackets = nebsim.createRandomIMUDataPacketList(50.0, 10, 1.0)
magPackets = nebsim.createRandomMAGDataPacketList(50.0, 10, 1.0)
for idx, imuPacket in enumerate(imuPackets):
magPacket = magPackets[idx]
imuPacketString = imuPacket.stringEncode()
magPacketString = magPacket.stringEncode()
unitTestDataPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_UnitTestMotionData, timestamp=imuPacket.data.timestamp,\
accel=imuPacket.data.accel, gyro=imuPacket.data.gyro, mag=magPacket.data.mag)
testPacketBytes = bytearray(unitTestDataPacket.stringEncode())
self.assertEqual(testPacketBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Debug)
self.assertEqual(testPacketBytes[3],
neb.DebugCmd_UnitTestMotionData)
self.assertEqual(testPacketBytes[8], imuPacketString[8])
self.assertEqual(testPacketBytes[9], imuPacketString[9])
self.assertEqual(testPacketBytes[10], imuPacketString[10])
self.assertEqual(testPacketBytes[11], imuPacketString[11])
self.assertEqual(testPacketBytes[12], imuPacketString[12])
self.assertEqual(testPacketBytes[13], imuPacketString[13])
self.assertEqual(testPacketBytes[14], imuPacketString[14])
self.assertEqual(testPacketBytes[15], imuPacketString[15])
self.assertEqual(testPacketBytes[16], imuPacketString[16])
self.assertEqual(testPacketBytes[17], imuPacketString[17])
self.assertEqual(testPacketBytes[18], imuPacketString[18])
self.assertEqual(testPacketBytes[19], imuPacketString[19])
self.assertEqual(testPacketBytes[20], magPacketString[8])
self.assertEqual(testPacketBytes[21], magPacketString[9])
self.assertEqual(testPacketBytes[22], magPacketString[10])
self.assertEqual(testPacketBytes[23], magPacketString[11])
self.assertEqual(testPacketBytes[24], magPacketString[12])
self.assertEqual(testPacketBytes[25], magPacketString[13])
# LED Command Packets
ledValues = [(0, 1), (1, 34), (3, 0), (4, 254), (5, 128), (12, 11)]
setLEDValuesPacket = neb.NebCommandPacket(\
neb.Subsys_LED, neb.LEDCmd_SetVal, ledValueTupleList=ledValues)
packetBytes = bytearray(setLEDValuesPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_LED)
self.assertEqual(packetBytes[3], neb.LEDCmd_SetVal)
self.assertEqual(packetBytes[4], 6)
self.assertEqual(packetBytes[5], 0x00)
self.assertEqual(packetBytes[6], 0x01)
self.assertEqual(packetBytes[7], 0x01)
self.assertEqual(packetBytes[8], 34)
self.assertEqual(packetBytes[9], 3)
self.assertEqual(packetBytes[10], 0)
self.assertEqual(packetBytes[11], 4)
self.assertEqual(packetBytes[12], 254)
self.assertEqual(packetBytes[13], 5)
self.assertEqual(packetBytes[14], 128)
self.assertEqual(packetBytes[15], 12)
self.assertEqual(packetBytes[16], 11)
leds = [3, 5, 1, 6, 7, 2, 4]
getLEDValuesPacket = neb.NebCommandPacket(\
neb.Subsys_LED, neb.LEDCmd_GetVal,\
ledIndices=leds)
packetBytes = bytearray(getLEDValuesPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_LED)
self.assertEqual(packetBytes[3], neb.LEDCmd_GetVal)
self.assertEqual(packetBytes[4], 7)
self.assertEqual(packetBytes[5], 3)
self.assertEqual(packetBytes[6], 5)
self.assertEqual(packetBytes[7], 1)
self.assertEqual(packetBytes[8], 6)
self.assertEqual(packetBytes[9], 7)
self.assertEqual(packetBytes[10], 2)
self.assertEqual(packetBytes[11], 4)
# Flash Session Info
sessionInfoCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage,\
neb.StorageCmd_SessionInfo, sessionID = 10)
packetBytes = bytearray(sessionInfoCommandPacket.stringEncode())
self.assertEqual(packetBytes[0],
(neb.PacketType_Command << 5) | neb.Subsys_Storage)
self.assertEqual(packetBytes[3], neb.StorageCmd_SessionInfo)
self.assertEqual(packetBytes[8], 0x0A)
self.assertEqual(packetBytes[9], 0x00)
def testEncodeCommandPackets(self):
print("\n*** Testing Encoding of Packets ***")
packetList = []
# Testing downsample command
# Downsample to 1Hz example
downSampleFactor = 1000
downSampleCommandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_Downsample, downSampleFactor)
packetString = downSampleCommandPacket.stringEncode()
packetBytes = bytearray(packetString)
self.assertEqual(len(packetBytes), 20)
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_MotionEngine)
self.assertEqual(packetBytes[1], 16)
self.assertEqual(packetBytes[3], neb.MotCmd_Downsample)
self.assertEqual(packetBytes[8], struct.pack('<H', downSampleFactor)[0])
self.assertEqual(packetBytes[9], struct.pack('<H', downSampleFactor)[1])
# Make sure these calls dont cause an exception
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_MotionState, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_MotionState, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_IMU_Data, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_IMU_Data, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_Quaternion, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_Quaternion, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_EulerAngle, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_EulerAngle, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_ExtForce, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_ExtForce, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_TrajectoryInfo, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_TrajectoryInfo, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_Pedometer, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_Pedometer, False)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_MAG_Data, True)
commandPacket = neb.NebCommandPacket(neb.Subsys_MotionEngine, neb.MotCmd_MAG_Data, False)
# Test encoding of recording packets
recordCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage, neb.StorageCmd_EraseAll)
packetBytes = bytearray(recordCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Storage)
self.assertEqual(packetBytes[1], 16)
self.assertEqual(packetBytes[3], neb.StorageCmd_EraseAll)
recordCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage, neb.StorageCmd_Record, True)
packetBytes = bytearray(recordCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Storage)
self.assertEqual(packetBytes[3], neb.StorageCmd_Record)
self.assertEqual(packetBytes[8], 0x01)
recordCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage, neb.StorageCmd_Record, False)
packetBytes = bytearray(recordCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Storage)
self.assertEqual(packetBytes[3], neb.StorageCmd_Record)
self.assertEqual(packetBytes[8], 0x00)
recordCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage, neb.StorageCmd_Playback, True, sessionID=42)
packetBytes = bytearray(recordCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Storage)
self.assertEqual(packetBytes[3], neb.StorageCmd_Playback)
self.assertEqual(packetBytes[8], 0x01)
recordCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage, neb.StorageCmd_Playback, False, sessionID=42)
packetBytes = bytearray(recordCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Storage)
self.assertEqual(packetBytes[3], neb.StorageCmd_Playback)
self.assertEqual(packetBytes[8], 0x00)
readCommandPacket = neb.NebCommandPacket(neb.Subsys_EEPROM, neb.EEPROMCmd_Read, pageNumber=5)
packetBytes = bytearray(readCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_EEPROM)
self.assertEqual(packetBytes[3], neb.EEPROMCmd_Read)
self.assertEqual(packetBytes[4], 0x05)
# EEPROM Command packet testing
writeCommandPacket = neb.NebCommandPacket(neb.Subsys_EEPROM, neb.EEPROMCmd_Write,\
pageNumber=11, dataBytes=b'\xde\xad\xbe\xef\xba\xbe\x92\x74')
packetBytes = bytearray(writeCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_EEPROM)
self.assertEqual(packetBytes[3], neb.EEPROMCmd_Write)
self.assertEqual(packetBytes[4], 0x0B)
self.assertEqual(packetBytes[5], 0x00)
self.assertEqual(packetBytes[6], 0xde)
self.assertEqual(packetBytes[7], 0xad)
self.assertEqual(packetBytes[8], 0xbe)
self.assertEqual(packetBytes[9], 0xef)
self.assertEqual(packetBytes[10], 0xba)
self.assertEqual(packetBytes[11], 0xbe)
self.assertEqual(packetBytes[12], 0x92)
self.assertEqual(packetBytes[13], 0x74)
# Debug packet set interface
switchInterfaceCommandPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_SetInterface,\
True)
packetBytes = bytearray(switchInterfaceCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Debug)
self.assertEqual(packetBytes[3], neb.DebugCmd_SetInterface)
self.assertEqual(packetBytes[8], 0x01)
switchInterfaceCommandPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_SetInterface,\
False)
packetBytes = bytearray(switchInterfaceCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Debug)
self.assertEqual(packetBytes[3], neb.DebugCmd_SetInterface)
self.assertEqual(packetBytes[8], 0x00)
flashAndRecorderStateCommandPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_MotAndFlashRecState)
packetBytes = bytearray(flashAndRecorderStateCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Debug)
self.assertEqual(packetBytes[3], neb.DebugCmd_MotAndFlashRecState)
# Unit test start command packet
unitTestStartCommandPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_StartUnitTestMotion, True)
packetBytes = bytearray(unitTestStartCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Debug)
self.assertEqual(packetBytes[3], neb.DebugCmd_StartUnitTestMotion)
self.assertEqual(packetBytes[8], 0x01)
unitTestStartCommandPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_StartUnitTestMotion, False)
packetBytes = bytearray(unitTestStartCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Debug)
self.assertEqual(packetBytes[3], neb.DebugCmd_StartUnitTestMotion)
self.assertEqual(packetBytes[8], 0x00)
# Unit test data command packet
imuPackets = nebsim.createRandomIMUDataPacketList(50.0, 10, 1.0)
magPackets = nebsim.createRandomMAGDataPacketList(50.0, 10, 1.0)
for idx,imuPacket in enumerate(imuPackets):
magPacket = magPackets[idx]
imuPacketString = imuPacket.stringEncode()
magPacketString = magPacket.stringEncode()
unitTestDataPacket = neb.NebCommandPacket(neb.Subsys_Debug,\
neb.DebugCmd_UnitTestMotionData, timestamp=imuPacket.data.timestamp,\
accel=imuPacket.data.accel, gyro=imuPacket.data.gyro, mag=magPacket.data.mag)
testPacketBytes = bytearray(unitTestDataPacket.stringEncode())
self.assertEqual(testPacketBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Debug)
self.assertEqual(testPacketBytes[3], neb.DebugCmd_UnitTestMotionData)
self.assertEqual(testPacketBytes[8], imuPacketString[8])
self.assertEqual(testPacketBytes[9], imuPacketString[9])
self.assertEqual(testPacketBytes[10], imuPacketString[10])
self.assertEqual(testPacketBytes[11], imuPacketString[11])
self.assertEqual(testPacketBytes[12], imuPacketString[12])
self.assertEqual(testPacketBytes[13], imuPacketString[13])
self.assertEqual(testPacketBytes[14], imuPacketString[14])
self.assertEqual(testPacketBytes[15], imuPacketString[15])
self.assertEqual(testPacketBytes[16], imuPacketString[16])
self.assertEqual(testPacketBytes[17], imuPacketString[17])
self.assertEqual(testPacketBytes[18], imuPacketString[18])
self.assertEqual(testPacketBytes[19], imuPacketString[19])
self.assertEqual(testPacketBytes[20], magPacketString[8])
self.assertEqual(testPacketBytes[21], magPacketString[9])
self.assertEqual(testPacketBytes[22], magPacketString[10])
self.assertEqual(testPacketBytes[23], magPacketString[11])
self.assertEqual(testPacketBytes[24], magPacketString[12])
self.assertEqual(testPacketBytes[25], magPacketString[13])
# LED Command Packets
ledValues = [(0,1),(1,34),(3,0),(4,254),(5,128),(12,11)]
setLEDValuesPacket = neb.NebCommandPacket(\
neb.Subsys_LED, neb.LEDCmd_SetVal, ledValueTupleList=ledValues)
packetBytes = bytearray(setLEDValuesPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_LED)
self.assertEqual(packetBytes[3], neb.LEDCmd_SetVal)
self.assertEqual(packetBytes[4], 6)
self.assertEqual(packetBytes[5], 0x00)
self.assertEqual(packetBytes[6], 0x01)
self.assertEqual(packetBytes[7], 0x01)
self.assertEqual(packetBytes[8], 34)
self.assertEqual(packetBytes[9], 3)
self.assertEqual(packetBytes[10], 0)
self.assertEqual(packetBytes[11], 4)
self.assertEqual(packetBytes[12], 254)
self.assertEqual(packetBytes[13], 5)
self.assertEqual(packetBytes[14], 128)
self.assertEqual(packetBytes[15], 12)
self.assertEqual(packetBytes[16], 11)
leds = [3,5,1,6,7,2,4]
getLEDValuesPacket = neb.NebCommandPacket(\
neb.Subsys_LED, neb.LEDCmd_GetVal,\
ledIndices=leds)
packetBytes = bytearray(getLEDValuesPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_LED)
self.assertEqual(packetBytes[3], neb.LEDCmd_GetVal)
self.assertEqual(packetBytes[4], 7)
self.assertEqual(packetBytes[5], 3)
self.assertEqual(packetBytes[6], 5)
self.assertEqual(packetBytes[7], 1)
self.assertEqual(packetBytes[8], 6)
self.assertEqual(packetBytes[9], 7)
self.assertEqual(packetBytes[10], 2)
self.assertEqual(packetBytes[11], 4)
# Flash Session Info
sessionInfoCommandPacket = neb.NebCommandPacket(neb.Subsys_Storage,\
neb.StorageCmd_SessionInfo, sessionID = 10)
packetBytes = bytearray(sessionInfoCommandPacket.stringEncode())
self.assertEqual(packetBytes[0], (neb.PacketType_Command << 5)| neb.Subsys_Storage)
self.assertEqual(packetBytes[3], neb.StorageCmd_SessionInfo)
self.assertEqual(packetBytes[8], 0x0A)
self.assertEqual(packetBytes[9], 0x00)
