def process_message(self, msg):
# read a Dragonfly message
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id == self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
return
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'PlotHead')
elif msg_type == rc.MT_POLARIS_POSITION:
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, msg)
positions = np.asarray(in_mdf.xyz[:])
orientations = self.shuffle_q(np.asarray(in_mdf.ori[:]))
if in_mdf.tool_id == (self.pointer + 1):
Qf = qa.norm(orientations)
Qr = qa.mult(Qf, qa.inv(self.pointer_Qi)).flatten()
#find_nans(self.store_head, Qr, 'Qr')
Tk = positions
#find_nans(self.store_head, Tk, 'Tk')
tip_pos = (qa.rotate(Qr, self.pointer_Xi) + Tk).flatten()
self.pointer_position = np.append(self.pointer_position,
(tip_pos[np.newaxis, :]),
axis=0)
#self.pl.reset(x=self.pointer_position[:,0], y=self.pointer_position[:,1], z=self.pointer_position[:,2])
print("old=", tip_pos)
print("new=", self.tp.get_pos(orientations, positions)[0])
def wobble(obt, wobble_psi2_model=get_wobble_psi2_maris, offset=0):
"""Gets array of OBT and returns an array of quaternions"""
R_psi1 = qarray.inv(qarray.rotation([0,0,1], private.WOBBLE_DX7['psi1_ref']))
R_psi2 = qarray.inv(qarray.rotation([0,1,0], private.WOBBLE_DX7['psi2_ref']))
psi2 = wobble_psi2_model(obt) - offset
R_psi2T = qarray.rotation([0,1,0], psi2)
wobble_rotation = qarray.mult(qarray.inv(R_psi1),
qarray.mult(R_psi2T ,
qarray.mult(R_psi2 , R_psi1)
)
)
#debug_here()
return wobble_rotation
def process_message(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.array(in_mdf.xyz[:])
orientations = self.shuffle_q(np.array(in_mdf.ori[:]))
# np.testing.assert_array_equal(positions[:,0], orientations[:,0], err_msg='Samples are not aligned')
if self.calibrated:
if in_mdf.tool_id == (self.marker + 1):
# calculating output
self.Qk = qa.norm(
orientations
) # need to find a way to discriminate the tools files in the messages???
Qr = qa.mult(self.Qk, qa.inv(self.Qi)).flatten()
Tk = positions
hotspot_position = (qa.rotate(Qr, self.Xi) + Tk).flatten()
hotspot_vector_head = qa.rotate(Qr, plate_vector)
if np.any(np.isnan(hotspot_position)) == True:
print "x",
# print ' *****nan present, check coil is within frame!*****'
# creating output message
out_mdf = rc.MDF_HOTSPOT_POSITION()
out_mdf.xyz[:] = hotspot_position
out_mdf.ori[:3] = hotspot_vector_head # Qk - coil active orientation
out_mdf.sample_header = in_mdf.sample_header
msg = CMessage(rc.MT_HOTSPOT_POSITION)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
sys.stdout.write("o")
else:
if np.any(np.isnan(positions)) == True:
raise Exception, "nan present"
if np.any(np.isnan(orientations)) == True:
raise Exception, "nan present"
if (
(self.store_plate >= self.store_plate_pos.shape[0])
& (self.store_plate >= self.store_plate_ori.shape[0])
& (self.store_coil >= self.store_coil_pos.shape[0])
& (self.store_coil >= self.store_coil_ori.shape[0])
):
self.calibrating = False
self.make_calibration_vector()
elif in_mdf.tool_id == (self.marker + 1):
self.store_coil_pos[self.store_coil, :] = positions
self.store_coil_ori[self.store_coil, :] = orientations
self.store_coil += 1
elif in_mdf.tool_id == (self.plate + 1):
self.store_plate_pos[self.store_plate, :] = positions
self.store_plate_ori[self.store_plate, :] = orientations
self.store_plate += 1
def ahf_wobble(obt):
"""Pointing period by pointing period correction for psi1 and psi2 from
the AHF observation files"""
R_psi1 = qarray.inv(qarray.rotation([0,0,1], private.WOBBLE['psi1_ref']))
R_psi2 = qarray.inv(qarray.rotation([0,1,0], private.WOBBLE['psi2_ref']))
psi1, psi2 = get_ahf_wobble(obt)
R_psi2T = qarray.rotation([0,1,0], psi2)
R_psi1T = qarray.rotation([0,0,1], psi1)
wobble_rotation = qarray.mult(R_psi1T,
qarray.mult(R_psi2T ,
qarray.mult(R_psi2 , R_psi1)
)
)
return wobble_rotation
def inv(self, rad, vec):
rad = parse_channel(rad)
l.info("Rotating to detector %s" % rad)
if self.deaberration:
l.warning("Applying deaberration correction")
vec -= correction.simple_deaberration(vec, self.obt, self.coord)
qarray.norm_inplace(vec)
vec_rad = qarray.rotate(qarray.inv(self.qsatgal_interp), vec)
invsiam = np.linalg.inv(self.siam.get(rad))
# invsiamquat = qarray.inv(qarray.norm(qarray.from_rotmat(self.siam.get(rad))))
# qarray.rotate(invsiamquat, vec_rad)
return np.array([np.dot(invsiam, row) for row in vec_rad])
def ptcor(obt, ptcorfile):
# Boresight rotation of 85 degrees in order to get in inscan-xscan reference frame
q_str_LOS = qarray.rotation(np.array([0,1,0]), np.radians(90-85))
# read variable correction for current OD from file
delta_inscan, delta_xscan = read_ptcor(obt, ptcorfile)
# rotation in inscan-xscan reference frame
qcor = qarray.mult(
qarray.rotation(np.array([0,1,0]), delta_xscan),
qarray.rotation(np.array([1,0,0]), delta_inscan)
)
qcor_tot = qarray.mult(q_str_LOS, qarray.mult(qcor, qarray.inv(q_str_LOS)))
return qcor_tot
def gal2ecl(vec):
return qarray.rotate(qarray.inv(QECL2GAL) , vec)
bore_v[:, 2],
nest=True)
pixels = np.tile(pix_1det, ndet)
# polarization weights
bore_v_proj_ortog = np.hstack([
-bore_v[:, [1]], bore_v[:, [0]],
np.zeros(len(bore_v))[:, np.newaxis]
])
bore_v_proj_ortog /= np.linalg.norm(bore_v_proj_ortog, axis=1)[:,
np.newaxis]
local_north_v = np.cross(bore_v, bore_v_proj_ortog)
bore_quat_inv = qa.inv(bore_quat)
local_north_v_fp = qa.rotate(bore_quat_inv, local_north_v)
psi_ref = np.arccos(qa.arraylist_dot(local_north_v_fp, z_axis).flatten())
psi_ref[np.isnan(psi_ref)] = 0
psi = {
"1A": psi_ref,
"1B": psi_ref + np.pi / 2,
"2A": psi_ref + np.pi / 4,
"2B": psi_ref + np.pi * 3 / 4,
}
del pix_1det, bore_v, rot_spin, bore_quat_inv, local_north_v_fp, local_north_v, bore_v_proj_ortog
pars = {}
pars["base_first"] = 60.0
pars["fsample"] = fsample
def vector_gal2ecl(vecl):
'''Convert arrays from Ecliptic to Galactic'''
l.info('Rotating to Galactic frame')
return qarray.rotate(qarray.inv(QECL2GAL) ,vecl)
def test_inv(self):
np.testing.assert_array_almost_equal(qarray.inv(self.q1) , self.q1inv)
def get_pos(self, Qk, Tk):
Qk = qa.norm(Qk)
Qr = (qa.mult(Qk, qa.inv(self.Qi))).flatten()
pos = (qa.rotate(Qr, self.Xi)).flatten() + Tk
return pos, Qr
