def main(name):
sm = generate_map(name)
opcd = OPCD_Interface(sm['opcd_ctrl'], name)
sys_id = opcd.get('id')
out_socket = sm['out']
in_socket = sm['in']
aci = ACI(sys_id, '/dev/ttyACM0')
acr = ACIReader(aci, out_socket)
acr.start()
# read from SCL in socket and send data via NRF
while True:
try:
msg = AirComm()
raw = self.in_socket.recv()
msg.ParseFromString(raw)
aci.send(msg.addr, msg.type, msg.data)
except:
sleep(0.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]))
