def sendInitMessage(self, pos, yaw):
        init = indexed_measurement_t()
        init.utime = getUtime()
        init.state_utime = init.utime
        init.measured_dim = 4
        init.z_effective = [ pos[0], pos[1], pos[2] , yaw ]
        init.z_indices = [9, 10, 11, 8]

        init.measured_cov_dim = init.measured_dim*init.measured_dim
        init.R_effective= [0] * init.measured_cov_dim
        init.R_effective[0]  = 0.25
        init.R_effective[5]  = 0.25
        init.R_effective[10] = 0.25
        init.R_effective[15] =  math.pow( 50*math.pi/180 , 2 )

        lcmUtils.publish('MAV_STATE_EST_VIEWER_MEASUREMENT', init)
Beispiel #2
0
    def sendInitMessage(self, pos, yaw):
        init = indexed_measurement_t()
        init.utime = getUtime()
        init.state_utime = init.utime
        init.measured_dim = 4
        init.z_effective = [pos[0], pos[1], pos[2], yaw]
        init.z_indices = [9, 10, 11, 8]

        init.measured_cov_dim = init.measured_dim * init.measured_dim
        init.R_effective = [0] * init.measured_cov_dim
        init.R_effective[0] = 0.25
        init.R_effective[5] = 0.25
        init.R_effective[10] = 0.25
        init.R_effective[15] = math.pow(50 * math.pi / 180, 2)

        lcmUtils.publish('MAV_STATE_EST_VIEWER_MEASUREMENT', init)
Beispiel #3
0
########################################################################################
def timestamp_now (): return int (time.time () * 1000000)

lc = lcm.LCM()
print "0 started"

o = utime_t()
o.utime = timestamp_now ()
lc.publish("STATE_EST_RESTART",o.encode())  
print '1 sent STATE_EST_RESTART'
time.sleep(1) # Time in seconds.

####################################################################
o = utime_t()
o.utime = timestamp_now ()
lc.publish("STATE_EST_READY",o.encode())  
print '2 sent STATE_EST_READY'
time.sleep(1) # Time in seconds.

####################################################################
m = indexed_measurement_t()
m.utime = timestamp_now ()
m.state_utime = m.utime
m.measured_dim = 4
m.z_effective = [0,0,0.85,0]
m.z_indices = [9, 10, 11, 8]
m.measured_cov_dim = 16
m.R_effective = [0.25, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0.75]
lc.publish("MAV_STATE_EST_VIEWER_MEASUREMENT",m.encode())  
print '3 sent MAV_STATE_EST_VIEWER_MEASUREMENT'
Beispiel #4
0
    return int(time.time() * 1000000)


lc = lcm.LCM()
print "0 started"

o = utime_t()
o.utime = timestamp_now()
lc.publish("STATE_EST_RESTART", o.encode())
print '1 sent STATE_EST_RESTART'
time.sleep(1)  # Time in seconds.

####################################################################
o = utime_t()
o.utime = timestamp_now()
lc.publish("STATE_EST_READY", o.encode())
print '2 sent STATE_EST_READY'
time.sleep(1)  # Time in seconds.

####################################################################
m = indexed_measurement_t()
m.utime = timestamp_now()
m.state_utime = m.utime
m.measured_dim = 4
m.z_effective = [0, 0, 0.85, 0]
m.z_indices = [9, 10, 11, 8]
m.measured_cov_dim = 16
m.R_effective = [0.25, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0.75]
lc.publish("MAV_STATE_EST_VIEWER_MEASUREMENT", m.encode())
print '3 sent MAV_STATE_EST_VIEWER_MEASUREMENT'