def __init__(self, model_path, fs_path, landmark_path, task_path):
self.load_keras_model(model_path, fs_path)
self.load_landmarks(landmark_path)
self.load_task_order(task_path)
# init node for mm_bridge
rospy.init_node('ttp_node', anonymous=True)
# init the publisher for command
self.cmd_pub = rospy.Publisher('cmd_pred', String, queue_size=10)
self.item_id = 0
self.rail_id = 1
self.thumbtack_id = 1
self.cmd_request_history = []
# init the signal processing unit
self.dsp = DSP()
self.z_buf = []
self.z_buf_size = 5 # min 5 timestamps for feature encoding
self.f_buf = []
self.f_buf_size = 120 # 20 Hz * 6 seconds
# record firing time
self.last_fire_time = time()
# init the subscriber at the end when all init is finished
rospy.Subscriber("mm_bridge_output", String, self.mm_msg_callback)
def feedback_test():
tb = DSP(filter_params, True, True)
cd_dsp4 = ClockDomain("dsp4", reset_less=True)
tb.clock_domains += cd_dsp4
tb.comb += tb.offset.eq(tb.output)
def run(tb):
dut = tb
yield dut.data.eq(1)
yield dut.coeff.eq(2)
yield
yield dut.data.eq(2)
yield
yield dut.data.eq(0)
for i in range(20):
yield
yield
run_simulation(
tb,
run(tb),
vcd_name="feedback_test.vcd",
clocks={
"sys": (8, 0),
"dsp4": (2, 0),
},
)
def dsp_test(double_reg=True):
tb = DSP(filter_params, double_reg)
def run(tb):
dut = tb
yield dut.data.eq(128)
yield dut.carryin.eq(20)
yield dut.coeff.eq(2)
for i in range(20):
yield
yield
run_simulation(tb, run(tb), vcd_name="dsp_test.vcd")
def __init__(self, ram=None, ipl_ram=None, dspreg_bytes=None):
self.RAM = bytearray(0x10000) if ram is None else ram
self.IPLRAM = bytearray(0x40) if ipl_ram is None else ipl_ram
self.DSP = None if dspreg_bytes is None else DSP(
self.RAM, dspreg_bytes)
# timing
self.clock = Clock(32040 * 768) ## 24Mhz
self.clock.add_clock("SPC", 24) ## 1Mhz
self.clock.add_clock("DSP", 768) ## 32Khz
self.timer0 = Timer(128) ## 8khz
self.timer1 = Timer(128) ## 8khz
self.timer2 = Timer(16) ## 64khz
#internal variables
self.iplROMEnable = True
self.dspaddress = 0
self.apu0 = 0
self.apu1 = 0
self.apu2 = 0
self.apu3 = 0
self.aux4 = 0
self.aux5 = 0
self.clockcounter = 0
#ports
self.ports = {
0x00F0: Port(0x00F0, "TEST", False, True),
0x00F1: Port(0x00F1, "CONTROL", False, True),
0x00F2: Port(0x00F2, "DSPADDR", True, True),
0x00F3: Port(0x00F3, "DSPDATA", True, True),
0x00F4: Port(0x00F4, "CPUIO0", True,
False), ## no CPU connected so readonly
0x00F5: Port(0x00F5, "CPUIO1", True,
False), ## no CPU connected so readonly
0x00F6: Port(0x00F6, "CPUIO2", True,
False), ## no CPU connected so readonly
0x00F7: Port(0x00F7, "CPUIO3", True,
False), ## no CPU connected so readonly
0x00F8: Port(0x00F8, "AUXIO4", True, True),
0x00F9: Port(0x00F9, "AUXIO5", True, True),
0x00FA: Port(0x00FA, "T0TARGET", False, True),
0x00FB: Port(0x00FB, "T1TARGET", False, True),
0x00FC: Port(0x00FC, "T2TARGET", False, True),
0x00FD: Port(0x00FD, "T0OUT", True, False),
0x00FE: Port(0x00FE, "T1OUT", True, False),
0x00FF: Port(0x00FF, "T2OUT", True, False),
}
def runVisualizer(roomDimensions, microphoneSensitivity, signalPos,
sigStrength, micGrid, gridOffset, gridHeight, camPos):
# Room parameters
width = roomDimensions[0]
length = roomDimensions[1]
ceiling = roomDimensions[2]
# Define room
room = Room(width, length, ceiling)
# Create a microphone array
microphonePositions = helpers.generateMicArray(micGrid[0], micGrid[1],
gridOffset, gridHeight)
if not helpers.allInRoom(microphonePositions, room):
print(
"Some microphones fell outside the boundaries of the room, please re-enter data."
)
return
# Microphone parameters
numMicrophones = len(microphonePositions)
microphoneArray = []
micSensitivity = microphoneSensitivity
# Set up microphone objects
for x in range(0, numMicrophones):
microphoneArray.append(
Microphone(microphonePositions[x], micSensitivity))
# Configure DSP object
dsp = DSP(99, microphoneArray)
# Set up camera controller
cameraPosition = camPos
if not helpers.allInRoom([cameraPosition], room):
print(
"The camera fell outside the boundaries of the room, please re-enter data."
)
return
cameraOrientation = [0, 0] # pointing along x-axis in degrees
cameraController = CameraController(cameraPosition, cameraOrientation, dsp,
room)
cameraController.setMicSensitivity(micSensitivity)
cameraController.setMicPositions(microphonePositions)
# Define Signal parameters
signalPosition = signalPos
if not helpers.allInRoom([signalPosition], room):
print(
"The signal fell outside the boundaries of the room, please re-enter data."
)
return
signalStrength = sigStrength
# Send signal to all microphones
for microphone in microphoneArray:
microphone.sendSignal(signalPosition, signalStrength)
print(
"-> Audio signal at position x: {0}, y: {1}, z: {2} with strength {3} broadcast to all microphones"
.format(signalPosition[0], signalPosition[1], signalPosition[2],
signalStrength))
# Predict signal location using sphere trilateration
predictedSignalLocation = cameraController.getSignalPosition(
signalStrength)
print("->>> Predicted Signal Location: {0}, Actual Signal Location: {1}".
format(predictedSignalLocation, signalPosition))
cameraController.rePositionCamera(predictedSignalLocation)
visualize(cameraController, signalStrength, predictedSignalLocation)
RFduinoConn = BleConn(inputArgs.sourceMac, inputArgs.MAC, inputArgs.interface)
def getValue():
if inputArgs.uuid:
return RFduinoConn.readValueFromUUID(inputArgs.uuid)
else:
return RFduinoConn.readValueFromHandle(inputArgs.handle)
def unpackFloat(hex):
return struct.unpack("f", binascii.unhexlify(hex))[0]
def unpackInt(hex):
return struct.unpack("h", binascii.unhexlify(hex))[0]
c = Calibration()
dsp = DSP(9)
value = getValue()
if inputArgs.debug:
print value
while inputArgs.continuous:
value = getValue()
hexStr = "".join(value.split(" "))
gx = unpackInt(hexStr[:4])
gy = unpackInt(hexStr[4:8])
gz = unpackInt(hexStr[8:12])
ax = unpackInt(hexStr[12:16])
ay = unpackInt(hexStr[16:20])
az = unpackInt(hexStr[20:24])
fig, ax = plt.subplots()
ax.set_xlim([0, VISUALIZER_LENGTH])
ax.set_ylim([0, VISUALIZER_HEIGHT])
x_axis = np.arange(VISUALIZER_LENGTH)
y_axis = np.zeros(VISUALIZER_LENGTH)
line, = ax.plot(x_axis, y_axis)
mic = Microphone(FORMAT=FORMAT,
CHANNELS=CHANNELS,
RATE=RATE,
FRAMES_PER_BUFFER=FRAMES_PER_BUFFER,
DEVICE_ID=DEVICE_ID)
sig_processor = DSP(ALPHA_SMOOTHING=ALPHA_SMOOTHING)
def init(): # only required for blitting to give a clean slate.
x_data = np.arange(VISUALIZER_LENGTH)
y_data = np.zeros(len(x_data))
line.set_data(x_data, y_data)
return line,
def animate(i):
x_data = np.arange(VISUALIZER_LENGTH)
y_data = np.zeros(len(x_data))
raw_data = mic.sampleInput()
if sum(raw_data) > 0:
processed_data = sig_processor.process_sample(raw_data)