adc_dump.cpp

Go to the documentation of this file.

#include "toposens_echo_driver/adc_dump.h"
 
#include <ros/ros.h>
 
#include "toposens_echo_driver/lib_utils.h"
 
namespace toposens_echo_driver
{
auto requestAdcDump(RequestAdcDump::Request& req, RequestAdcDump::Response& res) -> bool
{
  bool success = true;
  ROS_INFO("ADC Dump requested! Request: [sensor_id=%d, file_path=%s].", req.sensor_id,
           req.file_path.c_str());
 
  if (!toposens_echo_driver::SetTargetSensor(req.sensor_id))
  {
    res.success = false;
    return false;
  }
 
  success &= RequestADCDump();
 
  ROS_DEBUG("SetParameterSystemSensorMode");
  success &= SetParameterSystemSensorMode(SENSOR_MODE_SINGLE_SHOT_TRANSMIT_LISTEN);
  ROS_DEBUG("RequestMeasurement");
  success &= RequestMeasurement();
  ROS_DEBUG("WaitForSessionStart");
  const auto sender_u16 = WaitForSessionStart();
 
  ROS_DEBUG("WaitForSessionEnd");
  const auto sender_u16_end = WaitForSessionEnd();
  if (sender_u16 != sender_u16_end)
  {
    ROS_WARN("Unexpected: Received different sender IDs for session start and end!");
  }
  ROS_DEBUG("WaitForADCSessionEnd");
  WaitForADCSessionEnd();
 
  ADCDump_t* DumpData_t = GetADCDumpData(sender_u16);
  if (DumpData_t != nullptr)
  {
    success &= saveAdcBlobData(DumpData_t, req.file_path.c_str());
  }
  else
  {
    ROS_ERROR("Could not retrieve ADC Dump Data!");
    success = false;
  }
 
  res.success = success;
  ROS_INFO(
      "ADC Dump request finished! Request: [sensor_id=%d, file_path=%s]. Sending back response: "
      "[success=%d]!",
      req.sensor_id, req.file_path.c_str(), res.success);
 
  return success;
}
 
auto saveAdcBlobData(ADCDump_t* adc_dump, const char* file_path) -> bool
{
  bool success = true;
  uint32_t DumpSize_u32 = adc_dump->ReceivedDumpSize_u32;
 
  // TODO(tobi) Use ofstream instead
  FILE* fptr;
  fptr = fopen(file_path, "wb");
 
  if (fptr == nullptr)
  {
    ROS_ERROR("ADC Dump file open error!");
    success = false;
    return success;
  }
 
  size_t number_of_bytes_written = fwrite(adc_dump->DumpBlob_pu8, 1, DumpSize_u32, fptr);
 
  if (number_of_bytes_written == DumpSize_u32)
  {
    ROS_DEBUG("Save sucessful");
  }
  else
  {
    ROS_ERROR("ADC Dump file write error!");
    success = false;
  }
 
  fclose(fptr);
 
  return success;
}
 
void requestAdcDumpCallback(uint16_t sender_id, uint32_t data_size) {}
 
void adcDumpEndCallback(uint16_t sender_id) {}
 
}  // namespace toposens_echo_driver