Ejemplo n.º 1
0
      //! Main loop.
      void
      onMain(void)
      {
        Counter<double> m_mon_timer(1.0);

        while (!stopping())
        {
          waitForMessages(1.0);

          if (m_wdog.overflow())
          {
            setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR);
            throw RestartNeeded(Status::getString(Status::CODE_COM_ERROR), c_restart_delay);
          }
          else
          {
            setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
          }

          if (m_mon_timer.overflow())
          {
            m_mon_timer.reset();
            getMonitors();
          }
        }
      }
Ejemplo n.º 2
0
    void
    BasicRemoteOperation::consume(const IMC::ControlLoops* msg)
    {
      if (!(msg->mask & IMC::CL_TELEOPERATION))
        return;

      // If this scope is obsolete, ignore message
      if (msg->scope_ref < m_scope_ref)
        return;

      m_scope_ref = msg->scope_ref;

      if (msg->enable == isActive())
        return;

      if (msg->enable == IMC::ControlLoops::CL_ENABLE)
      {
        requestActivation();
        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
      }
      else
      {
        requestDeactivation();
        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
      }
    }
Ejemplo n.º 3
0
      void
      onMain(void)
      {
        char bfr[32];

        while (!stopping())
        {
          consumeMessages();

          if (m_wdog.overflow())
          {
            setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_COM_ERROR);
            throw RestartNeeded(DTR(Status::getString(CODE_COM_ERROR)), 5);
          }

          if (!Poll::poll(*m_uart, 1.0))
            continue;

          size_t rv = m_uart->readString(bfr, sizeof(bfr));

          if (rv == 0)
            throw RestartNeeded(DTR("I/O error"), 5);

          if (std::sscanf(bfr, "%f", &m_sspeed.value) != 1)
            continue;

          if ((m_sspeed.value < c_min_speed) || (m_sspeed.value > c_max_speed))
            m_sspeed.value = -1;

          setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
          m_wdog.reset();
          dispatch(m_sspeed);
        }
      }
Ejemplo n.º 4
0
  void
  Daemon::measureCpuUsage(void)
  {
    // Measure CPU usage per task.
    m_tman->measureCpuUsage();

    // Dispatch global CPU usage.
    IMC::CpuUsage cpu_usage;
    int value = m_sys_resources.getProcessorUsage();
    if (value >= 0 && value <= 100)
    {
      cpu_usage.value = value;
      dispatch(cpu_usage);

      double cpu_avg = m_cpu_avg->update(value);

      if (cpu_avg >= m_cpu_max_usage)
      {
        setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_CPU_TOO_HIGH);
        m_tman->adjustPriorities();
      }
      else
      {
        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
      }
    }
  }
Ejemplo n.º 5
0
    void
    BasicRemoteOperation::consume(const IMC::ControlLoops* msg)
    {
      if (!(msg->mask & IMC::CL_TELEOPERATION))
        return;

      // If this scope is obsolete, ignore message
      if (msg->scope_ref < m_scope_ref)
        return;

      m_scope_ref = msg->scope_ref;

      if (msg->enable == isActive())
        return;

      if (msg->enable == IMC::ControlLoops::CL_ENABLE)
      {
        requestActivation();
        if (m_teleop_src != 0)
        {
          std::string state = Utils::String::str(DTR("teleoperation by %s"), m_ctx.resolver.resolve(m_teleop_src));
          setEntityState(IMC::EntityState::ESTA_NORMAL, state);
        }
        else
          setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
      }
      else
      {
        requestDeactivation();
        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
      }
    }
Ejemplo n.º 6
0
      bool
      setup(void)
      {
        clear();

        setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_INIT);

        // Stop continuous mode.
        setContinuousMode();
        Delay::wait(1.0);
        m_uart->flushInput();

        // Stop continuous mode and read answer.
        setContinuousMode();
        if (!getMessage(MSG_CONTMODE))
        {
          setEntityState(IMC::EntityState::ESTA_FAILURE,
                         DTR("failed to stop device"));
          return false;
        }

        // Read GyroGainScale.
        queryEEPROM(130);
        if (!getMessage(MSG_EEPROM))
        {
          setEntityState(IMC::EntityState::ESTA_FAILURE,
                         DTR("failed to read EEPROM#130"));
          return false;
        }
        m_gyro_scale = (32768000.0 / m_eeprom);

        // Read AccelGainScale.
        queryEEPROM(230);
        if (!getMessage(MSG_EEPROM))
        {
          setEntityState(IMC::EntityState::ESTA_FAILURE,
                         DTR("failed to read EEPROM#230"));
          return false;
        }
        m_accel_scale = (32768000.0 / m_eeprom);

        // Set continuous mode for gyro-stabilized euler angles.
        setContinuousMode(MSG_GS_EULER);

        // Device is configured.
        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);

        return true;
      }
