Beispiel #1
0
 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))
Beispiel #2
0
 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)
Beispiel #3
0
 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)
Beispiel #4
0
   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()
Beispiel #5
0
 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")
Beispiel #6
0
 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)
Beispiel #7
0
 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')
Beispiel #8
0
    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')
Beispiel #9
0
   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')
Beispiel #10
0
 def error(self, state, event):
     msg = 'in state: %s, could not handle event: %s' % (state, event)
     log_err(msg)
     raise ICARUS_Exception(msg)
Beispiel #11
0
 def _error(self):
     msg = "flight state machine error"
     log_err(msg)
     raise ICARUS_Exception(msg)
Beispiel #12
0
 def error(self, state, event):
    msg = 'in state: %s, could not handle event: %s' % (state, event)
    log_err(msg)
    raise ICARUS_Exception(msg)