fix(AS5600): convert static variables to instance members

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,20 +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<std::recursive_mutex> 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);
-    static int prev_count = count_;
-    // compute diff
+    // 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)
@@ -199,19 +206,16 @@ 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;
+    // 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;
-    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 +238,13 @@ class As5600 : public BasePeripheral<> {
   void init(std::error_code &ec) {
     std::lock_guard<std::recursive_mutex> 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 +304,6 @@ class As5600 : public BasePeripheral<> {
   std::atomic<int> accumulator_{0};
   std::atomic<float> velocity_rpm_{0};
   std::unique_ptr<Task> task_;
+  uint64_t prev_time_us_{0};
 };
 } // namespace espp