Ejemplo n.º 7
0
      void
      updateBeacons(void)
      {
        m_beacons.clear();
        IMC::MessageList<IMC::LblBeacon>::const_iterator itr = m_lbl_cfg->beacons.begin();
        for (unsigned i = 0; itr != m_lbl_cfg->beacons.end(); ++itr, ++i)
          addBeacon(i, *itr);
        m_beacon = m_beacons.begin();
        m_ping_time.reset();

        if (isActive())
          setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
        else
          setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
      }
Ejemplo n.º 8
0
 //! Signal a bad vertical mode or incompatible
 //! @param[in] desc description of bad vertical mode
 void
 signalBadVertical(const char* desc = DTR("vertical control mode %d not supported"))
 {
   setEntityState(IMC::EntityState::ESTA_ERROR, Utils::String::str(DTR(desc), m_vertical_mode));
   err("%s", Utils::String::str(DTR(desc), m_vertical_mode).c_str());
   requestDeactivation();
 }
Ejemplo n.º 9
0
    BasicAutopilot::BasicAutopilot(const std::string& name, Tasks::Context& ctx,
                                   const uint32_t controllable_loops, const uint32_t required_loops):
      Tasks::Task(name, ctx),
      m_vertical_mode(VERTICAL_MODE_NONE),
      m_yaw_mode(YAW_MODE_NONE),
      m_aloops(0),
      m_controllable_loops(controllable_loops),
      m_required_loops(required_loops),
      m_scope_ref(0)
    {
      param("Heading Rate Bypass", m_hrate_bypass)
      .defaultValue("false")
      .description("Bypass heading rate controller and use reference directly on torques");

      m_ctx.config.get("General", "Absolute Maximum Depth", "50.0", m_max_depth);

      // Initialize entity state.
      setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);

      // Register handler routines.
      bind<IMC::EstimatedState>(this);
      bind<IMC::DesiredHeadingRate>(this);
      bind<IMC::DesiredHeading>(this);
      bind<IMC::DesiredZ>(this);
      bind<IMC::DesiredPitch>(this);
      bind<IMC::DesiredVelocity>(this);
      bind<IMC::ControlLoops>(this);
    }
Ejemplo n.º 10
0
 void
 onActivation(void)
 {
     inf("%s", DTR(Status::getString(Status::CODE_ACTIVE)));
     setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
     m_activating = false;
 }
Ejemplo n.º 11
0
 void
 BasicDeviceDriver::onActivation(void)
 {
   initializeDevice();
   setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
   debug("activation took %0.2f s", m_wdog.getElapsed());
 }
Ejemplo n.º 12
0
      //! Initialize resources.
      void
      onResourceInitialization(void)
      {
        if (!getConstantParameters())
          throw RestartNeeded(DTR("failed to get constant parameters"), c_restart_delay);

        setConfig();

        std::map<std::string, LED*>::iterator itr = m_led_by_name.begin();
        for (unsigned i = 0; i < c_led_count; ++i)
          setBrightness(itr->second, 0);

        if (!m_args.led_patterns.empty())
        {
          uint8_t count = m_args.led_patterns.size();

          UCTK::Frame frame;
          frame.setId(PKT_ID_LED_PATTERN);
          frame.setPayloadSize(1 + (count * 2));
          frame.set(count, 0);
          for (size_t i = 0; i < count; ++i)
            frame.set<uint16_t>(m_args.led_patterns[i], 1 + i * 2);
          if (!m_ctl->sendFrame(frame))
            throw RestartNeeded(DTR("failed to set LED patterns"), c_restart_delay);
        }

        m_wdog.reset();
        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
      }
