Exemplo n.º 1
0
      void
      task(void)
      {
        if (!isActive())
          return;

        // Valid reading ?
        bool valid = m_args.ir_prob < 0 ? true : m_prng->uniform() >= m_args.ir_prob;

        if (valid)
        {
          // Water velocity.
          m_wvel.x = m_sstate.u + m_prng->gaussian() * m_args.stdev_wvel;
          m_wvel.y = m_sstate.v + m_prng->gaussian() * m_args.stdev_wvel;
          m_wvel.z = m_sstate.w + m_prng->gaussian() * m_args.stdev_wvel;
          m_wvel.validity = (IMC::WaterVelocity::VAL_VEL_X
                             | IMC::WaterVelocity::VAL_VEL_Y
                             | IMC::WaterVelocity::VAL_VEL_Z);

          // Ground Velocity.
          double bf_wx = 0;
          double bf_wy = 0;
          double bf_wz = 0;
          BodyFixedFrame::toBodyFrame(m_sstate.phi, m_sstate.theta, m_sstate.psi,
                                      m_sstate.svx, m_sstate.svy, m_sstate.svz,
                                      &bf_wx, &bf_wy, &bf_wz);
          m_gvel.x = m_sstate.u + m_prng->gaussian() * m_args.stdev_gvel + bf_wx;
          m_gvel.y = m_sstate.v + m_prng->gaussian() * m_args.stdev_gvel + bf_wy;
          m_gvel.z = m_sstate.w + m_prng->gaussian() * m_args.stdev_gvel + bf_wz;
          m_gvel.validity = (IMC::GroundVelocity::VAL_VEL_X
                             | IMC::GroundVelocity::VAL_VEL_Y
                             | IMC::GroundVelocity::VAL_VEL_Z);
        }
        else
        {
          m_wvel.validity = 0;
          m_gvel.validity = 0;
        }

        // Send data to the bus.
        double tstamp = Time::Clock::getSinceEpoch();
        m_wvel.setTimeStamp(tstamp);
        m_gvel.setTimeStamp(tstamp);
        dispatch(m_wvel, DF_KEEP_TIME);
        dispatch(m_gvel, DF_KEEP_TIME);
      }
Exemplo n.º 2
0
      void
      fillGroundVelocity(IMC::GroundVelocity& velocity, double sound_speed)
      {
        velocity.setTimeStamp(m_timestamp);
        velocity.validity = 0;

        velocity.x = -((m_bottom_velocity[1] / 1000.0) * (sound_speed / m_sound_speed));
        if (m_bottom_velocity[1] != -32768)
          velocity.validity |= IMC::GroundVelocity::VAL_VEL_X;
        else
          velocity.x = 0.0;

        velocity.y = -((m_bottom_velocity[0] / 1000.0) * (sound_speed / m_sound_speed));
        if (m_bottom_velocity[0] != -32768)
          velocity.validity |= IMC::GroundVelocity::VAL_VEL_Y;
        else
          velocity.y = 0.0;

        velocity.z = -((m_bottom_velocity[2] / 1000.0) * (sound_speed / m_sound_speed));
        if (m_bottom_velocity[2] != -32768)
          velocity.validity |= IMC::GroundVelocity::VAL_VEL_Z;
        else
          velocity.z = 0.0;
      }