[经验]

【九联科技Unionpi Tiger开发板试用体验】ROS noteic rs485实时读取th10s

jf_25674040

2022-10-19 18:25:48

975 九联科技

1. 硬件连接

2. 部分软件代码

void Th10sUart::buildSensorStatus(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
  if (!serial_->isOpen())
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "IO error");
  }
  else
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "formaldehyde is ok");
  }
}

void Th10sUart::updateDiagnostics()
{
  int max_count = output_hz_;
  diag_count_++;
  if (diag_count_ >= max_count)
  {
    diag_count_ = 0;
    diagnostic_updater_.update();
  }
}

uint16_t Th10sUart::check_crc(const uint8_t *data, uint8_t len)
{
  int i,j;
  uint16_t crc = 0xffff;

  for(i = 0; i < len - 2; i++)
  {
    crc ^= data[i];
    for(j = 0; j < 8; j++)
    {
      if(crc & 0x01)
      {
        crc = crc >> 1;
        crc = crc ^ 0xa001;
      }
      else
      {
        crc = crc >> 1;
      }
    }
  }
  return crc;
}

bool Th10sUart::is_check_crc(uint8_t *data, uint8_t len)
{
  uint16_t crc_data = (uint16_t)data[len - 1] << 8| data[len -2];
  if(check_crc(data, len) == crc_data)
  {
    return true;
  }
  return false;
}

void Th10sUart::update(const ros::TimerEvent& e)
{
  if (serial_->isOpen())
  {
    uint8_t write_buffer[write_buffer_size] =
    {0x10, 0x03, 0x00, 0x00, 0x00, 0x02, 0xc7, 0x4a};

    uint8_t read_buffer[read_buffer_size];

    serial_->write(write_buffer, write_buffer_size);
    ros::Duration(0.25).sleep();

    size_t n = serial_->available();

    if ( n != 0)
    {
      serial_->read(read_buffer, read_buffer_size);
      int16_t humid,thermo;
      /*
      for(int i = 0; i< read_buffer_size; i++)
      {
        ROS_INFO("DATA[%d]: %02x", i, read_buffer[i]);
      }
      */
      if (!is_check_crc(read_buffer, read_buffer_size))
      {
        return;
      }
      thermo = (int16_t)(((uint16_t)read_buffer[3] << 8)| read_buffer[4]);
      humid = (int16_t)(((uint16_t)read_buffer[5] << 8) | read_buffer[6]);
      std_msgs::Float64 msg;
      msg.data = (double)thermo/10.0;//*1.25;
      thermo_pub_.publish(msg);
      msg.data = (double)humid/10.0;//*1.25;
      humid_pub_.publish(msg);
    }
  }
  else
  {
    int max_count = output_hz_ * 5;
    open_count_++;
    if (open_count_ >= max_count)
    {
      open_count_ = 0;
      try
      {
        serial_->open();
      }
      catch (std::exception &e)
      {
        ROS_WARN("open serial port failed");
      }
    }
  }
  updateDiagnostics();
}