Ejemplo n.º 13
0
      //! Request device to ping.
      //! @param[in] data_point data index.
      void
      ping(unsigned data_point)
      {
        m_sdata[SD_PACKET_NUM] = (uint8_t)data_point;
        m_sock.write((char*)m_sdata, c_sdata_size);

        int rv = m_sock.read((char*)m_rdata_hdr, c_rdata_hdr_size);
        if (rv != c_rdata_hdr_size)
          throw std::runtime_error(DTR("failed to read header"));

        unsigned dat_idx = data_point * c_rdata_dat_size;

        if (m_args.save_in_837)
          rv = m_sock.read((char*)(m_frame.getMessageData() + dat_idx), c_rdata_dat_size);
        else
          rv = m_sock.read(&m_ping.data[dat_idx], c_rdata_dat_size);

        if (rv != c_rdata_dat_size)
          throw std::runtime_error(DTR("failed to read data"));

        rv = m_sock.read((char*)m_rdata_ftr, c_rdata_ftr_size);
        if (rv != c_rdata_ftr_size)
          throw std::runtime_error(DTR("failed to read footer"));

        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
      }
Ejemplo n.º 14
0
      void
      consume(const IMC::LblConfig* msg)
      {
        if (msg->op == IMC::LblConfig::OP_SET_CFG)
        {
          // Save message to cache.
          IMC::CacheControl cop;
          cop.op = IMC::CacheControl::COP_STORE;
          cop.message.set(*msg);
          dispatch(cop);

          Memory::replace(m_lbl_cfg, new IMC::LblConfig(*msg));
          if (hasOrigin())
            updateBeacons();
          else
            setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_WAIT_GPS_FIX);
        }
        else if (msg->op == IMC::LblConfig::OP_GET_CFG)
        {
          IMC::LblConfig cfg;
          cfg.op = IMC::LblConfig::OP_CUR_CFG;
          if (m_lbl_cfg != NULL)
            cfg.beacons = m_lbl_cfg->beacons;
          dispatch(cfg);
        }
      }
Ejemplo n.º 15
0
        void
        consume(const IMC::GpsFix* msg)
        {
          if (msg->getSourceEntity() != m_gps_eid)
            return;

          if ((msg->validity & IMC::GpsFix::GFV_VALID_POS) == 0)
          {
            m_gps_rej.reason = IMC::GpsFixRejection::RR_INVALID;
            dispatch(m_gps_rej, DF_KEEP_TIME);
            return;
          }

          // Received valid GPS data.
          setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);

          m_time_without_gps.reset();

          m_estate.lat = msg->lat;
          m_estate.lon = msg->lon;

          // Decompose velocity vector.
          m_estate.vx = std::cos(msg->cog) * msg->sog;
          m_estate.vy = std::sin(msg->cog) * msg->sog;
          m_estate.u = msg->sog;

          dispatch(m_estate);
        }
Ejemplo n.º 16
0
 void
 onVersion(unsigned major, unsigned minor, unsigned patch)
 {
   m_hw_major = major;
   inf(DTR("version: %u.%u.%u"), major, minor, patch);
   setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
 }
Ejemplo n.º 17
0
 void
 BasicDeviceDriver::failActivation(const std::string& message)
 {
   activationFailed(message);
   turnPowerOff();
   setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
 }
Ejemplo n.º 18
0
 //! On activation enter active entity state
 //! Method from parent class
 void
 onActivation(void)
 {
   setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
   reset();
   onAutopilotActivation();
 }
Ejemplo n.º 19
0
 //! On deactivation leave error or active entity state
 //! Method from parent class
 void
 onDeactivation(void)
 {
   setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
   reset();
   onAutopilotDeactivation();
 }
Ejemplo n.º 20
0
 void
 setAndSendState(EntityStates state)
 {
   m_state = state;
   setEntityState((IMC::EntityState::StateEnum)m_states[m_state].state,
                  m_states[m_state].description);
 }
Ejemplo n.º 21
0
 void
 consume(const IMC::SimulatedState* msg)
 {
   setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
   m_active = true;
   m_sstate = *msg;
 }
Ejemplo n.º 22
0
      void
      openDB(void)
      {
        if (m_db != NULL)
          return;

        setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_INIT);

        Path db_file = m_ctx.dir_db / "Plan.db";

        debug("database file: '%s'", db_file.c_str());

        m_db = new Database::Connection(db_file.c_str(), true);
        m_get_plan_stmt = new Database::Statement(c_get_plan_stmt, *m_db);

        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
      }
Ejemplo n.º 23
0
 void
 Daemon::onResourceInitialization(void)
 {
   m_ctx.mbus.resume();
   m_tman->start();
   m_periodic_counter.setTop(1.0);
   setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
 }
