def main(name):
while True:
msg = AirComm()
raw = in_socket.recv()
msg.ParseFromString(raw)
#if msg:
# if msg.type == gpos.id:
# gps_nrf = array(gpos.unpack(msg.data)[0:2])
# gps_pos = array([gps_reader.data.lat, gps_reader.data.lon])
# print gps_pos, gps_nrf
# gps_target = array(gps_nrf)
# vec = gps_meters_offset(gps_pos, gps_target)
# ctrl = step(vec)
# print gps_pos, vec, norm(vec), ctrl
# request(i.move_xy(ctrl[0], ctrl[1]))
if msg:
ctrl = array(gpos.unpack(msg.data)[0:2])
request(i.move_xy(ctrl[0], ctrl[1]))
gps_reader = GPS_Reader(sm['gps'])
gps_reader.start()
_client = ICARUS_Client(sm['ctrl'])
i = ICARUS_MissionFactory()
def request(item):
try:
_client.execute(item)
except Exception, e:
print e
#ctrl = array([0.0, 0.0])
inf = Interface(1, '/dev/ttyACM0')
while True:
sleep(0.1)
msg = inf.receive()
if msg:
if 1:#msg.type == POS:
gps_nrf = array(gpos.unpack(msg.data))
gps_pos = array([gps_reader.data.lat, gps_reader.data.lon])
print gps_pos, gps_nrf
gps_target = array(gps_nrf)
vec = gps_meters_offset(gps_pos, gps_target)
ctrl = step(vec)
print gps_pos, vec, norm(vec), ctrl
request(i.move_xy(ctrl[0], ctrl[1]))
