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 = flight_sm(self)
self.landing_spots = LandingSpots(3.0)
self.core = CoreInterface(sockets["core"], 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.core_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")
class ICARUS:
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 = flight_sm(self)
self.landing_spots = LandingSpots(3.0)
self.core = CoreInterface(sockets["core"], 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.core_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")
def state_time_monitor(self):
log_info("starting state time monitor")
while True:
if self.fsm._state != flight_Standing:
# count the time for "in-the-air-states":
self.flight_time += 1
else:
# use system uptime here:
up_file = open("/proc/uptime", "r")
up_list = up_file.read().split()
self.uptime = int(up_list[0])
up_file.close()
sleep(1)
def core_monitor(self):
log_info("starting core state monitor")
rc_timeout = 1.0
return_when_signal_lost = False
self.mon_data = MonData()
last_valid = time()
while True:
self.core.mon_read(self.mon_data)
self.kalman_lat, self.kalman_lon = gps_add_meters(
(self.core.params.start_lat, self.core.params.start_lon), self.setpoints[0:2]
)
if self.mon_data.signal_valid:
last_valid = time()
else:
if time() - rc_timeout < last_valid and return_when_signal_lost:
if not self.icarus_takeover:
self.icarus_takeover = True
log_err("invalid RC signal, disabling mission interface")
def power_state_monitor(self):
log_info("starting power state monitor")
while True:
self.power_state = self.powerman.read()
if self.power_state.critical:
log_warn("critical power state: emergency landing")
# disable system interface and take over control:
self.icarus_takeover = True
if not self.emergency_land:
self.emergency_landing()
def move_and_land(self, x, y):
# try to stop and move somewhere
try:
self.fsm.stop()
except:
pass
try:
self.arg = IcarusReq()
self.arg.type = MOVE
self.arg.move_data.p0 = x
self.arg.move_data.p1 = y
self.fsm.move()
except:
pass
try:
self.fsm.land()
except:
pass
def emergency_landing(self):
return # TODO: test
# we don't care about the system's state:
# just try to stop and land it!
try:
self.fsm.stop()
except:
pass
try:
self.fsm.land()
except:
pass
# after emergency landing, the interface will stay locked
# and power circruitry will go offline
def rotate(self, arg):
if len(self.arg.pos) == 1:
self.yaw_target = self.arg.pos[0]
else:
self.yaw_target = self.arg.pos[0], self.arg.pos[1]
def yaw_update_thread(self):
"""
This method/thread is responsible for updating the yaw setpoint of the system.
The code is only executed when the system is in a valid state.
"""
prev_yaw = None
min_rot_z_ground = 1.0
while True:
sleep(1)
# when landing: setting a new rotation setpoint is not allowed:
if self.fsm._state is flight_Landing:
continue
if self.mon_data.z_ground < self.min_rot_z_ground:
print "system is able to rotate"
try:
if isinstance(self.yaw_target, float):
print "fixed yaw mode"
yaw = self.yaw
else:
poi_x = self.yaw_target[0]
poi_y = self.yaw_target[1]
print "POI mode, x =", poi_x, " y =", poi_y
yaw = atan2(self.mon_data.y - poi_y, self.mon_data.x - poi_x)
if prev_yaw != yaw:
print "setting yaw to:", yaw
self.core.set_ctrl_param(POS_YAW, yaw)
prev_yaw = yaw
except AttributeError:
pass
# called by ICARUS protocol driver:
def handle(self, req, local=False):
if not local and self.icarus_takeover:
msg = "request rejected due to emergency take-over"
log_err(msg)
raise ICARUS_Exception(msg)
self.arg = req
if req.type == TAKEOFF:
self.fsm.takeoff()
elif req.type == LAND:
self.fsm.land()
elif req.type == MOVE:
self.fsm.move()
elif req.type == STOP:
self.fsm.stop()
elif req.type == ROT:
self.rotate(arg)
# following _prefix methods are called internally from state machine
# and should not be called explicit (a) externally or (b) internally
def _error(self):
msg = "flight state machine error"
log_err(msg)
raise ICARUS_Exception(msg)
def _broadcast(self):
log_info("new state: %s" % to_string(self.fsm._state))
self.state_emitter.send(self.fsm._state)
def _save_power_activity(self):
log_info("standing")
self.powerman.stand_power()
def _takeoff_activity(self):
log_info("taking off")
self.landing_spots.add((self.mon_data.x, self.mon_data.y))
self.activity.cancel_and_join()
self.powerman.flight_power()
self.yaw_setpoint = self.mon_data.yaw
self.activity = TakeoffActivity(self.fsm, self)
self.activity.start()
def _land_activity(self):
log_info("landing")
self.activity.cancel_and_join()
self.activity = LandActivity(self)
self.activity.start()
def _move_activity(self):
log_info("moving")
self.activity.cancel_and_join()
self.activity = MoveActivity(self)
self.activity.start()
def _stop_activity(self):
log_info("stopping")
self.activity.cancel_and_join()
self.activity = StopActivity(self)
self.activity.start()
