ComClient(boost::asio::io_service& io_service, unsigned int baud, const string& device)
: active_(true), io_service_(io_service), serialPort(io_service, device)
{
if (!serialPort.is_open())
{
cerr << "Failed to open serial port\n";
return;
}
cout << "Open " << device << " successfully.\n";
boost::asio::serial_port_base::baud_rate baud_option(baud);
serialPort.set_option(baud_option); // set the baud rate after the port has been opened
read_start();
}
void read(boost::optional<mil_msgs::VelocityMeasurements> &res, boost::optional<mil_msgs::RangeStamped> &height_res)
{
res = boost::none;
height_res = boost::none;
if (!p.is_open()) // Open serial port if closed
{
open();
return;
}
ByteVec ensemble;
ensemble.resize(4);
// Header ID
if (!read_byte(ensemble[0]))
return;
if (ensemble[0] != 0x7F)
return;
ros::Time stamp = ros::Time::now();
// Data Source ID
if (!read_byte(ensemble[1]))
return;
if (ensemble[1] != 0x7F)
return;
// Size low
if (!read_byte(ensemble[2]))
return;
// Size high
if (!read_byte(ensemble[3]))
return;
uint16_t ensemble_size = getu16le(ensemble.data() + 2);
ensemble.resize(ensemble_size);
for (int i = 4; i < ensemble_size; i++)
{
if (!read_byte(ensemble[i]))
return;
}
uint16_t checksum = 0;
BOOST_FOREACH (uint16_t b, ensemble)
checksum += b;
uint16_t received_checksum;
if (!read_short(received_checksum))
return;
if (received_checksum != checksum)
{
ROS_ERROR_THROTTLE(0.5, "DVL: invalid ensemble checksum. received: %i calculated: %i size: %i", received_checksum,
checksum, ensemble_size);
return;
}
if (ensemble.size() < 6)
return;
for (int dt = 0; dt < ensemble[5]; dt++)
{
int offset = getu16le(ensemble.data() + 6 + 2 * dt);
if (ensemble.size() - offset < 2)
continue;
// Three modes, encoded by the section_id: Bottom Track High Resolution Velocity
// Bottom Track, Bottom Track Range
uint16_t section_id = getu16le(ensemble.data() + offset);
std::vector<double> correlations(4, nan(""));
if (section_id == 0x5803) // Bottom Track High Resolution Velocity
{
if (ensemble.size() - offset < 2 + 4 * 4)
continue;
res = boost::make_optional(mil_msgs::VelocityMeasurements());
res->header.stamp = stamp;
std::vector<geometry_msgs::Vector3> dirs;
{
double tilt = 30 * boost::math::constants::pi<double>() / 180;
double x = sin(tilt);
double z = cos(tilt);
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(-x, 0, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(+x, 0, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, +x, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, -x, -z));
}
// Keep track of which beams didn't return for logging
std::vector<size_t> invalid_beams;
invalid_beams.reserve(4);
for (int i = 0; i < 4; i++)
{
mil_msgs::VelocityMeasurement m;
m.direction = dirs[i];
int32_t vel = gets32le(ensemble.data() + offset + 2 + 4 * i);
m.velocity = -vel * .01e-3;
if (vel == -3276801) // -3276801 indicates no data
{
invalid_beams.push_back(i + 1);
m.velocity = nan("");
}
res->velocity_measurements.push_back(m);
}
// Report a list of invalid beams
if (invalid_beams.size() > 0)
{
std::string to_log{ "DVL: didn't return bottom velocity for beam(s): " };
for (auto beam : invalid_beams)
to_log += std::to_string(beam) + " ";
ROS_ERROR_THROTTLE(0.5, "%s", to_log.c_str());
}
}
else if (section_id == 0x0600) // Bottom Track
{
for (int i = 0; i < 4; i++)
correlations[i] = *(ensemble.data() + offset + 32 + i);
}
else if (section_id == 0x5804) // Bottom Track Range
{
if (ensemble.size() - offset < 2 + 4 * 3)
continue;
if (gets32le(ensemble.data() + offset + 10) <= 0)
{
ROS_ERROR_THROTTLE(0.5, "%s", "DVL: didn't return height over bottom");
continue;
}
height_res = boost::make_optional(mil_msgs::RangeStamped());
height_res->header.stamp = stamp;
height_res->range = gets32le(ensemble.data() + offset + 10) * 0.1e-3;
}
if (res)
{
for (int i = 0; i < 4; i++)
{
res->velocity_measurements[i].correlation = correlations[i];
}
}
}
}