refactor(merge): optimize data discard policy

ekarad1um · ekarad1um · commit fb4c0d27e7c3 · 2025-07-21T14:47:05.000Z
diff --git a/components/acoustics-porting/porting/sensor/i2smic.hpp b/components/acoustics-porting/porting/sensor/i2smic.hpp
@@ -145,33 +145,36 @@ class SensorI2SMic final: public Sensor
         _frame_index = 0;
         _data_bytes_available = 0;
 
-        if (_data_buffer)
+        if (_data_buffer) [[likely]]
         {
-            if (_data_buffer.use_count() > 1)
+            if (_data_buffer.use_count() > 1) [[unlikely]]
             {
                 LOG(ERROR, "Data buffer is already in use by another process");
                 return STATUS(EBUSY, "Data buffer is already in use");
             }
+
             _data_buffer.reset();
         }
 
-        if (_rx_chan)
+        if (_rx_chan) [[likely]]
         {
             auto ret = i2s_channel_disable(_rx_chan);
-            if (ret != ESP_OK)
+            if (ret != ESP_OK) [[unlikely]]
             {
                 LOG(ERROR, "Failed to disable I2S channel: %s", esp_err_to_name(ret));
                 return STATUS(EIO, "Failed to disable I2S channel");
             }
             ret = i2s_del_channel(_rx_chan);
-            if (ret != ESP_OK)
+            if (ret != ESP_OK) [[unlikely]]
             {
                 LOG(ERROR, "Failed to delete I2S channel: %s", esp_err_to_name(ret));
                 return STATUS(EIO, "Failed to delete I2S channel");
             }
             _rx_chan = nullptr;
         }
 
+        _info.status = Status::Uninitialized;
+
         return STATUS_OK();
     }
 
@@ -213,6 +216,12 @@ class SensorI2SMic final: public Sensor
     inline core::Status readDataFrame(core::DataFrame<std::unique_ptr<core::Tensor>> &data_frame,
         size_t batch_size) noexcept override
     {
+        if (batch_size == 0) [[unlikely]]
+        {
+            LOG(ERROR, "Batch size cannot be zero");
+            return STATUS(EINVAL, "Batch size cannot be zero");
+        }
+
         if (!initialized()) [[unlikely]]
         {
             LOG(ERROR, "Sensor is not initialized");
@@ -229,33 +238,21 @@ class SensorI2SMic final: public Sensor
             return STATUS(EBUSY, "Data buffer is already in use");
         }
 
-        if (batch_size == 0) [[unlikely]]
+        if (batch_size > _data_buffer_capacity_frames) [[unlikely]]
         {
-            LOG(ERROR, "Batch size cannot be zero");
-            return STATUS(EINVAL, "Batch size cannot be zero");
+            LOG(WARNING, "Batch size exceeds buffer capacity: %zu > %zu", batch_size, _data_buffer_capacity_frames);
+            batch_size = _data_buffer_capacity_frames;
         }
 
         const std::lock_guard<std::mutex> lock(_lock);
 
         _info.status = Status::Locked;
 
-        const size_t available = internalDataAvailable();
-        if (available > batch_size) [[unlikely]]
-        {
-            [[maybe_unused]] const auto discarded = internalDataDiscard(available - batch_size);
-            LOG(DEBUG, "Discarded %zu staled frames from data buffer", discarded);
-        }
-        while (internalDataAvailable() < batch_size)
-        {
-            vTaskDelay(pdMS_TO_TICKS(5));
-        }
-
         data_frame.timestamp = std::chrono::steady_clock::now();
 
         size_t read = 0;
         {
-            size_t size
-                = std::min(static_cast<size_t>(batch_size * _channels * sizeof(int16_t)), _data_buffer_capacity_bytes);
+            size_t size = batch_size * _channels * sizeof(int16_t);
             auto ret = i2s_channel_read(_rx_chan, _data_buffer.get(), size, &read,
                 pdMS_TO_TICKS((_buffered_duration + 1) * 1000));
             if (ret != ESP_OK) [[unlikely]]
@@ -264,12 +261,12 @@ class SensorI2SMic final: public Sensor
                 _info.status = Status::Idle;
                 return STATUS(EIO, "Failed to read data from I2S channel");
             }
-            internalConsumeData(size);
+            internalConsumeData(read);
 
             if (read != size) [[unlikely]]
             {
                 LOG(WARNING, "Read %zu bytes, expected %zu bytes", read, size);
-                if (read == 0)
+                if (!read) [[unlikely]]
                 {
                     _info.status = Status::Idle;
                     return STATUS(EIO, "No data read from I2S channel");
@@ -278,9 +275,9 @@ class SensorI2SMic final: public Sensor
                 read = batch_size * (_channels * sizeof(int16_t));
             }
         }
-
         data_frame.data = core::Tensor::create(core::Tensor::Type::Int16,
             core::Tensor::Shape(static_cast<int>(batch_size), static_cast<int>(_channels)), _data_buffer, read);
+
         data_frame.index = _frame_index;
         _frame_index += batch_size;
 
@@ -303,7 +300,7 @@ class SensorI2SMic final: public Sensor
         {
             const size_t new_data_bytes_available = std::min(data_bytes_available + size, _data_buffer_capacity_bytes);
             if (_data_bytes_available.compare_exchange_strong(data_bytes_available, new_data_bytes_available,
-                    std::memory_order_release, std::memory_order_relaxed))
+                    std::memory_order_release, std::memory_order_relaxed)) [[likely]]
             {
                 return false;
             }
