def msg_handler(self, p):
length = pmt.length(p)
for i in range(0,length):
element = pmt.nth(i, p)
key = pmt.nth(0, element)
value = pmt.nth(1, element)
if str(key) == "power":
output = pmt.f32vector_elements(value)[0]
output = 10 * math.log(output, 10)
output = pmt.make_f32vector(1, output)
if i==0:
outpmt = pmt.list1(pmt.list2(pmt.string_to_symbol(str(key) + "_dB"), output))
else:
outpmt = pmt.list_add(outpmt, pmt.list2(pmt.string_to_symbol(str(key) + "_dB"), output))
output = pmt.nth(1, element)
if i==0:
outpmt = pmt.list1(pmt.list2(key, output))
else:
outpmt = pmt.list_add(outpmt, pmt.list2(key, output))
self.message_port_pub(pmt.string_to_symbol("out"), outpmt)
def test_001_t (self):
# set up pmt
in_2 = (-1,2,-3,4,5,0)
in_3 = (1,2,3,4,5,0)
in_4 = (1,-2.1,3,-4.2,5.9,0)
pmt_1 = pmt.list2(pmt.string_to_symbol("rx_time"),pmt.from_long(0))
pmt_2 = pmt.list2(pmt.string_to_symbol("test"),pmt.init_f32vector(6,in_2))
pmt_3 = pmt.list2(pmt.string_to_symbol("velocity"),pmt.init_f32vector(6,in_3))
pmt_4 = pmt.list2(pmt.string_to_symbol("test2"),pmt.init_f32vector(6,in_4))
pmt_in = pmt.list4(pmt_1,pmt_2,pmt_3,pmt_4)
# set up fg
symbols = ("test", "test2")
const_add = (1, -2)
const_mult = (-5, 1)
strobe = blocks.message_strobe(pmt_in,400);
test = radar.msg_manipulator(symbols,const_add,const_mult)
debug = blocks.message_debug()
self.tb.msg_connect(strobe, "strobe", test, "Msg in")
self.tb.msg_connect(test, "Msg out", debug, "store")
self.tb.msg_connect(test, "Msg out", debug, "print")
# run fg
self.tb.start()
sleep(0.5)
self.tb.stop()
self.tb.wait()
# check data
msg = debug.get_message(0)
out_2 = pmt.f32vector_elements(pmt.nth(1,pmt.nth(1,msg)))
out_3 = pmt.f32vector_elements(pmt.nth(1,pmt.nth(2,msg)))
out_4 = pmt.f32vector_elements(pmt.nth(1,pmt.nth(3,msg)))
ref_2 = [0]*6
ref_3 = [0]*6
ref_4 = [0]*6
for k in range(6):
ref_2[k] = (in_2[k]+const_add[0])*const_mult[0]
ref_3[k] = in_3[k]
ref_4[k] = (in_4[k]+const_add[1])*const_mult[1]
for k in range(6): # do asserts
self.assertAlmostEqual(ref_2[k],out_2[k],3)
self.assertAlmostEqual(ref_3[k],out_3[k],3)
self.assertAlmostEqual(ref_4[k],out_4[k],3)
def test_001_t(self):
data = [ord('t'), ord('e'), ord('s'), ord('t')]
msg = pmt.list1(
pmt.list2(pmt.string_to_symbol("msg_clear"),
pmt.init_u8vector(len(data), data)))
filename_key = "secret.key"
nacl.generate_key(filename_key)
strobe = blocks.message_strobe(msg, 100)
encrypt_secret = nacl.encrypt_secret(filename_key)
debug = blocks.message_debug()
self.tb.msg_connect(strobe, "strobe", encrypt_secret, "Msg clear")
self.tb.msg_connect(encrypt_secret, "Msg encrypted", debug, "store")
self.tb.start()
sleep(0.15)
self.tb.stop()
self.tb.wait()
# check results
msg_stored = debug.get_message(0)
nonce = pmt.nth(0, msg_stored)
msg_encrypted = pmt.nth(1, msg_stored)
print pmt.symbol_to_string(pmt.nth(0, nonce)), pmt.u8vector_elements(
pmt.nth(1, nonce))
print pmt.symbol_to_string(pmt.nth(
0,
msg_encrypted)), pmt.u8vector_elements(pmt.nth(1, msg_encrypted))
def test_001_t (self):
data = [ord('t'),ord('e'),ord('s'),ord('t')]
msg = pmt.list1(pmt.list2(pmt.string_to_symbol("msg_clear"),pmt.init_u8vector(len(data),data)))
filename_sk = "secret.key"
filename_pk = "public.key"
nacl.generate_keypair(filename_sk,filename_pk)
strobe = blocks.message_strobe(msg, 100)
encrypt_public = nacl.encrypt_public(filename_pk,filename_sk)
debug = blocks.message_debug()
self.tb.msg_connect(strobe,"strobe",encrypt_public,"Msg clear")
self.tb.msg_connect(encrypt_public,"Msg encrypted",debug,"store")
self.tb.start()
sleep(0.15)
self.tb.stop()
self.tb.wait()
# check results
msg_stored = debug.get_message(0)
nonce = pmt.nth(0,msg_stored)
msg_encrypted = pmt.nth(1,msg_stored)
print pmt.symbol_to_string(pmt.nth(0,nonce)), pmt.u8vector_elements(pmt.nth(1,nonce))
print pmt.symbol_to_string(pmt.nth(0,msg_encrypted)), pmt.u8vector_elements(pmt.nth(1,msg_encrypted))
def test_001_t(self):
data = [ord("a"), ord("b"), ord("c"), ord("d")]
msg = pmt.list1(pmt.list2(pmt.string_to_symbol("msg_clear"), pmt.init_u8vector(len(data), data)))
filename_key = "secret.a"
nacl.generate_key(filename_key)
strobe = blocks.message_strobe(msg, 100)
encrypt_secret = nacl.encrypt_secret(filename_key)
decrypt_secret = nacl.decrypt_secret(filename_key)
debug = blocks.message_debug()
self.tb.msg_connect(strobe, "strobe", encrypt_secret, "Msg clear")
self.tb.msg_connect(encrypt_secret, "Msg encrypted", decrypt_secret, "Msg encrypted")
self.tb.msg_connect(decrypt_secret, "Msg decrypted", debug, "store")
self.tb.start()
sleep(0.15)
self.tb.stop()
self.tb.wait()
# check results
msg_out = debug.get_message(0)
msg_symbol = pmt.symbol_to_string(pmt.nth(0, pmt.nth(0, msg_out)))
msg_decrypted = pmt.u8vector_elements(pmt.nth(1, pmt.nth(0, msg_out)))
print msg_symbol, msg_decrypted
print "msg_clear", data
for k in range(len(data)):
self.assertEqual(data[k], msg_decrypted[k])
def test_001_t(self):
# set up message
msg1 = pmt.list3(pmt.string_to_symbol('id'),
pmt.string_to_symbol('int'), pmt.from_long(42))
msg2 = pmt.list3(pmt.string_to_symbol('value'),
pmt.string_to_symbol('float'), pmt.from_float(3.1416))
#msg3 = pmt.list3(pmt.string_to_symbol('text'),pmt.string_to_symbol('string'),pmt.string_to_symbol('some text'))
msg = pmt.list2(msg1, msg2)
# set up sql connection
host = '127.0.0.1'
port = 0 # default
user = '******'
password = '******'
database = 'my_db'
table = 'my_table'
# set up flowgraph
if 0: # Enable and disable here
msg_src = blocks.message_strobe(msg, 100)
sql_connector = sql.msg_to_table(user, password, database, table,
host, port)
self.tb.msg_connect(msg_src, 'strobe', sql_connector, 'Msg in')
# run flowgraph
self.tb.start()
sleep(0.2)
self.tb.stop()
self.tb.wait()
def test_001_t(self):
data = [ord('a'), ord('b'), ord('c'), ord('d')]
msg = pmt.list1(
pmt.list2(pmt.string_to_symbol("msg_clear"),
pmt.init_u8vector(len(data), data)))
filename_key = "secret.a"
nacl.generate_key(filename_key)
strobe = blocks.message_strobe(msg, 100)
encrypt_secret = nacl.encrypt_secret(filename_key)
decrypt_secret = nacl.decrypt_secret(filename_key)
debug = blocks.message_debug()
self.tb.msg_connect(strobe, "strobe", encrypt_secret, "Msg clear")
self.tb.msg_connect(encrypt_secret, "Msg encrypted", decrypt_secret,
"Msg encrypted")
self.tb.msg_connect(decrypt_secret, "Msg decrypted", debug, "store")
self.tb.start()
sleep(0.15)
self.tb.stop()
self.tb.wait()
# check results
msg_out = debug.get_message(0)
msg_symbol = pmt.symbol_to_string(pmt.nth(0, pmt.nth(0, msg_out)))
msg_decrypted = pmt.u8vector_elements(pmt.nth(1, pmt.nth(0, msg_out)))
print msg_symbol, msg_decrypted
print "msg_clear", data
for k in range(len(data)):
self.assertEqual(data[k], msg_decrypted[k])
def test_001_t (self):
# set up message
msg1 = pmt.list3(pmt.string_to_symbol('id'),pmt.string_to_symbol('int'),pmt.from_long(42))
msg2 = pmt.list3(pmt.string_to_symbol('value'),pmt.string_to_symbol('float'),pmt.from_float(3.1416))
#msg3 = pmt.list3(pmt.string_to_symbol('text'),pmt.string_to_symbol('string'),pmt.string_to_symbol('some text'))
msg = pmt.list2(msg1,msg2)
# set up sql connection
host = '127.0.0.1'
port = 0 # default
user = '******'
password = '******'
database = 'my_db'
table = 'my_table'
# set up flowgraph
if 0: # Enable and disable here
msg_src = blocks.message_strobe(msg,100)
sql_connector = sql.msg_to_table(user,password,database,table,host,port)
self.tb.msg_connect(msg_src,'strobe',sql_connector,'Msg in')
# run flowgraph
self.tb.start()
sleep(0.2)
self.tb.stop()
self.tb.wait()
def msg_handler(self, p):
length = pmt.length(p)
# iterate over all elements
for i in range(0, length):
element = pmt.nth(i, p)
key = pmt.nth(0, element)
value = pmt.nth(1, element)
initial_phase = None
# when we found the phase key
if str(key) == "phase" and pmt.length(value) >= 1:
# print "length of vector: ", pmt.length(value)
value = pmt.f32vector_elements(value)[0]
self.last_val = value
# save that value
initial_phase = value
value = value - self.subtract
while value < -numpy.pi:
value = value + numpy.pi
while value > numpy.pi:
value = value - numpy.pi
initial_phase = pmt.make_f32vector(1, initial_phase)
initial_phase = pmt.list2(pmt.string_to_symbol("phase_initial"), initial_phase)
outvalue = pmt.make_f32vector(1, value)
output = pmt.list2(pmt.string_to_symbol(str(key)), outvalue)
else:
output = element
if i == 0:
outpmt = pmt.list1(output)
else:
outpmt = pmt.list_add(outpmt, output)
if initial_phase != None:
outpmt = pmt.list_add(outpmt, initial_phase)
self.message_port_pub(pmt.string_to_symbol("out"), outpmt)
def test_001_t (self):
# set up fg
num_mean = 1
center_freq = 1
antenna_gain_tx = 1
antenna_gain_rx = 1
usrp_gain_rx = 1
amplitude = 1
corr_factor = 1
exponent = 1
Range = (10, 20, 30)
power = (15, 30, 45)
pmt_range = pmt.list2(pmt.string_to_symbol('range'),pmt.init_f32vector(len(Range),Range))
pmt_power = pmt.list2(pmt.string_to_symbol('power'),pmt.init_f32vector(len(power),power))
pmt_misc = pmt.list2(pmt.string_to_symbol('misc'),pmt.init_f32vector(3,(1,2,3)))
pmt_in = pmt.list3(pmt_misc,pmt_range,pmt_power)
src = blocks.message_strobe(pmt_in, 300)
est = radar.estimator_rcs(num_mean, center_freq, antenna_gain_tx, antenna_gain_rx, usrp_gain_rx, amplitude, corr_factor, exponent)
snk = blocks.message_debug()
self.tb.msg_connect(src,"strobe",est,"Msg in")
self.tb.msg_connect(est,"Msg out",snk,"store")
self.tb.msg_connect(est,"Msg out",snk,"print")
self.tb.start()
sleep(0.5)
self.tb.stop()
self.tb.wait()
# check data
msg = snk.get_message(0)
wavel = 3e8/center_freq
d_antenna_gain_abs_rx = 10**(antenna_gain_rx/10)
d_antenna_gain_abs_tx = 10**(antenna_gain_tx/10)
power_rx = power[0]**exponent / 10**(usrp_gain_rx/10);
power_tx = amplitude
rcs_ref = (4*math.pi)**(3)/(d_antenna_gain_abs_rx*d_antenna_gain_abs_tx*wavel**2)*Range[0]**4*power_rx/power_tx*corr_factor
self.assertAlmostEqual( rcs_ref, pmt.f32vector_ref(pmt.nth(1,(pmt.nth(0,msg))),0), 4 ) # check rcs value
def test_001_t(self):
# set up fg
Range = (2, 4, 22, 23)
velocity = (3, 12, 19, 19)
power = (10, 10, 10, 1) # last value is thrown away with merging peaks
pmt_time = pmt.list2(
pmt.string_to_symbol('rx_time'),
pmt.make_tuple(pmt.from_long(-1), pmt.from_double(0)))
pmt_axisx = pmt.list2(pmt.string_to_symbol('axis_x'),
pmt.init_f32vector(len(Range), Range))
pmt_axisy = pmt.list2(pmt.string_to_symbol('axis_y'),
pmt.init_f32vector(len(velocity), velocity))
pmt_power = pmt.list2(pmt.string_to_symbol('power'),
pmt.init_f32vector(len(power), power))
pmt_in = pmt.list4(pmt_time, pmt_axisx, pmt_axisy, pmt_power)
src = blocks.message_strobe(pmt_in, 300)
est = radar.estimator_ofdm('range', 30, (0, 40, -40, 10), 'velocity',
20, (-5, 5))
snk = blocks.message_debug()
self.tb.msg_connect(src, "strobe", est, "Msg in")
self.tb.msg_connect(est, "Msg out", snk, "store")
self.tb.msg_connect(est, "Msg out", snk, "print")
self.tb.start()
sleep(0.5)
self.tb.stop()
self.tb.wait()
# get ref data
ref_range = (0 + 40 * 2 / 15.0, 0 + 40 * 4 / 15.0,
-40 + 50 * (22 - 15) / 15.0)
ref_velocity = (-5 + 10 * 3 / 20.0, -5 + 10 * 12 / 20.0,
-5 + 10 * 19 / 20.0)
# check data
msg = snk.get_message(0)
val_range = pmt.f32vector_elements(pmt.nth(1, pmt.nth(1, msg)))
val_velocity = pmt.f32vector_elements(pmt.nth(1, pmt.nth(2, msg)))
print val_range
self.assertFloatTuplesAlmostEqual(val_velocity, ref_velocity, 4)
self.assertFloatTuplesAlmostEqual(val_range, ref_range, 4)
def test_001_t (self):
# set up fg
Range = (10, 20, 30)
power = (15, 30, 45)
block_time = 100
pmt_range = pmt.list2(pmt.string_to_symbol('range'),pmt.init_f32vector(len(Range),Range))
pmt_power = pmt.list2(pmt.string_to_symbol('power'),pmt.init_f32vector(len(power),power))
pmt_misc = pmt.list2(pmt.string_to_symbol('misc'),pmt.init_f32vector(3,(1,2,3)))
pmt_in = pmt.list3(pmt_misc,pmt_range,pmt_power)
src = blocks.message_strobe(pmt_in, 100)
test = radar.trigger_command("echo test_command",("Range","power"),(5, 10),(35, 50), block_time)
self.tb.msg_connect(src,"strobe",test,"Msg in")
self.tb.start()
sleep(0.5)
self.tb.stop()
self.tb.wait()
def handle_msg(self, msg):
cell_id = pmt.to_long(msg)
if cell_id == self.cell_id:
return
else:
self.cell_id = cell_id
print self.name(), " cell_id = ", self.cell_id, " generating RS map!"
#print "generate pilot map: cell_id = " + str(cell_id) + "\tant_port = " + str(self.ant_port)
Ncp = 1 # Always 1 for our purposes --> thus it's hard coded
[rs_poss, rs_vals] = self.frame_pilot_value_and_position(self.N_rb_dl, cell_id, Ncp, self.ant_port)
pmt_rs = self.rs_pos_to_pmt(rs_poss)
pmt_vals = self.rs_val_to_pmt(rs_vals)
pmt_pilots = pmt.list2(pmt_rs, pmt_vals)
self.message_port_pub(self.msg_buf_out, pmt_pilots)
def handle_msg(self, msg):
cell_id = pmt.to_long(msg)
if cell_id == self.cell_id:
return
else:
self.cell_id = cell_id
print self.name(), " cell_id = ", self.cell_id, " generating RS map!"
#print "generate pilot map: cell_id = " + str(cell_id) + "\tant_port = " + str(self.ant_port)
Ncp = 1 # Always 1 for our purposes --> thus it's hard coded
[rs_poss, rs_vals] = self.frame_pilot_value_and_position(self.N_rb_dl, cell_id, Ncp, self.ant_port)
pmt_rs = self.rs_pos_to_pmt(rs_poss)
pmt_vals = self.rs_val_to_pmt(rs_vals)
pmt_pilots = pmt.list2(pmt_rs, pmt_vals)
self.message_port_pub(self.msg_buf_out, pmt_pilots)
def msg_handler(self, p):
#os.system("pwd")
#time.sleep(1)
fd = open(self.filename, "r")
s = fd.readline()
fd.close()
#print "Got string ", s
try:
s = float(s)
except ValueError:
s = -200000.0
p = pmt.list_add(p, pmt.list2(pmt.string_to_symbol(self.key), pmt.make_f32vector(1, s)))
#if(s>-100000.0):
self.message_port_pub(pmt.string_to_symbol("out"), p)
def msg_handler(self, p):
self.counter = self.counter + 1
if self.counter == self.d_period:
self.counter = 0
#print "turning ", self.d_degrees_per_trigger, " degrees"
self.angle += self.d_degrees_per_trigger
if self.d_serial_port != "":
self.ttctrl.move_to(self.angle)
if self.angle > self.d_stop:
print "Stopping execution now"
#sys.exit(0)
ang_key = pmt.string_to_symbol("angle")
ang_value = pmt.init_f32vector(1, [self.angle])
ang_pack = pmt.list2(ang_key, ang_value)
#m = pmt.list1(ang_pack)
m = pmt.list4(pmt.nth(0, p), pmt.nth(1, p), pmt.nth(2, p), ang_pack)
#pmt.list_add(m, p)
self.message_port_pub(pmt.string_to_symbol("out"), m)
def test_001_t(self):
# create input data
steps = 200
vec_time = np.linspace(0, 20, steps)
vec_velocity = np.linspace(5, 5, steps)
vec_range = np.linspace(100, 1, steps)
vec_velocity_real = [0] * steps
vec_range_real = [0] * steps
for k in range(steps):
vec_velocity_real[k] = vec_velocity[k]
vec_range_real[k] = vec_range[k]
# random data on trajectory
mu = 0
sigma_vel = 0.5
sigma_rge = 7
for k in range(len(vec_velocity)):
vec_velocity[k] = vec_velocity[k] + random.gauss(mu, sigma_vel)
vec_range[k] = vec_range[k] + random.gauss(mu, sigma_rge)
# set up pmts with zero points
target_pmts = [0] * len(vec_velocity)
#zero_points = (5,12,17)
zero_points = ()
for k in range(len(vec_velocity)):
pmt_time = pmt.list2(
pmt.string_to_symbol("rx_time"),
pmt.make_tuple(pmt.from_long(int(vec_time[k])),
pmt.from_double(vec_time[k] -
int(vec_time[k]))))
if k in zero_points:
vec = [0]
pmt_velocity = pmt.list2(pmt.string_to_symbol("velocity"),
pmt.init_f32vector(1, vec))
pmt_range = pmt.list2(pmt.string_to_symbol("range"),
pmt.init_f32vector(1, vec))
else:
pmt_velocity = pmt.list2(
pmt.string_to_symbol("velocity"),
pmt.init_f32vector(1, (vec_velocity[k], )))
pmt_range = pmt.list2(pmt.string_to_symbol("range"),
pmt.init_f32vector(1, (vec_range[k], )))
target_pmts[k] = pmt.list3(pmt_time, pmt_velocity, pmt_range)
# set up fg
test_duration = 1000 # ms, do not change!
num_particle = 300
std_range_meas = sigma_rge
std_velocity_meas = sigma_vel
std_accel_sys = 0.1
threshold_track = 0.001
threshold_lost = 4
tracking_filter = "particle"
#tracking_filter = "kalman"
# connect multiple strobes for different msgs
src = [0] * len(target_pmts)
for k in range(len(target_pmts)):
src[k] = blocks.message_strobe(
target_pmts[k],
test_duration - 400 + 400 / len(target_pmts) * k)
tracking = radar.tracking_singletarget(num_particle, std_range_meas,
std_velocity_meas,
std_accel_sys, threshold_track,
threshold_lost, tracking_filter)
snk = blocks.message_debug()
for k in range(len(target_pmts)):
self.tb.msg_connect(src[k], "strobe", tracking, "Msg in")
self.tb.msg_connect(tracking, "Msg out", snk, "store")
self.tb.start()
sleep(test_duration / 1000.0)
self.tb.stop()
self.tb.wait
()
# check data
# show_data = False # Toggle visibility of single messages # broken
msg_num = snk.num_messages()
vec_out_range = []
vec_out_velocity = []
for k in range(msg_num):
msg_part = snk.get_message(k)
tme = pmt.nth(0, msg_part) # not used
vel = pmt.nth(1, msg_part)
rgn = pmt.nth(2, msg_part)
vec_out_range.append(pmt.f32vector_elements(pmt.nth(1, rgn))[0])
vec_out_velocity.append(pmt.f32vector_elements(pmt.nth(1, vel))[0])
# if show_data:
# print "msg:", k
# print pmt.symbol_to_string(pmt.nth(0,vel)), pmt.f32vector_elements(pmt.nth(1,vel))[0]
# print pmt.symbol_to_string(pmt.nth(0,rgn)), pmt.f32vector_elements(pmt.nth(1,rgn))[0]
# print
# print "RANGE:"
# print vec_out_range
# print "VELOCITY:"
# print vec_out_velocity
# make plots
show_plots = False # Toggle visibility of plots
if show_plots:
time = range(len(vec_range))
time_out = range(len(vec_out_range))
plt.figure(1)
plt.subplot(211)
marker = '-o'
p1 = plt.plot(time, vec_velocity_real, marker, time, vec_velocity,
marker, time_out, vec_out_velocity, marker)
plt.legend(p1, ["IN velocity real", "IN velocity", "OUT velocity"])
plt.title("VELOCITY")
plt.xlabel('time')
plt.subplot(212)
marker = '-o'
p1 = plt.plot(time, vec_range_real, marker, time, vec_range,
marker, time_out, vec_out_range, marker)
plt.legend(p1, ["IN range real", "IN range", "OUT range"])
plt.title("RANGE")
plt.xlabel('time')
plt.show()
sock = context.socket(zmq.REP)
rc = sock.bind(_ADDR)
#%%
flanks = pmt.make_f32vector(2, 0.0)
pmt.f32vector_set(flanks, 0, np.radians(15))
pmt.f32vector_set(flanks, 1, np.radians(70))
c = pmt.cons(pmt.to_pmt("doa"), flanks)
#%%
sock.recv()
#%%
sock.send(pmt.serialize_str(c))
# %% De acá para arriba anda
key0 = pmt.intern("doa")
val0 = pmt.from_long(123)
val1 = pmt.from_long(234)
val_list = pmt.list2(val0, val1)
pmt.length(val_list)
# %%
a = pmt.make_dict()
# %%
a = pmt.dict_add(a, key0, val_list)
print(pmt.cdr(a))
# %%
pmt.set_car(a, key0)
pmt.set_cdr(a, val_list)
# %%
print(pmt.cons(pmt.to_pmt("doa"), val_list))
# %%
print(
pmt.cons(pmt.to_pmt("doa"),
pmt.list2(pmt.from_long(123), pmt.from_long(234))))
def msg_handler(self, p):
verbose = False
length = pmt.length(p)
if verbose:
print "PMT contrains " + str(length) + " key/value pairs"
for i in range(0,length):
element = pmt.nth(i, p)
key = pmt.nth(0, element)
value = pmt.nth(1, element)
if verbose:
print "Key of " + str(i) + "th element: " + str(key)
print "Value of " + str(i) + "th element: " + str(value)
print
found = False
for j in range(0, len(self.n)):
if str(key) == self.kk[j]:
found = True
if verbose:
print "found the key " + str(key)
#rotate the values, the latest one disappears
self.prev_values[j] = numpy.roll(self.prev_values[j], -1)
#set the vector accordingly
self.prev_values[j][-1] = pmt.f32vector_elements(value)[0]
output = sum(self.prev_values[j])/len(self.prev_values[j])
output = pmt.make_f32vector(1, output)
if i==0:
outpmt = pmt.list1(pmt.list2(pmt.string_to_symbol(str(key) + "_filtered"), output))
else:
outpmt = pmt.list_add(outpmt, pmt.list2(pmt.string_to_symbol(str(key) + "_filtered"), output))
output = pmt.nth(1, element)
if i==0:
outpmt = pmt.list1(pmt.list2(key, output))
else:
outpmt = pmt.list_add(outpmt, pmt.list2(key, output))
if verbose:
print
#iterate over all keys
for i in range(0, len(self.kk)):
minimum = self.prev_values[i][0]
maximum = self.prev_values[i][0]
#iterate over all saved values
for j in range(0, self.n[i]):
if self.prev_values[i][j] < minimum:
minimum = self.prev_values[i][j]
if self.prev_values[i][j] > maximum:
maximum = self.prev_values[i][j]
#print out a min, diff, max for every key
difference = maximum-minimum
outpmt = pmt.list_add(outpmt, pmt.list2(pmt.string_to_symbol("min"), pmt.make_f32vector(1, minimum)))
outpmt = pmt.list_add(outpmt, pmt.list2(pmt.string_to_symbol("max"), pmt.make_f32vector(1, maximum)))
outpmt = pmt.list_add(outpmt, pmt.list2(pmt.string_to_symbol("diff"), pmt.make_f32vector(1, difference)))
self.message_port_pub(pmt.string_to_symbol("out"), outpmt)
def test_001_t (self):
# create input data
steps = 200
vec_time = np.linspace(0,20,steps);
vec_velocity = np.linspace(5,5,steps)
vec_range = np.linspace(100,1,steps)
vec_velocity_real = [0]*steps
vec_range_real = [0]*steps
for k in range(steps):
vec_velocity_real[k] = vec_velocity[k]
vec_range_real[k] = vec_range[k]
# random data on trajectory
mu = 0
sigma_vel = 0.5
sigma_rge = 7
for k in range(len(vec_velocity)):
vec_velocity[k] = vec_velocity[k] + random.gauss(mu,sigma_vel)
vec_range[k] = vec_range[k] + random.gauss(mu,sigma_rge)
# set up pmts with zero points
target_pmts = [0]*len(vec_velocity)
#zero_points = (5,12,17)
zero_points = ()
for k in range(len(vec_velocity)):
pmt_time = pmt.list2(pmt.string_to_symbol("rx_time"),pmt.make_tuple(pmt.from_long(int(vec_time[k])),pmt.from_double(vec_time[k]-int(vec_time[k]))))
if k in zero_points:
vec = [0]
pmt_velocity = pmt.list2(pmt.string_to_symbol("velocity"),pmt.init_f32vector(1,vec))
pmt_range = pmt.list2(pmt.string_to_symbol("range"),pmt.init_f32vector(1,vec))
else:
pmt_velocity = pmt.list2(pmt.string_to_symbol("velocity"),pmt.init_f32vector(1,(vec_velocity[k],)))
pmt_range = pmt.list2(pmt.string_to_symbol("range"),pmt.init_f32vector(1,(vec_range[k],)))
target_pmts[k] = pmt.list3(pmt_time,pmt_velocity,pmt_range)
# set up fg
test_duration = 1000 # ms, do not change!
num_particle = 300
std_range_meas = sigma_rge
std_velocity_meas = sigma_vel
std_accel_sys = 0.1
threshold_track = 0.001
threshold_lost = 4
tracking_filter = "particle"
#tracking_filter = "kalman"
# connect multiple strobes for different msgs
src = [0]*len(target_pmts)
for k in range(len(target_pmts)):
src[k] = blocks.message_strobe(target_pmts[k], test_duration-400+400/len(target_pmts)*k)
tracking = radar.tracking_singletarget(num_particle, std_range_meas, std_velocity_meas, std_accel_sys, threshold_track, threshold_lost, tracking_filter)
snk = blocks.message_debug()
for k in range(len(target_pmts)):
self.tb.msg_connect(src[k],"strobe",tracking,"Msg in")
self.tb.msg_connect(tracking,"Msg out",snk,"store")
self.tb.start()
sleep(test_duration/1000.0)
self.tb.stop()
self.tb.wait
()
# check data
# show_data = False # Toggle visibility of single messages # broken
msg_num = snk.num_messages()
vec_out_range = []
vec_out_velocity = []
for k in range(msg_num):
msg_part = snk.get_message(k)
tme = pmt.nth(0,msg_part) # not used
vel = pmt.nth(1,msg_part)
rgn = pmt.nth(2,msg_part)
vec_out_range.append(pmt.f32vector_elements(pmt.nth(1,rgn))[0])
vec_out_velocity.append(pmt.f32vector_elements(pmt.nth(1,vel))[0])
# if show_data:
# print "msg:", k
# print pmt.symbol_to_string(pmt.nth(0,vel)), pmt.f32vector_elements(pmt.nth(1,vel))[0]
# print pmt.symbol_to_string(pmt.nth(0,rgn)), pmt.f32vector_elements(pmt.nth(1,rgn))[0]
# print
# print "RANGE:"
# print vec_out_range
# print "VELOCITY:"
# print vec_out_velocity
# make plots
show_plots = False # Toggle visibility of plots
if show_plots:
time = range(len(vec_range))
time_out = range(len(vec_out_range))
plt.figure(1)
plt.subplot(211)
marker = '-o'
p1 = plt.plot(time,vec_velocity_real,marker,time,vec_velocity,marker,time_out,vec_out_velocity,marker)
plt.legend(p1,["IN velocity real", "IN velocity", "OUT velocity"])
plt.title("VELOCITY")
plt.xlabel('time')
plt.subplot(212)
marker = '-o'
p1 = plt.plot(time,vec_range_real,marker,time,vec_range,marker,time_out,vec_out_range,marker)
plt.legend(p1,["IN range real","IN range","OUT range"])
plt.title("RANGE")
plt.xlabel('time')
plt.show()
def send_tune(self, freq):
cmd = pmt.cons(pmt.intern("set_center_freq"),
pmt.list2(pmt.from_double(freq), pmt.from_uint64(0)))
#print "tune to", freq
self.message_port_pub(pmt.intern('command'), cmd)
