diff --git a/components/as5600/include/as5600.hpp b/components/as5600/include/as5600.hpp index d43919274..b586f9c10 100644 --- a/components/as5600/include/as5600.hpp +++ b/components/as5600/include/as5600.hpp @@ -177,17 +177,21 @@ class As5600 : public BasePeripheral<> { 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); - static int prev_count = count_; // compute diff 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) @@ -200,18 +204,15 @@ class As5600 : public BasePeripheral<> { 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(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); + } } } @@ -234,11 +235,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); + 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({ @@ -298,5 +301,6 @@ class As5600 : public BasePeripheral<> { std::atomic accumulator_{0}; std::atomic velocity_rpm_{0}; std::unique_ptr task_; + uint64_t prev_time_us_{0}; }; } // namespace espp