Skip to content

Synching #580

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
rahgirrafi wants to merge 6 commits into from
Closed

Synching #580

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
4d9a7ec
fix: convert static variables to instance members to enable supportin…
rahgirrafi Dec 9, 2025
2d65174
fix(as5600): initialize prev_count_ to prevent incorrect first diff
rahgirrafi Dec 9, 2025
e16c745
refactor(as5600): align implementation with MT6701 encoder
rahgirrafi Dec 10, 2025
e4a496a
Fix (AS5600): Used read_counts() instead of non-existant read() metho…
rahgirrafi Dec 10, 2025
e329de3
Fix (as5600): Wrong counts per revolution
rahgirrafi Dec 22, 2025
8e92378
Fix (as5600): Revert AS5600 timing to esp_timer_get_time()
rahgirrafi Dec 22, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 25 additions & 15 deletions components/as5600/include/as5600.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include <cmath>
#include <functional>

#include "esp_timer.h"
#include "base_peripheral.hpp"
#include "task.hpp"

Expand Down Expand Up @@ -40,9 +41,9 @@ class As5600 : public BasePeripheral<> {
typedef std::function<float(float raw)> 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.
Expand Down Expand Up @@ -162,32 +163,43 @@ class As5600 : public BasePeripheral<> {
int read_count(std::error_code &ec) {
logger_.info("read_count");
std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> 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();
}
Comment on lines +186 to +192
Copy link

Copilot AI Dec 22, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Consider initializing prev_time_us_ to the current time in the init() method before starting the task. This would prevent the first update() call from calculating an incorrect dt value based on the initial value of 0, which would result in a very large time delta.

Suggested change
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();
}
float seconds;
if (prev_time_us_ == 0) {
// First update call: avoid using a huge dt from boot time, fall back to configured period.
seconds = update_period_.count();
} else {
uint64_t dt = now_us - prev_time_us_;
seconds = dt / 1e6f;
if (seconds == 0.0f) {
seconds = update_period_.count();
}
}
prev_time_us_ = now_us;

Copilot uses AI. Check for mistakes.
Comment on lines +190 to +192
Copy link

Copilot AI Dec 22, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The fallback to update_period_.count() when seconds == 0.0f may not provide accurate results on the first call to update(), as prev_time_us_ is initialized to 0. This means the first call will calculate an incorrect (very large) dt value. Consider initializing prev_time_us_ in the constructor or at the start of the first update call to avoid this issue.

Copilot uses AI. Check for mistakes.
// update raw count
auto count = read_count(ec);
if (ec) {
return;
}
count_.store(count);
static int prev_count = 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_;
prev_count_ = count_;
Copy link

Copilot AI Dec 22, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Consider initializing prev_count_ in the init() method after reading the initial count value and before starting the task. This would ensure that the first velocity calculation in update() uses a valid baseline. Currently, prev_count_ is initialized to 0, which could cause an incorrect diff calculation on the first update if the encoder is not at position 0.

Copilot uses AI. Check for mistakes.
// check for zero crossing
if (diff > COUNTS_PER_REVOLUTION / 2) {
// we crossed zero going clockwise (1 -> 359)
Expand All @@ -199,12 +211,7 @@ 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();
auto now = std::chrono::high_resolution_clock::now();
float elapsed = std::chrono::duration<float>(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;
Copy link

Copilot AI Dec 22, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The static variable max_velocity should also be converted to an instance member variable to properly support multiple AS5600 sensor instances, similar to how prev_count_ and prev_time_us_ were converted. Currently, all instances would share the same max_velocity value.

Suggested change
static float max_velocity = 0.5f / update_period_.count() * SECONDS_PER_MINUTE;
float max_velocity = 0.5f / update_period_.count() * SECONDS_PER_MINUTE;

Copilot uses AI. Check for mistakes.
Expand Down Expand Up @@ -298,5 +305,8 @@ class As5600 : public BasePeripheral<> {
std::atomic<int> accumulator_{0};
std::atomic<float> velocity_rpm_{0};
std::unique_ptr<Task> task_;
// Instance-specific variables (not static) to support multiple AS5600 sensors
int prev_count_{0};
uint64_t prev_time_us_{0};
Comment on lines +309 to +310
Copy link

Copilot AI Dec 22, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The member variables prev_count_ and prev_time_us_ are accessed without mutex protection, but they're modified inside the update() method which is protected by base_mutex_. If update() can be called from multiple threads or if these variables are accessed from other methods, this could lead to race conditions. Consider using std::atomic for these variables or ensuring all accesses are protected by the mutex.

Suggested change
int prev_count_{0};
uint64_t prev_time_us_{0};
std::atomic<int> prev_count_{0};
std::atomic<uint64_t> prev_time_us_{0};

Copilot uses AI. Check for mistakes.
};
} // namespace espp