def adc_reader(self):
voltage_adc = ADC(self.opcd.get('voltage_adc'))
current_adc = ADC(self.opcd.get('current_adc'))
voltage_lambda = eval(self.opcd.get('adc_2_voltage'))
current_lambda = eval(self.opcd.get('adc_2_current'))
self.current_integral = 0.0
hysteresis = Hysteresis(self.opcd.get('low_voltage_hysteresis'))
while True:
sleep(1)
try:
self.voltage = voltage_lambda(voltage_adc.read())
self.current = current_lambda(current_adc.read())
if self.current < 4.0:
self.current = 0.5
self.current_integral += self.current / 3600
print self.voltage, self.low_battery_voltage
if self.voltage < self.low_battery_voltage:
self.critical = hysteresis.set()
else:
hysteresis.reset()
if self.critical:
if not self.warning_started:
self.warning_started = True
start_daemon_thread(self.battery_warning)
except Exception, e:
log_err(str(e))
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.handle('takeoff')
elif req.type == LAND:
self.fsm.handle('land')
elif req.type == MOVE:
self.fsm.handle('move')
elif req.type == STOP:
self.fsm.handle('stop')
elif req.type == ROT:
self.rotate(arg)
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)
def run(self):
arg = self.icarus.arg
core = self.icarus.core
mon_data = self.icarus.mon_data
params = self.icarus.core.params
if arg.HasField('move_data'):
z_setpoint = arg.move_data.z
if arg.HasField('rel'):
log_warn('rel field ignored for take-off')
if arg.HasField('glob'):
if not arg.glob:
if z_setpoint < core.params.start_alt + mon_data.z + 3.0:
msg = 'absolute z setpoint %f is below current altitude' % z_setpoint
log_err(msg)
raise ValueError(msg)
log_info('taking off to absolute altitude %f' % z_setpoint)
else:
z_setpoint = mon_data.z + z_setpoint
log_info('taking off to relative altitude %f' % z_setpoint)
else:
z_setpoint = self.STD_HOVERING_ALT
try:
core.spin_up()
except:
core.spin_down()
self.fsm.failed()
log_error('could not spin up motors');
return
if self.canceled:
core.spin_down()
log_error('take-off canceled');
return
# "point of no return":
# reset controllers:
core.set_ctrl_param(POS_YAW, mon_data.yaw)
core.set_ctrl_param(POS_X, mon_data.x)
core.set_ctrl_param(POS_Y, mon_data.y)
core.reset_ctrl()
# set new altitude setpoint and stabilize:
core.set_ctrl_param(POS_Z, z_setpoint)
self.stabilize()
self.fsm.done()
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 handle(self, req, local = False):
while not hasattr(self.pilot, 'mon'):
raise ICARUS_Exception('autopilot has no GPS fix')
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.handle('takeoff')
elif req.type == LAND:
self.fsm.handle('land')
elif req.type == MOVE:
self.fsm.handle('move')
elif req.type == STOP:
self.fsm.handle('stop')
elif req.type == ROT:
self.rotate(arg)
def pilot_monitor(self):
log_info('starting pilot state monitor')
rc_timeout = 1.0
return_when_signal_lost = False
self.mon_data = MonData()
last_valid = time()
while True:
self.pilot.mon_read(self.mon_data)
self.kalman_lat, self.kalman_lon = gps_add_meters(
(self.pilot.params.start_lat, self.pilot.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 run(self):
arg = self.icarus.arg
pilot = self.icarus.pilot
mon_data = self.icarus.mon_data
params = self.icarus.pilot.params
if arg.HasField('move_data'):
z_setpoint = arg.move_data.z
if arg.HasField('rel'):
log_warn('rel field ignored for take-off')
if arg.HasField('glob'):
if not arg.glob:
if z_setpoint < pilot.params.start_alt + mon_data.z + 3.0:
msg = 'absolute z setpoint %f is below current altitude' % z_setpoint
log_err(msg)
raise ValueError(msg)
log_info('taking off to absolute altitude %f' % z_setpoint)
else:
z_setpoint = mon_data.z + z_setpoint
log_info('taking off to relative altitude %f' % z_setpoint)
else:
z_setpoint = self.STD_HOVERING_ALT
pilot.start_motors()
if self.canceled:
pilot.stop_motors()
log_error('take-off canceled')
return
# "point of no return":
# reset controllers:
pilot.set_ctrl_param(POS_YAW, mon_data.yaw)
pilot.set_ctrl_param(POS_E, mon_data.e)
pilot.set_ctrl_param(POS_N, mon_data.n)
pilot.reset_ctrl()
# set new altitude setpoint and stabilize:
pilot.set_ctrl_param(POS_U, u_setpoint)
self.stabilize()
self.fsm.handle('done')
def run(self):
arg = self.icarus.arg
pilot = self.icarus.pilot
mon_data = self.icarus.mon_data
params = self.icarus.pilot.params
if arg.HasField('move_data'):
z_setpoint = arg.move_data.z
if arg.HasField('rel'):
log_warn('rel field ignored for take-off')
if arg.HasField('glob'):
if not arg.glob:
if z_setpoint < pilot.params.start_alt + mon_data.z + 3.0:
msg = 'absolute z setpoint %f is below current altitude' % z_setpoint
log_err(msg)
raise ValueError(msg)
log_info('taking off to absolute altitude %f' % z_setpoint)
else:
z_setpoint = mon_data.z + z_setpoint
log_info('taking off to relative altitude %f' % z_setpoint)
else:
z_setpoint = self.STD_HOVERING_ALT
pilot.start_motors()
if self.canceled:
pilot.stop_motors()
log_error('take-off canceled');
return
# "point of no return":
# reset controllers:
pilot.set_ctrl_param(POS_YAW, mon_data.yaw)
pilot.set_ctrl_param(POS_E, mon_data.e)
pilot.set_ctrl_param(POS_N, mon_data.n)
pilot.reset_ctrl()
# set new altitude setpoint and stabilize:
pilot.set_ctrl_param(POS_U, u_setpoint)
self.stabilize()
self.fsm.handle('done')
def error(self, state, event):
msg = 'in state: %s, could not handle event: %s' % (state, event)
log_err(msg)
raise ICARUS_Exception(msg)
def _error(self):
msg = "flight state machine error"
log_err(msg)
raise ICARUS_Exception(msg)
def error(self, state, event):
msg = 'in state: %s, could not handle event: %s' % (state, event)
log_err(msg)
raise ICARUS_Exception(msg)