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]))