def __init__(self, sockets): logfile = user_data_dir + sep + 'ICARUS.log' log_config(filename=logfile, level=DEBUG, format='%(asctime)s - %(levelname)s: %(message)s') log_info('icarus starting up') self.setpoints = [0.0, 0.0, 2.0] # x, y, z self.flight_time = 0 self.icarus_takeover = False self.emergency_land = False self.factory = ICARUS_MissionFactory self.fsm = FlightSM(self.error, self.broadcast, self.takeoff, self.land, self.move, self.stop) self.landing_spots = LandingSpots(3.0) self.pilot = PilotInterface(sockets['pilot'], sockets['mon']) #self.gps_shifter = GPS_Shifter() self.state_emitter = StateEmitter(sockets['state']) self.powerman = PowerMan(sockets['power_ctrl'], sockets['power_mon']) start_daemon_thread(self.power_state_monitor) start_daemon_thread(self.state_time_monitor) start_daemon_thread(self.pilot_monitor) self.activity = DummyActivity() self.activity.start() self.icarus_srv = ICARUS_Server(sockets['ctrl'], self) self.icarus_srv.start() log_info('icarus up and running')
Copyright (C) 2014 Tobias Simon, Ilmenau University of Technology This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. """ from scl import generate_map from pilot_pb2 import * from pilot_interface import PilotInterface from time import sleep from math import sin, cos _map = generate_map('pilot_shell') _ctrl_socket = _map['ctrl'] _mon_socket = _map['mon'] i = PilotInterface(_ctrl_socket, _mon_socket) rad = 5.0 pos = 0.0 while True: pos += 0.01 i.set_ctrl_param(POS_N, cos(pos) * rad + rad) i.set_ctrl_param(POS_E, sin(pos) * rad + rad) sleep(0.2)