From 4d9a7ec14e731e3e3c11c0477e50170abcb1008e Mon Sep 17 00:00:00 2001 From: Rahgir Arefin Rafi <112659971+rahgirrafi@users.noreply.github.com> Date: Wed, 10 Dec 2025 02:06:14 +0600 Subject: [PATCH 1/4] fix: convert static variables to instance members to enable supporting sampling multiple sensors Replace static variables `prev_count` and `prev_time` with instance member variables to support multiple AS5600 sensor instances running simultaneously. Previously, all instances would share the same static variables, causing incorrect position and velocity calculations in multi-sensor configurations. - Add `prev_count_` and `prev_time_` as class member variables - Initialize `prev_count_` to 0 and `prev_time_` to current time - Remove static keyword from variables in `update()` method - Ensures each sensor instance maintains independent state --- components/as5600/include/as5600.hpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/components/as5600/include/as5600.hpp b/components/as5600/include/as5600.hpp index d43919274..3e46f2392 100644 --- a/components/as5600/include/as5600.hpp +++ b/components/as5600/include/as5600.hpp @@ -95,6 +95,8 @@ class As5600 : public BasePeripheral<> { */ void initialize(std::error_code &ec) { init(ec); } + + /** * @brief Return whether the sensor has found absolute 0 yet. * @note The AS5600 (using I2C/SPI) does not need to search for absolute 0 @@ -174,6 +176,8 @@ class As5600 : public BasePeripheral<> { return (int)((angle_h << 6) | angle_l); } + + void update(std::error_code &ec) { logger_.info("update"); std::lock_guard lock(base_mutex_); @@ -183,11 +187,10 @@ class As5600 : public BasePeripheral<> { return; } count_.store(count); - static int prev_count = count_; - // compute diff - int diff = count_ - prev_count; + // compute diff + int diff = count_ - prev_count_; // update prev_count - prev_count = count_; + prev_count_ = count_; // check for zero crossing if (diff > COUNTS_PER_REVOLUTION / 2) { // we crossed zero going clockwise (1 -> 359) @@ -199,11 +202,10 @@ class As5600 : public BasePeripheral<> { // update accumulator accumulator_ += diff; logger_.debug("CDA: {}, {}, {}", count_, diff, accumulator_); - // update velocity (filtering it) - static auto prev_time = std::chrono::high_resolution_clock::now(); + // update velocity (filtering it) auto now = std::chrono::high_resolution_clock::now(); - float elapsed = std::chrono::duration(now - prev_time).count(); - prev_time = now; + float elapsed = std::chrono::duration(now - prev_time_).count(); + prev_time_ = now; float seconds = elapsed ? elapsed : update_period_.count(); float raw_velocity = (float)(diff) / COUNTS_PER_REVOLUTION_F / seconds * SECONDS_PER_MINUTE; velocity_rpm_ = velocity_filter_ ? velocity_filter_(raw_velocity) : raw_velocity; @@ -298,5 +300,8 @@ class As5600 : public BasePeripheral<> { std::atomic accumulator_{0}; std::atomic velocity_rpm_{0}; std::unique_ptr task_; + // Instance-specific variables (not static) to support multiple AS5600 sensors + int prev_count_{0}; + std::chrono::high_resolution_clock::time_point prev_time_{std::chrono::high_resolution_clock::now()}; }; } // namespace espp From 2d65174280d381fdd6c7c2562aed38b4b323b33a Mon Sep 17 00:00:00 2001 From: Rahgir Arefin Rafi <112659971+rahgirrafi@users.noreply.github.com> Date: Wed, 10 Dec 2025 02:25:02 +0600 Subject: [PATCH 2/4] fix(as5600): initialize prev_count_ to prevent incorrect first diff Initialize prev_count_ to the first sensor reading in init() to avoid calculating an incorrect diff on the first update() call. Previously, prev_count_ defaulted to 0 while count_ was set to the actual sensor reading, causing a large erroneous jump in the accumulator on startup. - Set prev_count_ = count after reading initial angle - Ensures diff calculation is correct from the first update - Prevents accumulator corruption on initialization --- components/as5600/include/as5600.hpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/components/as5600/include/as5600.hpp b/components/as5600/include/as5600.hpp index 3e46f2392..611dc2d74 100644 --- a/components/as5600/include/as5600.hpp +++ b/components/as5600/include/as5600.hpp @@ -95,8 +95,6 @@ class As5600 : public BasePeripheral<> { */ void initialize(std::error_code &ec) { init(ec); } - - /** * @brief Return whether the sensor has found absolute 0 yet. * @note The AS5600 (using I2C/SPI) does not need to search for absolute 0 @@ -176,8 +174,6 @@ class As5600 : public BasePeripheral<> { return (int)((angle_h << 6) | angle_l); } - - void update(std::error_code &ec) { logger_.info("update"); std::lock_guard lock(base_mutex_); @@ -237,6 +233,7 @@ class As5600 : public BasePeripheral<> { std::lock_guard lock(base_mutex_); // initialize the accumulator to have the current angle auto count = read_count(ec); + prev_count_ = count; if (ec) { return; } From e16c745826da9136571c34b3edc2ccb7f982a948 Mon Sep 17 00:00:00 2001 From: Rahgir Arefin Rafi <112659971+rahgirrafi@users.noreply.github.com> Date: Wed, 10 Dec 2025 11:51:11 +0600 Subject: [PATCH 3/4] refactor(as5600): align implementation with MT6701 encoder - Replace std::chrono::high_resolution_clock with esp_timer_get_time() - Convert prev_time_ to uint64_t prev_time_us_ for microsecond precision - Store prev_count as local variable instead of class member --- components/as5600/include/as5600.hpp | 47 +++++++++++++++------------- 1 file changed, 26 insertions(+), 21 deletions(-) diff --git a/components/as5600/include/as5600.hpp b/components/as5600/include/as5600.hpp index 611dc2d74..141faf52e 100644 --- a/components/as5600/include/as5600.hpp +++ b/components/as5600/include/as5600.hpp @@ -95,6 +95,8 @@ class As5600 : public BasePeripheral<> { */ void initialize(std::error_code &ec) { init(ec); } + + /** * @brief Return whether the sensor has found absolute 0 yet. * @note The AS5600 (using I2C/SPI) does not need to search for absolute 0 @@ -174,19 +176,25 @@ class As5600 : public BasePeripheral<> { return (int)((angle_h << 6) | angle_l); } + + void update(std::error_code &ec) { logger_.info("update"); std::lock_guard lock(base_mutex_); + // measure update timing + uint64_t now_us = esp_timer_get_time(); + auto dt = now_us - prev_time_us_; + float seconds = dt / 1e6f; + prev_time_us_ = now_us; + // store the previous count + int prev_count = count_.load(); // update raw count - auto count = read_count(ec); + read(ec); if (ec) { return; } - count_.store(count); // compute diff - int diff = count_ - prev_count_; - // update prev_count - prev_count_ = count_; + int diff = count_ - prev_count; // check for zero crossing if (diff > COUNTS_PER_REVOLUTION / 2) { // we crossed zero going clockwise (1 -> 359) @@ -199,17 +207,15 @@ class As5600 : public BasePeripheral<> { accumulator_ += diff; logger_.debug("CDA: {}, {}, {}", count_, diff, accumulator_); // update velocity (filtering it) - auto now = std::chrono::high_resolution_clock::now(); - float elapsed = std::chrono::duration(now - prev_time_).count(); - prev_time_ = now; - float seconds = elapsed ? elapsed : update_period_.count(); - float raw_velocity = (float)(diff) / COUNTS_PER_REVOLUTION_F / seconds * SECONDS_PER_MINUTE; + float raw_velocity = (dt > 0) ? (float)(diff) / COUNTS_PER_REVOLUTION_F / seconds * SECONDS_PER_MINUTE : 0.0f; velocity_rpm_ = velocity_filter_ ? velocity_filter_(raw_velocity) : raw_velocity; - static float max_velocity = 0.5f / update_period_.count() * SECONDS_PER_MINUTE; - if (raw_velocity >= max_velocity) { - logger_.warn("Velocity nearing measurement limit ({:.3f} RPM), consider decreasing your " - "update period!", - max_velocity); + if (dt > 0) { + float max_velocity = 0.5f / seconds * SECONDS_PER_MINUTE; + if (raw_velocity >= max_velocity) { + logger_.warn("Velocity nearing measurement limit ({:.3f} RPM), consider decreasing your " + "update period!", + max_velocity); + } } } @@ -232,12 +238,13 @@ class As5600 : public BasePeripheral<> { void init(std::error_code &ec) { std::lock_guard lock(base_mutex_); // initialize the accumulator to have the current angle - auto count = read_count(ec); - prev_count_ = count; + read_count(ec); if (ec) { return; } - accumulator_ = count; + accumulator_ = count_.load(); + // initialize timing + prev_time_us_ = esp_timer_get_time(); // start the task using namespace std::placeholders; task_ = Task::make_unique({ @@ -297,8 +304,6 @@ class As5600 : public BasePeripheral<> { std::atomic accumulator_{0}; std::atomic velocity_rpm_{0}; std::unique_ptr task_; - // Instance-specific variables (not static) to support multiple AS5600 sensors - int prev_count_{0}; - std::chrono::high_resolution_clock::time_point prev_time_{std::chrono::high_resolution_clock::now()}; + uint64_t prev_time_us_{0}; }; } // namespace espp From e4a496a1966d3ec482622dee1332ff87061a1184 Mon Sep 17 00:00:00 2001 From: Rahgir Arefin Rafi <112659971+rahgirrafi@users.noreply.github.com> Date: Wed, 10 Dec 2025 21:38:39 +0600 Subject: [PATCH 4/4] Fix (AS5600): Used read_counts() instead of non-existant read() method and cleaned the code. - Used read_counts() method instead of the wrongly used non-existed read() method - Removed unnecessary trailing white spaces --- components/as5600/include/as5600.hpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/components/as5600/include/as5600.hpp b/components/as5600/include/as5600.hpp index 141faf52e..b586f9c10 100644 --- a/components/as5600/include/as5600.hpp +++ b/components/as5600/include/as5600.hpp @@ -95,8 +95,6 @@ class As5600 : public BasePeripheral<> { */ void initialize(std::error_code &ec) { init(ec); } - - /** * @brief Return whether the sensor has found absolute 0 yet. * @note The AS5600 (using I2C/SPI) does not need to search for absolute 0 @@ -176,8 +174,6 @@ class As5600 : public BasePeripheral<> { return (int)((angle_h << 6) | angle_l); } - - void update(std::error_code &ec) { logger_.info("update"); std::lock_guard lock(base_mutex_); @@ -189,11 +185,12 @@ class As5600 : public BasePeripheral<> { // store the previous count int prev_count = count_.load(); // update raw count - read(ec); + auto count = read_count(ec); if (ec) { return; } - // compute diff + count_.store(count); + // compute diff int diff = count_ - prev_count; // check for zero crossing if (diff > COUNTS_PER_REVOLUTION / 2) { @@ -206,7 +203,7 @@ class As5600 : public BasePeripheral<> { // update accumulator accumulator_ += diff; logger_.debug("CDA: {}, {}, {}", count_, diff, accumulator_); - // update velocity (filtering it) + // update velocity (filtering it) float raw_velocity = (dt > 0) ? (float)(diff) / COUNTS_PER_REVOLUTION_F / seconds * SECONDS_PER_MINUTE : 0.0f; velocity_rpm_ = velocity_filter_ ? velocity_filter_(raw_velocity) : raw_velocity; if (dt > 0) {