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/6] 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/6] 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/6] 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/6] 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) { From e329de34bf968534b55a0a50a3f43af7162bbdbd Mon Sep 17 00:00:00 2001 From: Rahgir Arefin Rafi <112659971+rahgirrafi@users.noreply.github.com> Date: Mon, 22 Dec 2025 18:32:22 +0600 Subject: [PATCH 5/6] Fix (as5600): Wrong counts per revolution MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Fix AS5600 count resolution and conversion - Correct COUNTS_PER_REVOLUTION to 4096 (12-bit) for AS5600 encoder - Remove incorrect bit shift in read_count(), now properly combines ANGLE_H and ANGLE_L for full 12-bit value - Update count-to-radian and count-to-degree conversions to match 4096 counts per revolution - Ensures correct angle readings and full 0–360° range from sensor --- components/as5600/include/as5600.hpp | 56 ++++++++++++++-------------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/components/as5600/include/as5600.hpp b/components/as5600/include/as5600.hpp index b586f9c10..2b187a374 100644 --- a/components/as5600/include/as5600.hpp +++ b/components/as5600/include/as5600.hpp @@ -40,9 +40,9 @@ class As5600 : public BasePeripheral<> { typedef std::function velocity_filter_fn; static constexpr int COUNTS_PER_REVOLUTION = - 16384; ///< Int number of counts per revolution for the magnetic encoder. + 4096; ///< Int number of counts per revolution for the magnetic encoder (12-bit). static constexpr float COUNTS_PER_REVOLUTION_F = - 16384.0f; ///< Float number of counts per revolution for the magnetic encoder. + 4096.0f; ///< Float number of counts per revolution for the magnetic encoder (12-bit). static constexpr float COUNTS_TO_RADIANS = 2.0f * (float)(M_PI) / COUNTS_PER_REVOLUTION_F; ///< Conversion factor to convert from count value to radians. @@ -162,36 +162,34 @@ class As5600 : public BasePeripheral<> { int read_count(std::error_code &ec) { logger_.info("read_count"); std::lock_guard lock(base_mutex_); - // read the angle count registers + // read the angle count registers (12-bit value: 0-4095) uint8_t angle_h = read_u8_from_register((uint8_t)Registers::ANGLE_H, ec); if (ec) { return 0; } - uint8_t angle_l = read_u8_from_register((uint8_t)Registers::ANGLE_L, ec) >> 2; + uint8_t angle_l = read_u8_from_register((uint8_t)Registers::ANGLE_L, ec); if (ec) { return 0; } - return (int)((angle_h << 6) | angle_l); + // Combine the high byte (bits 11-4) and low byte (bits 3-0) + // ANGLE_H contains bits [11:8] in the lower nibble + // ANGLE_L contains bits [7:0] + return (int)(((angle_h & 0x0F) << 8) | 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); if (ec) { return; } count_.store(count); - // compute diff - int diff = count_ - prev_count; + // compute diff (use member variable instead of static to support multiple instances) + int diff = count_ - prev_count_; + // update prev_count + prev_count_ = count_; // check for zero crossing if (diff > COUNTS_PER_REVOLUTION / 2) { // we crossed zero going clockwise (1 -> 359) @@ -203,16 +201,18 @@ class As5600 : public BasePeripheral<> { // update accumulator accumulator_ += diff; logger_.debug("CDA: {}, {}, {}", count_, diff, accumulator_); - // update velocity (filtering it) - float raw_velocity = (dt > 0) ? (float)(diff) / COUNTS_PER_REVOLUTION_F / seconds * SECONDS_PER_MINUTE : 0.0f; + // update velocity (filtering it) (use member variable instead of static) + 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; velocity_rpm_ = velocity_filter_ ? velocity_filter_(raw_velocity) : raw_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); - } + 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); } } @@ -235,13 +235,11 @@ class As5600 : public BasePeripheral<> { void init(std::error_code &ec) { std::lock_guard lock(base_mutex_); // initialize the accumulator to have the current angle - read_count(ec); + auto count = read_count(ec); if (ec) { return; } - accumulator_ = count_.load(); - // initialize timing - prev_time_us_ = esp_timer_get_time(); + accumulator_ = count; // start the task using namespace std::placeholders; task_ = Task::make_unique({ @@ -301,6 +299,8 @@ class As5600 : public BasePeripheral<> { std::atomic accumulator_{0}; std::atomic velocity_rpm_{0}; std::unique_ptr task_; - uint64_t prev_time_us_{0}; + // 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 8e923785e0c4ecf98680ab34420cdd8a8caf0977 Mon Sep 17 00:00:00 2001 From: Rahgir Arefin Rafi <112659971+rahgirrafi@users.noreply.github.com> Date: Mon, 22 Dec 2025 18:41:41 +0600 Subject: [PATCH 6/6] Fix (as5600): Revert AS5600 timing to esp_timer_get_time() - Replace std::chrono with ESP32-native esp_timer_get_time() for velocity timing - Use uint64_t microsecond timestamps instead of chrono::time_point - Add esp_timer.h include for ESP-IDF timer functions - Maintain backward compatibility with ESP-IDF toolchain --- components/as5600/include/as5600.hpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/components/as5600/include/as5600.hpp b/components/as5600/include/as5600.hpp index 2b187a374..ba74de4a5 100644 --- a/components/as5600/include/as5600.hpp +++ b/components/as5600/include/as5600.hpp @@ -4,6 +4,7 @@ #include #include +#include "esp_timer.h" #include "base_peripheral.hpp" #include "task.hpp" @@ -180,6 +181,15 @@ class As5600 : public BasePeripheral<> { void update(std::error_code &ec) { logger_.info("update"); std::lock_guard lock(base_mutex_); + // update velocity (filtering it) (use member variable instead of static) + uint64_t now_us = esp_timer_get_time(); + uint64_t dt = now_us - prev_time_us_; + float seconds = dt / 1e6f; + prev_time_us_ = now_us; + + if (seconds == 0.0f) { + seconds = update_period_.count(); + } // update raw count auto count = read_count(ec); if (ec) { @@ -201,11 +211,7 @@ class As5600 : public BasePeripheral<> { // update accumulator accumulator_ += diff; logger_.debug("CDA: {}, {}, {}", count_, diff, accumulator_); - // update velocity (filtering it) (use member variable instead of static) - 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; velocity_rpm_ = velocity_filter_ ? velocity_filter_(raw_velocity) : raw_velocity; static float max_velocity = 0.5f / update_period_.count() * SECONDS_PER_MINUTE; @@ -301,6 +307,6 @@ class As5600 : public BasePeripheral<> { 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