fix(bldc_motor): Ensure reading the shaft angle doesnt change the filter / update state

components/bldc_haptics/include/bldc_haptics.hpp

@@ -20,11 +20,11 @@ concept MotorConcept = requires {
       &FOO::move); ///< Move the motor to a new target (position, velocity, or torque depending on
                    ///< the motor control type)
   static_cast<void (FOO::*)(detail::MotionControlType)>(
-      &FOO::set_motion_control_type);                            ///< Set the motion control type
-  static_cast<void (FOO::*)(void)>(&FOO::loop_foc);              ///< Run the FOC loop
-  static_cast<float (FOO::*)(void)>(&FOO::get_shaft_angle);      ///< Get the shaft angle
-  static_cast<float (FOO::*)(void)>(&FOO::get_shaft_velocity);   ///< Get the shaft velocity
-  static_cast<float (FOO::*)(void)>(&FOO::get_electrical_angle); ///< Get the electrical angle
+      &FOO::set_motion_control_type);                             ///< Set the motion control type
+  static_cast<void (FOO::*)(void)>(&FOO::loop_foc);               ///< Run the FOC loop
+  static_cast<float (FOO::*)(void) const>(&FOO::get_shaft_angle); ///< Get the shaft angle
+  static_cast<float (FOO::*)(void) const>(&FOO::get_shaft_velocity); ///< Get the shaft velocity
+  static_cast<float (FOO::*)(void)>(&FOO::get_electrical_angle);     ///< Get the electrical angle
 };
 
 /// @brief Class which creates haptic feedback for the user by vibrating the

components/bldc_motor/include/bldc_motor.hpp

@@ -482,28 +482,18 @@ class BldcMotor : public BaseComponent {
   }
 
   /**
-   * @brief Return the shaft angle, in radians.
-   * @return Motor shaft angle in radians.
+   * @brief Return the most recently sampled (or estimated in open-loop) shaft
+   *        angle, in radians.
+   * @return Cached motor shaft angle in radians.
    */
-  float get_shaft_angle() {
-    // if no sensor linked return previous value ( for open loop )
-    if (!sensor_)
-      return shaft_angle_;
-    auto sensed = angle_filter_ ? angle_filter_(sensor_->get_radians()) : sensor_->get_radians();
-    return (float)sensor_direction_ * sensed - sensor_offset_;
-  }
+  float get_shaft_angle() const { return shaft_angle_; }
 
   /**
-   * @brief Return the shaft velocity, in radians per second (rad/s).
-   * @return Motor shaft velocity (rad/s).
+   * @brief Return the most recently sampled (or estimated in open-loop) shaft
+   *        velocity, in radians per second (rad/s).
+   * @return Cached motor shaft velocity (rad/s).
    */
-  float get_shaft_velocity() {
-    // if no sensor linked return previous value ( for open loop )
-    if (!sensor_)
-      return shaft_velocity_;
-    auto sensed = velocity_filter_ ? velocity_filter_(sensor_->get_rpm()) : sensor_->get_rpm();
-    return (float)sensor_direction_ * sensed * RPM_TO_RADS;
-  }
+  float get_shaft_velocity() const { return shaft_velocity_; }
 
   /**
    * @brief Get the electrical angle of the motor - using the mechanical
@@ -612,13 +602,13 @@ class BldcMotor : public BaseComponent {
     //                        representation.
     if (motion_control_type_ != detail::MotionControlType::ANGLE_OPENLOOP &&
         motion_control_type_ != detail::MotionControlType::VELOCITY_OPENLOOP) {
-      shaft_angle_ = get_shaft_angle();
+      this->sample_shaft_angle();
     }
 
     // get angular velocity TODO the velocity reading probably also shouldn't
     // happen in open loop modes?
 
-    shaft_velocity_ = get_shaft_velocity();
+    this->sample_shaft_velocity();
 
     // set internal target variable
     target_ = new_target;
@@ -736,9 +726,16 @@ class BldcMotor : public BaseComponent {
     }
     // align sensor and motor
     bool success = align_sensor();
-    if (run_sensor_update_)
+    if (run_sensor_update_) {
       sensor_->update(ec);
-    shaft_angle_ = get_shaft_angle();
+      if (ec) {
+        logger_.error("Sensor update failed during init_foc: {}", ec.message());
+        status_ = Status::FAILED_CALIBRATION;
+        return;
+      }
+    }
+    this->sample_shaft_angle();
+    this->sample_shaft_velocity();
 
     if (current_sense_) {
       success &= align_current_sense();
@@ -748,6 +745,24 @@ class BldcMotor : public BaseComponent {
     logger_.debug("Init FOC completed: {}", success);
   }
 
+  float sample_shaft_angle() {
+    if (!sensor_) {
+      return shaft_angle_;
+    }
+    auto sensed = angle_filter_ ? angle_filter_(sensor_->get_radians()) : sensor_->get_radians();
+    shaft_angle_ = (float)sensor_direction_ * sensed - sensor_offset_;
+    return shaft_angle_;
+  }
+
+  float sample_shaft_velocity() {
+    if (!sensor_) {
+      return shaft_velocity_;
+    }
+    auto sensed = velocity_filter_ ? velocity_filter_(sensor_->get_rpm()) : sensor_->get_rpm();
+    shaft_velocity_ = (float)sensor_direction_ * sensed * RPM_TO_RADS;
+    return shaft_velocity_;
+  }
+
   bool align_sensor() {
     using namespace std::chrono_literals;
     int exit_flag = 1; // success
@@ -773,6 +788,7 @@ class BldcMotor : public BaseComponent {
           sensor_->update(ec);
         std::this_thread::sleep_for(1ms * 2);
       }
+
       // take an angle in the middle
       if (run_sensor_update_)
         sensor_->update(ec);