@@ -339,13 +336,17 @@ class SensorI2SMic final: public Sensor
 
     inline void internalConsumeData(size_t size) noexcept
     {
+        if (!size) [[unlikely]]
+        {
+            return;
+        }
         size_t data_bytes_available = _data_bytes_available.load(std::memory_order_acquire);
         while (1)
         {
             size = std::min(size, data_bytes_available);
             const size_t new_data_bytes_available = data_bytes_available - size;
             if (_data_bytes_available.compare_exchange_strong(data_bytes_available, new_data_bytes_available,
-                    std::memory_order_release, std::memory_order_relaxed))
+                    std::memory_order_release, std::memory_order_relaxed)) [[likely]]
             {
                 return;
             }
diff --git a/components/acoustics-porting/porting/sensor/lis3dhtr.hpp b/components/acoustics-porting/porting/sensor/lis3dhtr.hpp
@@ -30,7 +30,7 @@ class SensorLIS3DHTR final: public Sensor
             CONFIG_OBJECT_DECL_INTEGER("scl", "SCL pin number", 6, 1, 20),
             CONFIG_OBJECT_DECL_INTEGER("fsr", "Full scale range in G (2, 4, 8, 16)", 2, 2, 16),
             CONFIG_OBJECT_DECL_INTEGER("ord", "Output data rate in HZ (1, 10, 25, 50, 100, 200, 400)", 200, 1, 400),
-            CONFIG_OBJECT_DECL_INTEGER("sr", "Sample rate in Hz", 100, 1, 200) };
+            CONFIG_OBJECT_DECL_INTEGER("sr", "Sample rate in Hz", 100, 20, 200) };
     }
 
     SensorLIS3DHTR() noexcept
@@ -133,7 +133,7 @@ class SensorLIS3DHTR final: public Sensor
                 LOG(DEBUG, "Allocated buffer of size: %zu bytes at %p", _buffer_capacity, _buffer.get());
             }
 
-            const TickType_t period = pdMS_TO_TICKS(1000 / sr);
+            _data_period = 1000 / sr;
             if (_timer)
             {
                 auto ret = xTimerDelete(_timer, portMAX_DELAY);
@@ -143,7 +143,7 @@ class SensorLIS3DHTR final: public Sensor
                     return STATUS(EFAULT, "Failed to delete existing timer");
                 }
             }
