.. _program_listing_file_src_iio_buffer.cpp:

Program Listing for File iio_buffer.cpp
=======================================

|exhale_lsh| :ref:`Return to documentation for file <file_src_iio_buffer.cpp>` (``src/iio_buffer.cpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // Copyright 2025 Analog Devices, Inc.
   //
   // Licensed under the Apache License, Version 2.0 (the "License");
   // you may not use this file except in compliance with the License.
   // You may obtain a copy of the License at
   //
   //     http://www.apache.org/licenses/LICENSE-2.0
   //
   // Unless required by applicable law or agreed to in writing, software
   // distributed under the License is distributed on an "AS IS" BASIS,
   // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   // See the License for the specific language governing permissions and
   // limitations under the License.
   
   #include "adi_iio/iio_buffer.hpp"
   
   IIOBuffer::IIOBuffer(std::shared_ptr<IIONode> nh, std::string device_path)
   {
     this->m_nh = nh;
     this->m_device_path = device_path;
     m_canceled = false;
     this->m_buffer = nullptr;
   }
   
   IIOBuffer::~IIOBuffer()
   {
     destroyIIOBuffer();
   
     m_stopThread = true;
     if (m_th.joinable()) {
       m_th.join();
     }
   }
   
   void IIOBuffer::destroyIIOBuffer()
   {
     if (m_buffer) {
       m_canceled = true;
       iio_buffer_cancel(m_buffer);
       std::lock_guard<std::mutex> lock(m_mutex);
       iio_buffer_destroy(m_buffer);
       m_buffer = nullptr;
       RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Destroyed buffer %p", (void *)m_buffer);
     }
   }
   
   bool IIOBuffer::createIIOBuffer(std::string & message)
   {
     std::lock_guard<std::mutex> lock(m_mutex);
     iio_device * dev = iio_context_find_device(m_nh->ctx(), m_device_path.c_str());
     if (dev == nullptr) {
       message = "Device not found";
       RCLCPP_WARN(
         rclcpp::get_logger(
           "rclcpp"), "could not find device \"%s\" - errno %d - %s", m_device_path.c_str(),
         errno, message.c_str());
       return false;
     }
   
     // verify channel count
     if (m_channels.size() == 0) {
       message = "No channels to read";
       return false;
     }
   
     if (m_samples_count == 0) {
       message = "No samples to read";
       return false;
     }
   
     // validate that all channels exist
     for (auto & channel : m_channels) {
       iio_channel * ch = iio_device_find_channel(dev, channel.c_str(), false);
       if (ch == nullptr) {
         message = strerror(-errno);
         RCLCPP_WARN(
           rclcpp::get_logger(
             "rclcpp"), "could not find channel \"%s\" in device \"%s\" - errno %d - %s",
           channel.c_str(), m_device_path.c_str(), errno, message.c_str());
         return false;
       }
     }
   
     // disable all channels
     for (unsigned int i = 0; i < iio_device_get_channels_count(dev); i++) {
       iio_channel_disable(iio_device_get_channel(dev, i));
       RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Disabling channel %d", i);
     }
   
     // enable channels
     for (auto & channel : m_channels) {
       iio_channel * ch = iio_device_find_channel(dev, channel.c_str(), false);
       if (ch == nullptr) {
         message = strerror(-errno);
         RCLCPP_WARN(
           rclcpp::get_logger(
             "rclcpp"), "could not find channel \"%s\" in device \"%s\" - errno %d - %s",
           channel.c_str(), m_device_path.c_str(), errno, message.c_str());
         return false;
       }
       iio_channel_enable(ch);
       RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Enabling channel %s", channel.c_str());
     }
   
     m_canceled = false;
     m_buffer = iio_device_create_buffer(dev, m_samples_count, false);
     if (m_buffer == nullptr) {
       message = strerror(-errno);
       RCLCPP_WARN(
         rclcpp::get_logger("rclcpp"), "could not create buffer in device \"%s\" - errno %d - %s",
         m_device_path.c_str(), errno, message.c_str());
       return false;
     }
   
     m_data.layout.dim.clear();
     m_data.layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
     m_data.layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
     m_data.layout.dim[0].label = "samples";
     m_data.layout.dim[0].size = m_samples_count;
     m_data.layout.dim[0].stride = m_channels.size() * m_samples_count;
     m_data.layout.dim[1].label = "channels";
     m_data.layout.dim[1].size = m_channels.size();
     m_data.layout.dim[1].stride = m_channels.size();
   
   
     RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Created buffer %p", (void *)m_buffer);
     message = "Success";
     return true;
   }
   
   bool IIOBuffer::refill(std::string & message)
   {
     std::lock_guard<std::mutex> lock(m_mutex);
     iio_device * dev = iio_context_find_device(m_nh->ctx(), m_device_path.c_str());
     if (!m_buffer || m_canceled) {
       message = "Buffer not created";
       return false;
     }
   
     ssize_t size = iio_buffer_refill(m_buffer);
     RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Refilled buffer with %ld bytes from HW", size);
   
     if (size < 0) {
       message = strerror(-errno);
       RCLCPP_WARN(
         rclcpp::get_logger("rclcpp"), "could not refill buffer in device \"%s\" - errno %d - %s",
         m_device_path.c_str(), errno, message.c_str());
       return false;
     }
   
     m_data.data.clear();
     for (int i = 0; i < m_samples_count; i++) {
       for (auto & channel : m_channels) {
         iio_channel * ch = iio_device_find_channel(dev, channel.c_str(), false);
   
         uint8_t * base_ptr = reinterpret_cast<uint8_t *>(iio_buffer_first(m_buffer, ch));
         size_t step = iio_buffer_step(m_buffer); // Should be in bytes
         uint8_t * sample = base_ptr + (step * i);
   
         //uint8_t* sample = (((uint8_t*)iio_buffer_first(m_buffer, ch)) + iio_buffer_step(m_buffer) * i);
         int32_t val = 0;
         iio_channel_convert(ch, &val, sample);
         const iio_data_format * fmt = iio_channel_get_data_format(ch);
         // val = val & ((1<<fmt->bits) - 1);
         if (val & (1 << (fmt->bits))) {  // sign extension
           val = val | (~((1 << fmt->bits) - 1));
         }
         m_data.data.push_back(val);
       }
     }
     message = "Success";
     return true;
   }
   
   bool IIOBuffer::push(std::string & message, std_msgs::msg::Int32MultiArray & data)
   {
     std::lock_guard<std::mutex> lock(m_mutex);
     iio_device * dev = iio_context_find_device(m_nh->ctx(), m_device_path.c_str());
     if (!m_buffer) {
       message = "Buffer not created";
       return false;
     }
   
     // get data from multiarray and add it to a memory "arena"
     for (int i = 0; i < m_samples_count; i++) {
       for (size_t j = 0; j < m_channels.size(); j++) {
         iio_channel * ch = iio_device_find_channel(dev, m_channels[j].c_str(), true);
         void * sample = reinterpret_cast<int32_t *>(iio_buffer_first(m_buffer, ch)) +
           iio_buffer_step(m_buffer) * i;
         int32_t val = data.data[i * m_channels.size() + j];
         iio_channel_convert(ch, sample, &val);
       }
     }
   
     iio_buffer_push(m_buffer);
     message = "Success";
     return true;
   }
   
   
   void IIOBuffer::enableTopic(std::string topic_name)
   {
     m_topic_enabled = true;
     m_stopThread = false;
   
     if (topic_name == "") {
       m_topic_name = m_nh->convertAttrPathToTopicName(m_device_path);
     } else {
       m_topic_name = m_nh->convertAttrPathToTopicName(topic_name);
     }
   
   
     m_pub = m_nh->create_publisher<std_msgs::msg::Int32MultiArray>(
       m_topic_name + BUFFER_READ_SUFFIX,
       BUFFER_QOS_QUEUE_SIZE);
     m_th = std::thread(&IIOBuffer::publishingLoop, this);
   }
   // create topic
   // create thread that refills
   // update publishes to topic
   // move Int32MultiArray to class - and unlock mutex to get data in refill service
   
   void IIOBuffer::publishingLoop()
   {
     while (rclcpp::ok() && !m_stopThread) {
       std::string msg;
       std_msgs::msg::Int32MultiArray buffer;
       if (refill(msg)) {
         m_pub->publish(m_data);
       }
       // Sleep to maintain the loop rate
       //std::this_thread::yield();
     }
   }
   
   void IIOBuffer::disableTopic()
   {
     m_topic_enabled = false;
     m_stopThread = true;
   
     if (m_th.joinable()) {
       m_th.join();  // stop thread
     }
     m_pub.reset();  // destroy topic
     RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Disabled IIOBuffer topic");
   }