Ejemplo n.º 24
0
 void
 onActivation(void)
 {
   connect();
   setupScanList();
   configAnalogueInputs();
   setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
 }
Ejemplo n.º 25
0
 void
 onResourceAcquisition(void)
 {
   setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_INIT);
   m_uart = new SerialPort(m_args.uart_dev, m_args.uart_baud);
   m_uart->setCanonicalInput(true);
   m_uart->flush();
 }
Ejemplo n.º 26
0
    void
    BasicRemoteOperation::consume(const IMC::ControlLoops* msg)
    {
      if (!(msg->mask & IMC::CL_TELEOPERATION) || msg->enable == isActive())
        return;

      if (msg->enable == IMC::ControlLoops::CL_ENABLE)
      {
        activate();
        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
      }
      else
      {
        deactivate();
        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
      }
    }
Ejemplo n.º 27
0
    void
    Maneuver::onDeactivation(void)
    {
      onManeuverDeactivation();
      setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
      debug("disabling");

      unlock();
    }
Ejemplo n.º 28
0
      void
      consume(const IMC::SimulatedState* msg)
      {
        if (!isActive())
        {
          m_vel[0] = msg->u;
          m_vel[1] = msg->v;
          m_vel[2] = msg->w;

          requestActivation();
        }

        // Compute time delta.
        double tstep = m_delta.getDelta();
        // Check if we have a valid time delta.
        if (tstep <= 0)
          return;

        // Define Euler Angles variables and add gaussian noise component.
        if (m_args.euler)
        {
          m_euler.phi = Angles::normalizeRadian(msg->phi + m_prng->gaussian() * Angles::radians(m_args.stdev_euler));
          m_euler.theta = Angles::normalizeRadian(msg->theta + m_prng->gaussian() * Angles::radians(m_args.stdev_euler));
          m_euler.psi_magnetic = Angles::normalizeRadian(msg->psi + m_prng->gaussian() * Angles::radians(m_args.stdev_euler));
          m_euler.psi = Angles::normalizeRadian(m_euler.psi_magnetic + m_heading_offset);

          // Heading offset will increment through time according with gyro rate bias.
          m_heading_offset += Angles::radians(m_args.gyro_bias / 3600) * tstep;

          m_euler.setTimeStamp(msg->getTimeStamp());
          dispatch(m_euler, DF_KEEP_TIME);
        }

        // Define Angular Velocity variables and add gaussian noise component.
        m_agvel.x = Angles::normalizeRadian(msg->p + m_prng->gaussian() * Angles::radians(m_args.stdev_agvel));
        m_agvel.y = Angles::normalizeRadian(msg->q + m_prng->gaussian() * Angles::radians(m_args.stdev_agvel));
        m_agvel.z = Angles::normalizeRadian(msg->r + m_prng->gaussian() * Angles::radians(m_args.stdev_agvel));

        // Compute acceleration values using simulated state velocity fields.
        m_accel.x = (msg->u - m_vel[0]) / tstep;
        m_accel.y = (msg->v - m_vel[1]) / tstep;
        m_accel.z = (msg->w - m_vel[2]) / tstep;

        // Store velocity for next iteration.
        m_vel[0] = msg->u;
        m_vel[1] = msg->v;
        m_vel[2] = msg->w;

        // Define messages timestamp and dispatch them to the bus.
        m_agvel.setTimeStamp(msg->getTimeStamp());
        m_accel.setTimeStamp(msg->getTimeStamp());
        dispatch(m_agvel, DF_KEEP_TIME);
        dispatch(m_accel, DF_KEEP_TIME);

        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
      }
Ejemplo n.º 29
0
      //! Initialize resources.
      void
      onResourceInitialization(void)
      {
        // Initialize RPM values.
        m_rpm.value = 0;
        m_rpm_new = 0;
        m_avg_motor = new MovingAverage<double>(m_args.avg_samples);

        setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
      }
Ejemplo n.º 30
0
      void
      consume(const IMC::EntityControl* msg)
      {
        if (msg->getDestinationEntity() != getEntityId())
          return;

        m_active = (msg->op == IMC::EntityControl::ECO_ACTIVATE);

        if (m_active)
        {
          setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
          if (!m_log_file.is_open())
            m_log_file.open((m_ctx.dir_log / m_log_filename / "Data.837").c_str(), std::ios::binary);
        }
        else
        {
          setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_IDLE);
        }
      }