command.cpp

Go to the documentation of this file.

#include <toposens_driver/command.h>

namespace toposens_driver
{
Command::Command(TsParam param, float value)
{
  _param = param;
  memset(&_bytes, '\0', sizeof(_bytes));
  std::string format = "%c%s%05d\r";

  if (param == TsParam::ExternalTemperature)
  {
    if (value >= 0)
      value = std::max(value * 10, value);
    else
      value = std::min(value * 10, value);
  }

  // Clips desired parameter value if they would cause a value overflow in the
  // command message.
  if ((value < MIN_VALUE) || (value > MAX_VALUE))
  {
    ROS_WARN_STREAM("Out of range value "
                    << (param == TsParam::ExternalTemperature ? value / 10 : value)
                    << " clipped to closest limit");
    value = (value < MIN_VALUE) ? MIN_VALUE : MAX_VALUE;
  }
  _value = value;
  std::sprintf(_bytes, format.c_str(), kPrefix, _getKey(param).c_str(), ((int)_value));
}

Command::Command(TsService service)
{
  memset(&_bytes, '\0', sizeof(_bytes));
  std::string format = "%c%s\r";

  _value = 0;
  std::sprintf(_bytes, format.c_str(), kPrefix, _getKey(service).c_str());
}

std::string Command::_getKey(TsParam param)
{
  if (param == TsParam::NumberOfPulses)
    return "sPuls";
  else if (param == TsParam::PeakDetectionWindow)
    return "sPeak";
  else if (param == TsParam::EchoRejectionThreshold)
    return "sReje";
  else if (param == TsParam::ExternalTemperature)
    return "sTemp";
  else if (param == TsParam::NoiseIndicatorThreshold)
    return "sNois";
  else if (param == TsParam::ScanMode)
    return "sMode";
  return "";
}

std::string Command::_getKey(TsService service)
{
  if (service == TsService::FirmwareConfiguration)
    return "gConf";
  else if (service == TsService::FirmwareVersion)
    return "gVers";
  return "";
}

std::string Command::getParamName()
{
  if (this->_param == TsParam::NumberOfPulses)
    return "Number of pulses";
  else if (this->_param == TsParam::PeakDetectionWindow)
    return "Peak detection window";
  else if (this->_param == TsParam::EchoRejectionThreshold)
    return "Echo rejection threshold";
  else if (this->_param == TsParam::ExternalTemperature)
    return "Calibration temperature";
  else if (this->_param == TsParam::NoiseIndicatorThreshold)
    return "Noise indicator threshold";
  else if (this->_param == TsParam::ScanMode)
    return "Scan mode";
  return "Unknown parameter";
}

}  // namespace toposens_driver