-            _timer = xTimerCreate("lis3dhtr", period, pdTRUE, nullptr, timerCallback);
+            _timer = xTimerCreate("lis3dhtr", pdMS_TO_TICKS(_data_period), pdTRUE, nullptr, timerCallback);
             if (_timer == nullptr)
             {
                 LOG(ERROR, "Failed to create timer");
@@ -282,6 +282,12 @@ class SensorLIS3DHTR final: public Sensor
     inline core::Status readDataFrame(core::DataFrame<std::unique_ptr<core::Tensor>> &data_frame,
         size_t batch_size) noexcept override
     {
+        if (batch_size == 0) [[unlikely]]
+        {
+            LOG(ERROR, "Batch size cannot be zero");
+            return STATUS(EINVAL, "Batch size cannot be zero");
+        }
+
         if (!initialized()) [[unlikely]]
         {
             LOG(ERROR, "Sensor is not initialized");
@@ -298,11 +304,6 @@ class SensorLIS3DHTR final: public Sensor
             return STATUS(EBUSY, "Data buffer is already in use");
         }
 
-        if (batch_size == 0) [[unlikely]]
-        {
-            LOG(ERROR, "Batch size cannot be zero");
-            return STATUS(EINVAL, "Batch size cannot be zero");
-        }
         if (batch_size > _buffer_capacity) [[unlikely]]
         {
             LOG(WARNING, "Batch size exceeds buffer capacity: %zu > %zu", batch_size, _buffer_capacity);
@@ -318,18 +319,22 @@ class SensorLIS3DHTR final: public Sensor
 
         _info.status = Status::Locked;
 
-        const size_t available = _buffer->size();
-        if (available > batch_size)
-        {
-            [[maybe_unused]] const auto discarded = _buffer->read(nullptr, available - batch_size);
-            LOG(DEBUG, "Discarded %zu staled elements from buffer", discarded);
-        }
-        while (_buffer->size() < batch_size)
+        size_t available = _buffer->size();
+        auto ts = std::chrono::steady_clock::now();
+        while (available < batch_size)
         {
             vTaskDelay(pdMS_TO_TICKS(5));
-        }
+            if (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - ts)
+                > std::chrono::milliseconds(50 * _data_buffer_capacity)) [[unlikely]]
+            {
+                LOG(ERROR, "Timeout while waiting for data, available: %zu, requested: %zu", available, batch_size);
+                _info.status = Status::Idle;
+                return STATUS(ETIMEDOUT, "Timeout while waiting for data");
+            }
 
-        data_frame.timestamp = std::chrono::steady_clock::now();
+            available = _buffer->size();
+        }
+        data_frame.timestamp = ts;
 
         batch_size = _buffer->read(reinterpret_cast<Data *>(_data_buffer.get()), batch_size);
         data_frame.data = core::Tensor::create(core::Tensor::Type::Float32,
@@ -379,15 +384,16 @@ class SensorLIS3DHTR final: public Sensor
 private:
     mutable std::mutex _lock;
     driver::LIS3DHTR *_lis3dhtr = nullptr;
-    static constexpr size_t _buffer_capacity = 2048;
+    static inline constexpr const size_t _buffer_capacity = 2048;
     std::unique_ptr<core::RingBuffer<Data>> _buffer = nullptr;
+    size_t _data_period = 0;
     volatile int _sensor_status = 0;
     TimerHandle_t _timer = nullptr;
     static SensorLIS3DHTR *_this;
 
     size_t _frame_index = 0;
     std::shared_ptr<std::byte[]> _data_buffer = nullptr;
-    static constexpr size_t _data_buffer_capacity = _buffer_capacity / 2;
+    static inline constexpr const size_t _data_buffer_capacity = _buffer_capacity / 2;
 };
 
 SensorLIS3DHTR *SensorLIS3DHTR::_this = nullptr;

Original file line number	Diff line number	Diff line change
`@@ -145,33 +145,36 @@ class SensorI2SMic final: public Sensor`
`145`	`145`	`_frame_index = 0;`
`146`	`146`	`_data_bytes_available = 0;`
`147`	`147`
`148`		`- if (_data_buffer)`
	`148`	`+ if (_data_buffer) [[likely]]`
`149`	`149`	`{`
`150`		`- if (_data_buffer.use_count() > 1)`
	`150`	`+ if (_data_buffer.use_count() > 1) [[unlikely]]`
`151`	`151`	`{`
`152`	`152`	`LOG(ERROR, "Data buffer is already in use by another process");`
`153`	`153`	`return STATUS(EBUSY, "Data buffer is already in use");`
`154`	`154`	`}`
	`155`	`+`
`155`	`156`	`_data_buffer.reset();`
`156`	`157`	`}`
`157`	`158`
`158`		`- if (_rx_chan)`
	`159`	`+ if (_rx_chan) [[likely]]`
`159`	`160`	`{`
`160`	`161`	`auto ret = i2s_channel_disable(_rx_chan);`
`161`		`- if (ret != ESP_OK)`
	`162`	`+ if (ret != ESP_OK) [[unlikely]]`
`162`	`163`	`{`
`163`	`164`	`LOG(ERROR, "Failed to disable I2S channel: %s", esp_err_to_name(ret));`
`164`	`165`	`return STATUS(EIO, "Failed to disable I2S channel");`
`165`	`166`	`}`
`166`	`167`	`ret = i2s_del_channel(_rx_chan);`
`167`		`- if (ret != ESP_OK)`
	`168`	`+ if (ret != ESP_OK) [[unlikely]]`
`168`	`169`	`{`
`169`	`170`	`LOG(ERROR, "Failed to delete I2S channel: %s", esp_err_to_name(ret));`
`170`	`171`	`return STATUS(EIO, "Failed to delete I2S channel");`
`171`	`172`	`}`
`172`	`173`	`_rx_chan = nullptr;`
`173`	`174`	`}`
`174`	`175`
	`176`	`+ _info.status = Status::Uninitialized;`
	`177`	`+`
`175`	`178`	`return STATUS_OK();`
`176`	`179`	`}`
`177`	`180`
`@@ -213,6 +216,12 @@ class SensorI2SMic final: public Sensor`
`213`	`216`	`inline core::Status readDataFrame(core::DataFrame<std::unique_ptr<core::Tensor>> &data_frame,`
`214`	`217`	`size_t batch_size) noexcept override`
`215`	`218`	`{`
	`219`	`+ if (batch_size == 0) [[unlikely]]`
	`220`	`+ {`
	`221`	`+ LOG(ERROR, "Batch size cannot be zero");`
	`222`	`+ return STATUS(EINVAL, "Batch size cannot be zero");`
	`223`	`+ }`
	`224`	`+`
`216`	`225`	`if (!initialized()) [[unlikely]]`
`217`	`226`	`{`
`218`	`227`	`LOG(ERROR, "Sensor is not initialized");`
`@@ -229,33 +238,21 @@ class SensorI2SMic final: public Sensor`
`229`	`238`	`return STATUS(EBUSY, "Data buffer is already in use");`
`230`	`239`	`}`
`231`	`240`
`232`		`- if (batch_size == 0) [[unlikely]]`
	`241`	`+ if (batch_size > _data_buffer_capacity_frames) [[unlikely]]`
`233`	`242`	`{`
`234`		`- LOG(ERROR, "Batch size cannot be zero");`
`235`		`- return STATUS(EINVAL, "Batch size cannot be zero");`
	`243`	`+ LOG(WARNING, "Batch size exceeds buffer capacity: %zu > %zu", batch_size, _data_buffer_capacity_frames);`
	`244`	`+ batch_size = _data_buffer_capacity_frames;`
`236`	`245`	`}`
`237`	`246`
`238`	`247`	`const std::lock_guard<std::mutex> lock(_lock);`
`239`	`248`
`240`	`249`	`_info.status = Status::Locked;`
`241`	`250`
`242`		`- const size_t available = internalDataAvailable();`
`243`		`- if (available > batch_size) [[unlikely]]`
`244`		`- {`
`245`		`- [[maybe_unused]] const auto discarded = internalDataDiscard(available - batch_size);`
`246`		`- LOG(DEBUG, "Discarded %zu staled frames from data buffer", discarded);`
`247`		`- }`
`248`		`- while (internalDataAvailable() < batch_size)`
`249`		`- {`
`250`		`- vTaskDelay(pdMS_TO_TICKS(5));`
`251`		`- }`
`252`		`-`
`253`	`251`	`data_frame.timestamp = std::chrono::steady_clock::now();`
`254`	`252`
`255`	`253`	`size_t read = 0;`
`256`	`254`	`{`
`257`		`- size_t size`
`258`		`- = std::min(static_cast<size_t>(batch_size * _channels * sizeof(int16_t)), _data_buffer_capacity_bytes);`
	`255`	`+ size_t size = batch_size * _channels * sizeof(int16_t);`
`259`	`256`	`auto ret = i2s_channel_read(_rx_chan, _data_buffer.get(), size, &read,`
`260`	`257`	`pdMS_TO_TICKS((_buffered_duration + 1) * 1000));`
`261`	`258`	`if (ret != ESP_OK) [[unlikely]]`
`@@ -264,12 +261,12 @@ class SensorI2SMic final: public Sensor`
`264`	`261`	`_info.status = Status::Idle;`
`265`	`262`	`return STATUS(EIO, "Failed to read data from I2S channel");`
`266`	`263`	`}`
`267`		`- internalConsumeData(size);`
	`264`	`+ internalConsumeData(read);`
`268`	`265`
`269`	`266`	`if (read != size) [[unlikely]]`
`270`	`267`	`{`
`271`	`268`	`LOG(WARNING, "Read %zu bytes, expected %zu bytes", read, size);`
`272`		`- if (read == 0)`
	`269`	`+ if (!read) [[unlikely]]`
`273`	`270`	`{`
`274`	`271`	`_info.status = Status::Idle;`
`275`	`272`	`return STATUS(EIO, "No data read from I2S channel");`
`@@ -278,9 +275,9 @@ class SensorI2SMic final: public Sensor`
`278`	`275`	`read = batch_size * (_channels * sizeof(int16_t));`
`279`	`276`	`}`
`280`	`277`	`}`
`281`		`-`
`282`	`278`	`data_frame.data = core::Tensor::create(core::Tensor::Type::Int16,`
`283`	`279`	`core::Tensor::Shape(static_cast<int>(batch_size), static_cast<int>(_channels)), _data_buffer, read);`
	`280`	`+`
`284`	`281`	`data_frame.index = _frame_index;`
`285`	`282`	`_frame_index += batch_size;`
`286`	`283`
`@@ -303,7 +300,7 @@ class SensorI2SMic final: public Sensor`
`303`	`300`	`{`
`304`	`301`	`const size_t new_data_bytes_available = std::min(data_bytes_available + size, _data_buffer_capacity_bytes);`
`305`	`302`	`if (_data_bytes_available.compare_exchange_strong(data_bytes_available, new_data_bytes_available,`
`306`		`- std::memory_order_release, std::memory_order_relaxed))`
	`303`	`+ std::memory_order_release, std::memory_order_relaxed)) [[likely]]`
`307`	`304`	`{`
`308`	`305`	`return false;`
`309`	`306`	`}`
`@@ -339,13 +336,17 @@ class SensorI2SMic final: public Sensor`
`339`	`336`
`340`	`337`	`inline void internalConsumeData(size_t size) noexcept`
`341`	`338`	`{`
	`339`	`+ if (!size) [[unlikely]]`
	`340`	`+ {`
	`341`	`+ return;`
	`342`	`+ }`
`342`	`343`	`size_t data_bytes_available = _data_bytes_available.load(std::memory_order_acquire);`
`343`	`344`	`while (1)`
`344`	`345`	`{`
`345`	`346`	`size = std::min(size, data_bytes_available);`
`346`	`347`	`const size_t new_data_bytes_available = data_bytes_available - size;`
`347`	`348`	`if (_data_bytes_available.compare_exchange_strong(data_bytes_available, new_data_bytes_available,`
`348`		`- std::memory_order_release, std::memory_order_relaxed))`
	`349`	`+ std::memory_order_release, std::memory_order_relaxed)) [[likely]]`
`349`	`350`	`{`
`350`	`351`	`return;`
`351`	`352`	`}`