Beispiel #1
0
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
Beispiel #2
0
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
Beispiel #3
0
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
Beispiel #4
0
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
Beispiel #6
0
    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
Beispiel #7
0
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
Beispiel #8
0
 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)
Beispiel #9
0
 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)
Beispiel #10
0
 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)
Beispiel #11
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)
Beispiel #12
0
 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
Beispiel #13
0
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
Beispiel #14
0
 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])
Beispiel #15
0
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
Beispiel #16
0
 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)
Beispiel #17
0
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