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);
}
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;
}