def pulse_sweep(delay_dict, min_dist, max_dist, dist_step, target_distance,
dist_interval, raw_frame_dict, frame_dict, frame_count, window,
scale_factor, sw_gain, sw_offset, cam_handle):
logger = logging.getLogger(__name__)
logger.info('Running Delay Sweep')
min_delay = min(delay_dict.keys())
max_delay = max(delay_dict.keys())
delay_list = np.arange(min_delay, max_delay) #[::-1]
dist_list = generate_distance_list(target_distance, min_dist, max_dist,
dist_step)
depth_crop = np.empty(
[len(dist_list), frame_count, window['X'], window['Y']])
ir_crop = np.empty([len(dist_list), frame_count, window['X'], window['Y']])
results = []
for step, curr_dist in enumerate(dist_list):
delay_list_ind = np.searchsorted(
delay_list, round((target_distance - curr_dist) / dist_interval))
curr_delay = delay_list[delay_list_ind]
logger.debug('curr_delay: %d, curr_dist: %d', curr_delay, curr_dist)
addr_list = [int(x, 16) for x in delay_dict[curr_delay].keys()]
value_list = [x for x in delay_dict[curr_delay].values()]
device.write_AFE_reg(cam_handle, addr_list, value_list)
#for addr, value in zip(addr_list, value_list):
# print('a: ' + hex(addr) + ' v: ' + hex(value))
#dummy reads
frame.dummy_read(cam_handle)
for frame_ind in np.arange(0, frame_count):
depth_image, ir_image = frame.get_depth_ir_images_df(
cam_handle, raw_frame_dict['width'], raw_frame_dict['height'])
depth_crop[step][frame_ind] = frame.crop_center(
depth_image, frame_dict['width'], frame_dict['height'],
window['X'], window['Y'])
ir_crop[step][frame_ind] = frame.crop_center(
ir_image, frame_dict['width'], frame_dict['height'],
window['X'], window['Y'])
#Process Data
data = data_stats(curr_delay, curr_dist, depth_crop[step],
ir_crop[step], frame_count, window, scale_factor,
sw_gain, sw_offset, cam_handle)
results.append(data)
logger.debug('expected_depth: %d, measured_depth: %d', data[1],
data[3])
# Reset Timing Registers
addr_list = [int(x, 16) for x in delay_dict[0].keys()]
value_list = [x for x in delay_dict[0].values()]
device.write_AFE_reg(cam_handle, addr_list, value_list)
dfResults = pd.DataFrame(results, columns = ['delay','expected_depth_12b', 'meas_depth_12b', 'meas_depth_12b_adj', 'error', 'meas_ir_12b', \
'saturation_count', 'depth_std', 'depth_noise', 'ir_std', 'ir_noise', \
'TAL1_R','TAL1_F','TAL2_R','TAL2_F','TAL3_R','TAL3_F','TAL4_R','TAL4_F','TAL5_R','TAL5_F', \
'expected_depth_14b', 'meas_depth_14b', 'correction_offset_14b'])
return dfResults
def rail_stats(min_dist, max_dist, dist_step, raw_frame_dict, frame_dict,
frame_count, window, scale_factor, sw_gain, sw_offset, rail,
cam_handle):
logger = logging.getLogger(__name__)
logger.info('Running Rail Sweep')
depth_crop = np.empty([frame_count, window['X'], window['Y']])
ir_crop = np.empty([frame_count, window['X'], window['Y']])
results = []
img = None
rail.moveRailMM(int(min_dist))
for i, curr_loc in enumerate(
range(int(min_dist), int(max_dist + 1), int(dist_step))):
rail.moveRailMM(curr_loc)
frame.dummy_read(cam_handle)
for frame_ind in np.arange(0, frame_count):
depthImage, irImage = frame.get_depth_ir_images_df(
cam_handle, raw_frame_dict['width'], raw_frame_dict['height'])
depth_crop[frame_ind] = frame.crop_center(depthImage,
frame_dict['width'],
frame_dict['height'],
window['X'], window['Y'])
ir_crop[frame_ind] = frame.crop_center(irImage,
frame_dict['width'],
frame_dict['height'],
window['X'], window['Y'])
data = data_stats(i, curr_loc, depth_crop, ir_crop, frame_count,
window, scale_factor, sw_gain, sw_offset, cam_handle)
results.append(data)
logger.debug('expected_depth: %d, measured_depth: %d', curr_loc,
data[3])
dfResults = pd.DataFrame(results, columns = ['delay','expected_depth_12b', 'meas_depth_12b', 'meas_depth_12b_adj', 'error', 'meas_ir_12b', \
'saturation_count', 'depth_std', 'depth_noise', 'ir_std', 'ir_noise', \
'TAL1_R','TAL1_F','TAL2_R','TAL2_F','TAL3_R','TAL3_F','TAL4_R','TAL4_F','TAL5_R','TAL5_F', \
'expected_depth_14b', 'meas_depth_14b', 'correction_offset_14b'])
return dfResults
def rail_sweep(min_dist, max_dist, dist_step, raw_frame_dict, frame_dict,
frame_count, window, scale_factor, sw_gain, sw_offset, rail,
cam_handle):
logger = logging.getLogger(__name__)
logger.info('Running Rail Sweep')
depthCrop = np.empty([frame_count, window['X'], window['Y']])
results = []
img = None
rail.moveRailMM(int(min_dist))
time.sleep(1) #inital wait
for i, curr_loc in enumerate(
range(int(min_dist), int(max_dist + 1), int(dist_step))):
rail.moveRailMM(curr_loc)
#Wait time for target to stop shaking
time.sleep(5)
frame.dummy_read(cam_handle)
for frame_ind in np.arange(0, frame_count):
depthImage = frame.get_depth_image_df(cam_handle,
raw_frame_dict['width'],
raw_frame_dict['height'])
depthCrop[frame_ind] = frame.crop_center(depthImage,
frame_dict['width'],
frame_dict['height'],
window['X'], window['Y'])
depthMasked = ma.masked_equal(depthCrop, 0)
satCount = ma.count_masked(depthMasked)
meas_depth = np.median(depthCrop)
meas_depth_14b = meas_depth * scale_factor
expected_depth_14b = curr_loc * scale_factor
correction_offset_14b = ((
(curr_loc - sw_offset) / sw_gain) * 4) - meas_depth_14b
meas_depth = (np.median(depthCrop) * sw_gain) + sw_offset
results.append([
0, curr_loc, expected_depth_14b, meas_depth, meas_depth_14b,
correction_offset_14b, satCount
])
logger.debug('expected_depth: %d, measured_depth: %d', curr_loc,
meas_depth)
dfResults = pd.DataFrame(results,
columns=[
'delay', 'expected_depth_12b',
'expected_depth_14b', 'meas_depth_12b',
'meas_depth_14b', 'correction_offset_14b',
'saturation_count'
])
return dfResults
def sx_pulse_sweep(delay_dict, raw_frame_dict, frame_dict, frame_count, window,
dev):
logger = logging.getLogger(__name__)
logger.info('Running Delay Sweep')
min_delay = min(delay_dict.keys())
max_delay = max(delay_dict.keys())
delay_list = np.arange(max_delay, min_delay, -4)
data1Crop = np.empty(
[len(delay_list), frame_count, window['X'], window['Y']])
data2Crop = np.empty(
[len(delay_list), frame_count, window['X'], window['Y']])
results = []
for step, delay_list_ind in enumerate(delay_list):
logger.debug('curr_delay: %d', delay_list_ind)
addr_list = [int(x, 16) for x in delay_dict[delay_list_ind].keys()]
value_list = [x for x in delay_dict[delay_list_ind].values()]
write_to_AFE(dev, addr_list, value_list)
#dummy reads
frame.dummy_read(dev)
for frame_ind in np.arange(0, frame_count):
data1, data2 = frame.get_depth_ir_images_df(
dev, raw_frame_dict['width'], raw_frame_dict['height'])
data1Crop[step][frame_ind] = frame.crop_center(
data1, frame_dict['width'], frame_dict['height'], window['X'],
window['Y'])
data2Crop[step][frame_ind] = frame.crop_center(
data2, frame_dict['width'], frame_dict['height'], window['X'],
window['Y'])
data1Avg = np.median(data1Crop[step])
data2Avg = np.median(data2Crop[step])
results.append([-delay_list_ind, data1Avg, data2Avg])
# Reset Timing Registers
addr_list = [int(x, 16) for x in delay_dict[0].keys()]
value_list = [x for x in delay_dict[0].values()]
write_to_AFE(dev, addr_list, value_list)
return results
def verify_depth(dev_handle, cfg, cfg_prev, target_dist, firmware_path,
num_frames):
device.program_firmware(dev_handle, firmware_path)
# Get Parameters
window = {}
window['X'] = cfg['window_x']
window['Y'] = cfg['window_y']
raw_frame_dict = {}
raw_frame_dict['height'] = cfg['raw_frame_height']
raw_frame_dict['width'] = cfg['raw_frame_width']
frame_dict = {}
frame_dict['height'] = cfg['frame_height']
frame_dict['width'] = cfg['frame_width']
gain = cfg_prev['sw_gain']
offset = cfg_prev['sw_offset']
# Empty ndarray
depthCrop = np.empty([1, num_frames, window['X'], window['Y']])
frame.dummy_read(dev_handle)
# Get image
for i in range(num_frames):
depthImage = frame.get_depth_image_df(dev_handle,
raw_frame_dict['width'],
raw_frame_dict['height'])
depthCrop[0][i] = frame.crop_center(depthImage, frame_dict['width'],
frame_dict['height'], window['X'],
window['Y'])
meas_depth = (np.median(depthCrop[0]) * gain) + offset
logger.info(
"Measured Depth for previous station calibrated firmware: %.2f" %
meas_depth)
#print(target_dist)
depth_error = meas_depth - target_dist
depth_error_perc = (depth_error / target_dist) * 100
logger.info("Depth error percent is:" + str(depth_error_perc))
if depth_error_perc >= cfg['depth_perc_err_threshold']:
logger.error("Depth Error GREATER than threshold - FAILED")
else:
logger.info("Depth error LESS than threshold - PASSED")
def get_target_pc(cam_handle, code_dict, pc, target_ir, max_pc, cfg,
multiply_factor):
switch_count = 0
old_flag = 1
flag = 1
data = np.zeros(len(list(code_dict.keys())))
data[:] = pc
to_multiply_keys = [0x4110, 0x46aa, 0x4128, 0x46f2, 0x40c2]
no_multiply_keys = Diff(list(code_dict.keys()), to_multiply_keys)
data_simple = np.zeros(len(no_multiply_keys))
data_simple[:] = pc
data_mul = np.zeros(len(to_multiply_keys))
data_mul[:] = pc * multiply_factor
ir_crop = np.empty((cfg['frame_count'], cfg['window_x'], cfg['window_y']))
while (switch_count != 3):
# data_simple[:] = pc
# device.write_AFE_reg(cam_handle, np.array(no_multiply_keys), data_simple)
if multiply_factor != 1: # avoid writing into registers coresponding to mid
data_mul[:] = pc * multiply_factor
device.write_AFE_reg(cam_handle, np.array(to_multiply_keys),
data_mul)
data[:] = pc
device.write_AFE_reg(cam_handle, np.array(list(code_dict.keys())),
data)
for i in range(5):
frame.dummy_read(cam_handle)
for frame_ind in np.arange(0, cfg['frame_count']):
ir_image = frame.get_ir_image_df(cam_handle,
cfg['raw_frame_width'],
cfg['raw_frame_height'])
ir_crop[frame_ind] = frame.crop_center(ir_image,
cfg['frame_width'],
cfg['frame_height'],
cfg['window_x'],
cfg['window_y'])
ir_val = np.average(ir_crop)
print('PC:' + str(pc) + ' IR:' + str(ir_val))
log_dict['results'].append({'PC': str(pc), 'IR': str(ir_val)})
if old_flag != flag:
switch_count = switch_count + 1
if switch_count == 2:
break
if pc >= max_pc:
print('Could not achieve target IR')
break
if (ir_val < target_ir):
if switch_count:
pc = pc + 5
else:
pc = pc + 15
flag = 1
elif (ir_val > target_ir):
if switch_count:
pc = pc - 5
else:
pc = pc - 15
flag = -1
code_dict = dict.fromkeys(code_dict.keys(), pc)
if multiply_factor != 1: # check required in order to avoid KeyError in other modes than mid
for addr in to_multiply_keys:
code_dict[addr] = code_dict[addr] * multiply_factor
return pc, code_dict
