Esempio n. 1
0
    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)
Esempio n. 2
0
	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)
Esempio n. 3
0
    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))
Esempio n. 4
0
 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))
Esempio n. 5
0
    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])
Esempio n. 6
0
    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()
Esempio n. 7
0
    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])
Esempio n. 8
0
    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()
Esempio n. 9
0
    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)
Esempio n. 10
0
	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
Esempio n. 11
0
    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()
Esempio n. 13
0
    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)
Esempio n. 14
0
    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)
Esempio n. 15
0
    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)
Esempio n. 16
0
    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))))
Esempio n. 19
0
    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()
Esempio n. 21
0
 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)